(draft) add per callback group thread isolation

rclcpp/include/rclcpp/executors/events_cbg_executor/events_cbg_executor.hpp

@@ -15,8 +15,10 @@
 #pragma once
 
 #include <chrono>
+#include <functional>
 #include <memory>
 #include <mutex>
+#include <string>
 #include <thread>
 #include <vector>
 
@@ -36,6 +38,7 @@ class TimerManager;
 struct RegisteredEntityCache;
 class CBGScheduler;
 struct GlobalWeakExecutableCache;
+struct DedicatedThreadPool;
 }
 
 class EventsCBGExecutor : public rclcpp::Executor
@@ -120,6 +123,87 @@ class EventsCBGExecutor : public rclcpp::Executor
   // add a callback group to the executor, not bound to any node
   void add_callback_group_only(const rclcpp::CallbackGroup::SharedPtr & group_ptr);
 
+  /// Configuration of a dedicated worker thread.
+  /**
+   * All settings are optional. Defaults inherit the corresponding
+   * property from the process / parent thread.
+   */
+  struct DedicatedThreadOptions
+  {
+    enum class SchedulingPolicy
+    {
+      /// Keep the scheduling policy of the process (default)
+      Inherit,
+      /// SCHED_OTHER, the standard time sharing policy
+      Other,
+      /// SCHED_RR, realtime round robin policy
+      RoundRobin,
+      /// SCHED_FIFO, realtime first in first out policy
+      Fifo,
+    };
+
+    /// Name of the dedicated thread, as shown by debugging and tracing
+    /// tools. Empty keeps the default thread name. Note, on Linux thread
+    /// names are limited to 15 characters, longer names are truncated.
+    std::string name;
+
+    /// Scheduling policy of the dedicated thread.
+    SchedulingPolicy scheduling_policy = SchedulingPolicy::Inherit;
+
+    /// Scheduling priority of the dedicated thread. Only used with the
+    /// RoundRobin and Fifo policies. Note, setting a realtime policy
+    /// usually requires elevated privileges (e.g. CAP_SYS_NICE or an
+    /// appropriate RLIMIT_RTPRIO).
+    int priority = 0;
+
+    /// Indices of the cpu cores the dedicated thread may run on.
+    /// Empty keeps the affinity mask of the process.
+    std::vector<size_t> cpu_affinity;
+
+    /// Optional callback, executed once inside the dedicated thread after
+    /// the settings above were applied, and before any events are
+    /// processed. Escape hatch for settings not covered by this struct.
+    std::function<void()> thread_init_callback;
+  };
+
+  /// Assign a dedicated worker thread to the given callback group.
+  /**
+   * All events of the given callback group will be executed exclusively by
+   * a thread dedicated to this callback group, instead of the shared worker
+   * pool. This isolates the execution of the callback group from the load
+   * of the rest of the system, and allows the use of custom scheduling
+   * settings (priority, affinity) for the dedicated thread.
+   *
+   * If applying one of the requested thread settings fails (e.g. a
+   * realtime policy was requested without sufficient privileges), an
+   * error is logged and the thread continues with the inherited settings.
+   *
+   * Dedicated worker threads only process events while spin() or
+   * spin(exception_handler) is active. spin_once, spin_some and spin_all
+   * will NOT execute events of dedicated callback groups.
+   *
+   * Must be called before the callback group is added to this executor,
+   * either directly, or indirectly by adding its node.
+   *
+   * \param group_ptr the callback group that shall be executed by a
+   *        dedicated worker thread
+   * \param options thread settings (name, scheduling policy, priority,
+   *        cpu affinity) of the dedicated thread
+   * \throws std::runtime_error if the callback group was already added to
+   *         this executor
+   */
+  RCLCPP_PUBLIC
+  void
+  set_dedicated_thread_for_callback_group(
+    const rclcpp::CallbackGroup::SharedPtr & group_ptr,
+    DedicatedThreadOptions options);
+
+  /// \sa set_dedicated_thread_for_callback_group, with default options
+  RCLCPP_PUBLIC
+  void
+  set_dedicated_thread_for_callback_group(
+    const rclcpp::CallbackGroup::SharedPtr & group_ptr);
+
   /**
    * \sa rclcpp::Executor:spin() for more details
    * \throws std::runtime_error when spin() called while already spinning
@@ -294,6 +378,23 @@ class EventsCBGExecutor : public rclcpp::Executor
 
   void sync_callback_groups();
 
+  /**
+   * Spawns dedicated worker threads for all callback groups configured
+   * via set_dedicated_thread_for_callback_group, that do not have a
+   * running worker thread yet.
+   *
+   * No op, if no threaded spin is active.
+   */
+  void start_dedicated_worker_threads();
+
+  /**
+   * Releases all dedicated worker threads and joins them.
+   * If called from within a dedicated worker thread (e.g. a callback
+   * initiated the shutdown), the calling thread is detached instead
+   * of joined.
+   */
+  void stop_dedicated_worker_threads();
+
   /**
    * Either triggers a sync, or if not spinning,
    * syncs directly.
@@ -352,6 +453,28 @@ class EventsCBGExecutor : public rclcpp::Executor
 
   /// Stores the executables for guard conditions of the nodes
   std::unique_ptr<cbg_executor::GlobalWeakExecutableCache> nodes_executable_cache;
+
+  struct DedicatedThreadConfig
+  {
+    rclcpp::CallbackGroup::WeakPtr callback_group;
+    DedicatedThreadOptions options;
+  };
+
+  std::mutex dedicated_thread_configs_mutex_;
+
+  /// Callback groups that shall be executed by a dedicated worker thread
+  std::vector<DedicatedThreadConfig> dedicated_thread_configs_;
+
+  /// The running dedicated worker threads, keyed by their scheduler handle
+  std::unique_ptr<cbg_executor::DedicatedThreadPool> dedicated_threads_;
+
+  /// True while a threaded spin (spin() / spin(exception_handler)) is active.
+  /// Dedicated worker threads are only spawned while this is set.
+  std::atomic_bool dedicated_workers_active_ = false;
+
+  /// Exception handler passed to the dedicated worker threads,
+  /// set by spin(exception_handler)
+  std::function<void(const std::exception &)> dedicated_exception_handler_;
 };
 
 }  // namespace executors