feat(Executor): Added spin API with exception handler.

Janosch Machowinski · Janosch Machowinski · commit d557d7b7f49a · 2024-03-11T13:27:48.000+01:00
Signed-off-by: Janosch Machowinski &lt;j.machowinski@nospam.org&gt;
diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
@@ -84,6 +84,18 @@ class Executor
   virtual void
   spin() = 0;
 
+  /**
+   * \sa rclcpp::Executor:spin() for more details
+   * \throws std::runtime_error when spin() called while already spinning
+   * @param exception_handler will be called for every exception in the processing threads
+   *
+   * The exception_handler can be called from multiple threads at the same time.
+   * The exception_handler shall rethrow the exception it if wants to terminate the program.
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  spin(const std::function<void(const std::exception & e)> & exception_handler) = 0;
+
   /// Add a callback group to an executor.
   /**
    * An executor can have zero or more callback groups which provide work during `spin` functions.
@@ -476,6 +488,18 @@ class Executor
   void
   execute_any_executable(AnyExecutable & any_exec);
 
+  /// Find the next available executable and do the work associated with it.
+  /**
+   * \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
+   * service, client).
+   * \throws std::runtime_error if there is an issue triggering the guard condition
+   */
+  RCLCPP_PUBLIC
+  void
+  execute_any_executable(
+    AnyExecutable & any_exec,
+    const std::function<void(const std::exception & e)> & exception_handler);
+
   /// Run subscription executable.
   /**
    * Do necessary setup and tear-down as well as executing the subscription.
diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
@@ -79,7 +79,7 @@ class MultiThreadedExecutor : public rclcpp::Executor
    */
   RCLCPP_PUBLIC
   void
-  spin(std::function<void(const std::exception & e)> exception_handler);
+  spin(const std::function<void(const std::exception & e)> & exception_handler) override;
 
   RCLCPP_PUBLIC
   size_t
@@ -88,7 +88,9 @@ class MultiThreadedExecutor : public rclcpp::Executor
 protected:
   RCLCPP_PUBLIC
   void
-  run(size_t this_thread_number, std::function<void(const std::exception & e)> exception_handler);
+  run(
+    size_t this_thread_number,
+    const std::function<void(const std::exception & e)> & exception_handler);
 
 private:
   RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
@@ -65,6 +65,16 @@ class SingleThreadedExecutor : public rclcpp::Executor
   void
   spin() override;
 
+  /**
+   * \sa rclcpp::SingleThreadedExecutor:spin() for more details
+   * \throws std::runtime_error when spin() called while already spinning
+   * @param exception_handler will be called for every exception in the processing threads
+   *
+   * The exception_handler shall rethrow the exception it if wants to terminate the program.
+   */RCLCPP_PUBLIC
+  virtual void
+  spin(const std::function<void(const std::exception & e)> & exception_handler) override;
+
 private:
   RCLCPP_DISABLE_COPY(SingleThreadedExecutor)
 };
diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp
@@ -79,6 +79,16 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor
   void
   spin() override;
 
+  /**
+   * \sa rclcpp::SingleThreadedExecutor:spin() for more details
+   * \throws std::runtime_error when spin() called while already spinning
+   * @param exception_handler will be called for every exception in the processing threads
+   *
+   * The exception_handler shall rethrow the exception it if wants to terminate the program.
+   */RCLCPP_PUBLIC
+  virtual void
+  spin(const std::function<void(const std::exception & e)> & exception_handler) override;
+
   /// Static executor implementation of spin some
   /**
    * This non-blocking function will execute entities that
diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp
@@ -92,6 +92,18 @@ class EventsExecutor : public rclcpp::Executor
   void
   spin() override;
 
+  /**
+   * \sa rclcpp::Executor:spin() for more details
+   * \throws std::runtime_error when spin() called while already spinning
+   * @param exception_handler will be called for every exception in the processing threads
+   *
+   * The exception_handler can be called from multiple threads at the same time.
+   * The exception_handler shall rethrow the exception it if wants to terminate the program.
+   */
+  RCLCPP_PUBLIC
+  void
+  spin(const std::function<void(const std::exception & e)> & exception_handler) override;
+
   /// Events executor implementation of spin some
   /**
    * This non-blocking function will execute the timers and events
diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp
@@ -124,9 +124,13 @@ class TimersManager
   /**
    * @brief Starts a thread that takes care of executing the timers stored in this object.
    * Function will throw an error if the timers thread was already running.
+   *
+   * @param exception_handler if valid, the execution of the timer will be done in a try catch block,
+   *                          and any occurring exception will be passed to the given handler
    */
   RCLCPP_PUBLIC
