API update for CPU and rt-priority assignment

README.md

@@ -40,28 +40,23 @@
   ```
   exec1.enable_callback_priority();
   ```
-- Set executor's rt priority (`SCHED_FIFO` in linux) and CPU assignment
+- Set executor's rt priority and CPU assignment
   - For single-threaded executors:
   ```
-  exec1.set_executor_priority_cpu(90, 5); // (90: rt priority, 5: cpu #)
+  exec1.set_executor_priority_cpu(SCHED_FIFO, 90, 5); // (SCHED_FIFO: scheduling policy, 90: rt priority, 5: cpu #)
   ```  
   - For multi-threaded executors:
   ```
-  exec1.cpus = {1, 2, 3}; // CPU1, 2, 3
-  exec1.rt_attr.sched_priority = SCHED_FIFO;
-  exec1.rt_attr.sched_priority = 80;
+  std::vector<int> cpus = {1, 2, 3}; // CPU 1, 2, 3
+  exec1.set_executor_priority_cpu(SCHED_FIFO, 90, cpus); // (SCHED_FIFO: scheduling policy, 90: rt priority, cpus: cpu #)
   ```
   Other policies such as SCHED_RR and SCHED_DEADLINE are also usable.
 - Set priority of callback
   ```
   exec1.set_callback_priority(timer_callback->timer_, 10); // 10: callback's priority (the higher, more critical callback)
   ```
 - Finally, spin
-  - Single-threaded executor: use `spin_rt` for real-time priority in linux
-  ```
-  std::thread spinThread1(&rclcpp::executors::SingleThreadedExecutor::spin_rt, &exec1);
-  ```
-  - Multi-threaded executor: `spin` assigns each thread's rt priority and cpu affinity based on prespecified params.
+  - `spin` assigns each executor thread's rt priority and cpu affinity based on prespecified params.
   ```
   exec1.spin();
   ```

rclcpp/include/rclcpp/executor.hpp

@@ -42,6 +42,36 @@
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/scope_exit.hpp"
 
