feat(MultiThreadedExecutor): Added ability to handle exceptions from threads

This commit adds external exception handling for the worker threads,
allowing application code to implement custom exception handling.

Author: Janosch Machowinski

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -69,11 +69,29 @@ class MultiThreadedExecutor : 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(std::function<void(const std::exception &)> exception_handler);
+
   RCLCPP_PUBLIC
   size_t
   get_number_of_threads();
 
 protected:
+  RCLCPP_PUBLIC
+  void
+  run_guarded(
+    size_t this_thread_number,
+    std::function<void(const std::exception &)> exception_handler);
+
   RCLCPP_PUBLIC
   void
   run(size_t this_thread_number);

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -51,6 +51,12 @@ MultiThreadedExecutor::~MultiThreadedExecutor() {}
 
 void
 MultiThreadedExecutor::spin()
+{
+  spin(std::function<void(const std::exception &)>());
+}
+
+void
+MultiThreadedExecutor::spin(std::function<void(const std::exception &)> exception_handler)
 {
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
@@ -61,12 +67,13 @@ MultiThreadedExecutor::spin()
   {
     std::lock_guard wait_lock{wait_mutex_};
     for (; thread_id < number_of_threads_ - 1; ++thread_id) {
-      auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id);
+      auto func =
+        std::bind(&MultiThreadedExecutor::run_guarded, this, thread_id, exception_handler);
       threads.emplace_back(func);
     }
   }
 
-  run(thread_id);
+  run_guarded(thread_id, exception_handler);
   for (auto & thread : threads) {
     thread.join();
   }
@@ -79,7 +86,8 @@ MultiThreadedExecutor::get_number_of_threads()
 }
 
 void
-MultiThreadedExecutor::run(size_t this_thread_number)
+MultiThreadedExecutor::run(
+  size_t this_thread_number)
 {
   (void)this_thread_number;
   while (rclcpp::ok(this->context_) && spinning.load()) {
@@ -104,3 +112,23 @@ MultiThreadedExecutor::run(size_t this_thread_number)
     any_exec.callback_group.reset();
   }
 }
+
+void
+MultiThreadedExecutor::run_guarded(
+  size_t this_thread_number,
+  std::function<void(const std::exception &)> exception_handler)
+{
+  if (exception_handler) {
+    try {
+      run(this_thread_number);
+    } catch (const std::exception & e) {
+      RCLCPP_ERROR_STREAM(
+        rclcpp::get_logger("rclcpp"),
+        "Exception while spinning : " << e.what());
+
+      exception_handler(e);
+    }
+  } else {
+    run(this_thread_number);
+  }
+}