-  void start();
+  void start(
+    const std::function<void(const std::exception & e)> & exception_handler = std::function<void(const std::exception & e)>());
 
   /**
    * @brief Stops the timers thread.
@@ -509,6 +513,11 @@ class TimersManager
    */
   void run_timers();
 
+  /**
+   * @brief calls run_timers with a try catch block.
+   */
+  void run_timers(const std::function<void(const std::exception & e)> & exception_handler);
+
   /**
    * @brief Get the amount of time before the next timer triggers.
    * This function is not thread safe, acquire a mutex before calling it.
@@ -526,7 +535,7 @@ class TimersManager
    * while keeping the heap correctly sorted.
    * This function is not thread safe, acquire the timers_mutex_ before calling it.
    */
-  void execute_ready_timers_unsafe();
+  void execute_ready_timers_unsafe(std::function<void(const std::exception & e)> exception_handler);
 
   // Callback to be called when timer is ready
   std::function<void(const rclcpp::TimerBase *)> on_ready_callback_;
diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
@@ -575,6 +575,72 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   }
 }
 
+template<typename Function>
+void execute_guarded(
+  const Function & fun,
+  const std::function<void(const std::exception & e)> & exception_handler)
+{
+  try {
+    fun();
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("rclcpp"),
+      "Exception while spinning : " << e.what());
+
+    exception_handler(e);
+  }
+}
+
+void
+Executor::execute_any_executable(
+  AnyExecutable & any_exec,
+  const std::function<void(const std::exception & e)> & exception_handler)
+{
+  if (!spinning.load()) {
+    return;
+  }
+
+  if (any_exec.timer) {
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_executor_execute,
+      static_cast<const void *>(any_exec.timer->get_timer_handle().get()));
+    execute_guarded([&any_exec]() {execute_timer(any_exec.timer);}, exception_handler);
+  }
+  if (any_exec.subscription) {
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_executor_execute,
+      static_cast<const void *>(any_exec.subscription->get_subscription_handle().get()));
+    execute_guarded(
+      [&any_exec]() {
+        execute_subscription(any_exec.subscription);
+      }, exception_handler);
+
+  }
+  if (any_exec.service) {
+    execute_guarded([&any_exec]() {execute_service(any_exec.service);}, exception_handler);
+
+  }
+  if (any_exec.client) {
+    execute_guarded([&any_exec]() {execute_client(any_exec.client);}, exception_handler);
+
+  }
+  if (any_exec.waitable) {
+    execute_guarded([&any_exec]() {any_exec.waitable->execute(any_exec.data);}, exception_handler);
+  }
+  // Reset the callback_group, regardless of type
+  any_exec.callback_group->can_be_taken_from().store(true);
+  // Wake the wait, because it may need to be recalculated or work that
+  // was previously blocked is now available.
+  try {
+    interrupt_guard_condition_->trigger();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to trigger guard condition from execute_any_executable: ") + ex.what());
+  }
+}
+
+
 template<typename Taker, typename Handler>
 static
 void
diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp
@@ -57,7 +57,7 @@ MultiThreadedExecutor::spin()
 
 void
 