+#ifdef PICAS
+#include <rclcpp/cb_sched.hpp>
+#include <unistd.h>
+#include <sys/types.h>
+#include <errno.h>
+#include <sys/syscall.h>
+#include <linux/sched.h>
+#include <pthread.h>  // for sched_deadline
+
+#define gettid() syscall(__NR_gettid)
+
+struct sched_attr {
+    int32_t size;
+
+    int32_t sched_policy;
+    int64_t sched_flags;
+
+    /* SCHED_NORMAL, SCHED_BATCH */
+    int32_t sched_nice;
+
+    /* SCHED_FIFO, SCHED_RR */
+    int32_t sched_priority;
+
+    /* SCHED_DEADLINE (nsec) */
+    int64_t sched_runtime;
+    int64_t sched_deadline;
+    int64_t sched_period;
+};
+#endif
+
 namespace rclcpp
 {
 
@@ -403,15 +433,23 @@ class Executor
 
 #ifdef PICAS
   bool callback_priority_enabled = false;
-  int executor_priority = 0;
-  int executor_cpu = 0;
+  struct sched_attr rt_attr;
+  std::vector<int> cpus;
 
   RCLCPP_PUBLIC
   void 
-  set_executor_priority_cpu(int priority, int cpu) // deprecated: for single-threaded executors only
-  {
-    executor_priority = priority;
-    executor_cpu = cpu;    
+  set_executor_priority_cpu(int policy, int priority, std::vector<int> assigned_cpus){
+    rt_attr.sched_policy = policy;
+    rt_attr.sched_priority = priority;
+    cpus = assigned_cpus;
+  }
+
+  RCLCPP_PUBLIC
+  void 
+  set_executor_priority_cpu(int policy, int priority, int cpu){
+    rt_attr.sched_policy = policy;
+    rt_attr.sched_priority = priority;
+    cpus.push_back(cpu);
   }
 
   RCLCPP_PUBLIC
@@ -483,6 +521,10 @@ class Executor
   RCLCPP_PUBLIC
   void
   print_list_ready_executable(AnyExecutable & any_executable);
+
+  RCLCPP_PUBLIC
+  long int
+  sched_setattr(pid_t pid, const struct sched_attr *attr, unsigned int flags);
 #endif
 
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -28,104 +28,69 @@
 #include "rclcpp/memory_strategies.hpp"
 #include "rclcpp/visibility_control.hpp"
 
-#ifdef PICAS
-#include <rclcpp/cb_sched.hpp>
-#include <unistd.h>
-#include <sys/types.h>
-#include <errno.h>
-#include <sys/syscall.h>
-#include <linux/sched.h>
-
-// for sched_deadline
-#include <pthread.h>
-#define gettid() syscall(__NR_gettid)
-struct sched_attr {
-    int32_t size;
-
-    int32_t sched_policy;
-    int64_t sched_flags;
-
-    /* SCHED_NORMAL, SCHED_BATCH */
-    int32_t sched_nice;
-
-    /* SCHED_FIFO, SCHED_RR */
-    int32_t sched_priority;
-
-    /* SCHED_DEADLINE (nsec) */
-    int64_t sched_runtime;
-    int64_t sched_deadline;
-    int64_t sched_period;
-};
-#endif
-
 namespace rclcpp
 {
-namespace executors
-{
-
-class MultiThreadedExecutor : public rclcpp::Executor
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor)
-
-  /// Constructor for MultiThreadedExecutor.
-  /**
-   * For the yield_before_execute option, when true std::this_thread::yield()
-   * will be called after acquiring work (as an AnyExecutable) and
-   * releasing the spinning lock, but before executing the work.
-   * This is useful for reproducing some bugs related to taking work more than
-   * once.
-   *
-   * \param options common options for all executors
-   * \param number_of_threads number of threads to have in the thread pool,
-   *   the default 0 will use the number of cpu cores found instead
-   * \param yield_before_execute if true std::this_thread::yield() is called
-   * \param timeout maximum time to wait
-   */
-  RCLCPP_PUBLIC
-  MultiThreadedExecutor(
-    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(),
-    size_t number_of_threads = 0,
-    bool yield_before_execute = false,
-    std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
-
-  RCLCPP_PUBLIC
-  virtual ~MultiThreadedExecutor();
-
-  /**
-   * \sa rclcpp::Executor:spin() for more details
-   * \throws std::runtime_error when spin() called while already spinning
-   */
-  RCLCPP_PUBLIC
-  void
-  spin() override;
-
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_threads();
-
-#ifdef PICAS
-  std::vector<int> cpus;
-  struct sched_attr rt_attr;
-#endif
-
-protected:
-  RCLCPP_PUBLIC
-  void
-  run(size_t this_thread_number);
-
-private:
-  RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
-
-  detail::MutexTwoPriorities wait_mutex_;
-  size_t number_of_threads_;
-  bool yield_before_execute_;
-  std::chrono::nanoseconds next_exec_timeout_;
-
-  std::set<TimerBase::SharedPtr> scheduled_timers_;
-};
-
-}  // namespace executors
-}  // namespace rclcpp
-
-#endif  // RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
+  namespace executors
+  {
+
+    class MultiThreadedExecutor : public rclcpp::Executor
+    {
+    public:
+      RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor)
+
+      /// Constructor for MultiThreadedExecutor.
+      /**
+       * For the yield_before_execute option, when true std::this_thread::yield()
+       * will be called after acquiring work (as an AnyExecutable) and
+       * releasing the spinning lock, but before executing the work.
+       * This is useful for reproducing some bugs related to taking work more than
+       * once.
+       *
+       * \param options common options for all executors
+       * \param number_of_threads number of threads to have in the thread pool,
+       *   the default 0 will use the number of cpu cores found instead
+       * \param yield_before_execute if true std::this_thread::yield() is called
+       * \param timeout maximum time to wait
+       */
+      RCLCPP_PUBLIC
+      MultiThreadedExecutor(
+          const rclcpp::ExecutorOptions &options = rclcpp::ExecutorOptions(),
+          size_t number_of_threads = 0,
+          bool yield_before_execute = false,
+          std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+      RCLCPP_PUBLIC
+      virtual ~MultiThreadedExecutor();
+
+      /**
+       * \sa rclcpp::Executor:spin() for more details
+       * \throws std::runtime_error when spin() called while already spinning
+       */
+      RCLCPP_PUBLIC
+      void
+      spin() override;
+
+      RCLCPP_PUBLIC
+      size_t
+      get_number_of_threads();
+
+    protected:
+      RCLCPP_PUBLIC
+      void
+      run(size_t this_thread_number);
+
+    private:
+      RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
+
+      detail::MutexTwoPriorities wait_mutex_;
+      size_t number_of_threads_;
+      bool yield_before_execute_;
+      std::chrono::nanoseconds next_exec_timeout_;
+
+      std::set<TimerBase::SharedPtr> scheduled_timers_;
+    };
+
+  } // namespace executors
+} // namespace rclcpp
+
+#endif // RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_