-MultiThreadedExecutor::spin(std::function<void(const std::exception & e)> exception_handler)
+MultiThreadedExecutor::spin(const std::function<void(const std::exception & e)> & exception_handler)
 {
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
@@ -88,38 +88,28 @@ MultiThreadedExecutor::get_number_of_threads()
 void
 MultiThreadedExecutor::run(
   size_t this_thread_number,
-  std::function<void(const std::exception & e)> exception_handler)
+  const std::function<void(const std::exception & e)> & exception_handler)
 {
-  try {
-
-    (void)this_thread_number;
-    while (rclcpp::ok(this->context_) && spinning.load()) {
-      rclcpp::AnyExecutable any_exec;
-      {
-        std::lock_guard wait_lock{wait_mutex_};
-        if (!rclcpp::ok(this->context_) || !spinning.load()) {
-          return;
-        }
-        if (!get_next_executable(any_exec, next_exec_timeout_)) {
-          continue;
-        }
+  (void)this_thread_number;
+  while (rclcpp::ok(this->context_) && spinning.load()) {
+    rclcpp::AnyExecutable any_exec;
+    {
+      std::lock_guard wait_lock{wait_mutex_};
+      if (!rclcpp::ok(this->context_) || !spinning.load()) {
+        return;
       }
-      if (yield_before_execute_) {
-        std::this_thread::yield();
+      if (!get_next_executable(any_exec, next_exec_timeout_)) {
+        continue;
       }
-
-      execute_any_executable(any_exec);
-
-      // Clear the callback_group to prevent the AnyExecutable destructor from
-      // resetting the callback group `can_be_taken_from`
-      any_exec.callback_group.reset();
+    }
+    if (yield_before_execute_) {
+      std::this_thread::yield();
     }
 
-  } catch (const std::exception & e) {
-    RCLCPP_ERROR_STREAM(
-      rclcpp::get_logger("rclcpp"),
-      "Exception while spinning : " << e.what());
+    execute_any_executable(any_exec, exception_handler);
 
-    exception_handler(e);
+    // Clear the callback_group to prevent the AnyExecutable destructor from
+    // resetting the callback group `can_be_taken_from`
+    any_exec.callback_group.reset();
   }
 }
diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp
@@ -38,3 +38,20 @@ SingleThreadedExecutor::spin()
     }
   }
 }
+
+
+void
+SingleThreadedExecutor::spin(
+  const std::function<void(const std::exception & e)> & exception_handler)
+{
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin() called while already spinning");
+  }
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  while (rclcpp::ok(this->context_) && spinning.load()) {
+    rclcpp::AnyExecutable any_executable;
+    if (get_next_executable(any_executable)) {
+      execute_any_executable(any_executable, exception_handler);
+    }
+  }
+}
diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp
@@ -57,6 +57,41 @@ StaticSingleThreadedExecutor::spin()
   }
 }
 
+
+void
+StaticSingleThreadedExecutor::spin(
+  const std::function<void(const std::exception & e)> & exception_handler)
+{
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin() called while already spinning");
+  }
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+
+  // Set memory_strategy_ and exec_list_ based on weak_nodes_
+  // Prepare wait_set_ based on memory_strategy_
+  entities_collector_->init(&wait_set_, memory_strategy_);
+
+  while (rclcpp::ok(this->context_) && spinning.load()) {
+    // Refresh wait set and wait for work
+    entities_collector_->refresh_wait_set();
+
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("rclcpp"),
+      "Waitset Refresh");
+
+    try {
+      execute_ready_executables();
+
+    } catch (const std::exception & e) {
+      RCLCPP_ERROR_STREAM(
+        rclcpp::get_logger("rclcpp"),
+        "Exception while spinning : " << e.what());
+
+      exception_handler(e);
+    }
+  }
+}
+
 void
 StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration)
 {
diff --git a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp
@@ -124,6 +124,35 @@ EventsExecutor::spin()
   }
 }
 
+void
+EventsExecutor::spin(const std::function<void(const std::exception & e)> & exception_handler)
+{
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin() called while already spinning");
+  }
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+
+  timers_manager_->start(exception_handler);
+  RCPPUTILS_SCOPE_EXIT(timers_manager_->stop(); );
+
+  while (rclcpp::ok(context_) && spinning.load()) {
+    // Wait until we get an event
+    ExecutorEvent event;
+    bool has_event = events_queue_->dequeue(event);
+    if (has_event) {
+      try {
+        this->execute_event(event);
+      } catch (const std::exception & e) {
+        RCLCPP_ERROR_STREAM(
+          rclcpp::get_logger("rclcpp"),
+          "Exception while spinning : " << e.what());
+
+        exception_handler(e);
+      }
+    }
+  }
+}
+
 void
 EventsExecutor::spin_some(std::chrono::nanoseconds max_duration)
 {
diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
diff --git a/rclcpp/test/benchmark/benchmark_parameter_client.cpp b/rclcpp/test/benchmark/benchmark_parameter_client.cpp
diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp
diff --git a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp
diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp
