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

Signed-off-by: Janosch Machowinski <[email protected]>

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(std::function<void(const std::exception & e)> exception_handler);
+
   /// Add a callback group to an executor.
   /**
    * An executor can have zero or more callback groups which provide work during `spin` functions.

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(std::function<void(const std::exception & e)> exception_handler) override;
 
   RCLCPP_PUBLIC
   size_t

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -65,6 +65,8 @@ class SingleThreadedExecutor : public rclcpp::Executor
   void
   spin() override;
 
+  using Executor::spin;
+
 private:
   RCLCPP_DISABLE_COPY(SingleThreadedExecutor)
 };

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -79,6 +79,8 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor
   void
   spin() override;
 
+  using Executor::spin;
+
   /// Static executor implementation of spin some
   /**
    * This non-blocking function will execute entities that

rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp

@@ -92,6 +92,8 @@ class EventsExecutor : public rclcpp::Executor
   void
   spin() override;
 
+  using Executor::spin;
+
   /// Events executor implementation of spin some
   /**
    * This non-blocking function will execute the timers and events

rclcpp/src/rclcpp/executor.cpp

@@ -402,6 +402,20 @@ Executor::remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
   this->remove_node(node_ptr->get_node_base_interface(), notify);
 }
 
+void
+Executor::spin(std::function<void(const std::exception & e)> exception_handler)
+{
+  try {
+    spin();
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("rclcpp"),
+      "Exception while spinning : " << e.what());
+
+    exception_handler(e);
+  }
+}
+
 void
 Executor::spin_node_once_nanoseconds(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,

rclcpp/test/benchmark/benchmark_parameter_client.cpp

@@ -46,7 +46,10 @@ class RemoteNodeTest : public benchmark::Fixture
       remote_node_name, rclcpp::NodeOptions().context(remote_context));
     remote_executor->add_node(remote_node);
 
-    remote_thread = std::thread(&rclcpp::executors::SingleThreadedExecutor::spin, remote_executor);
+    remote_thread =
+      std::thread(
+      static_cast<void (rclcpp::executors::SingleThreadedExecutor::*)()>(&rclcpp::
+      executors::SingleThreadedExecutor::spin), remote_executor);
   }
 
   void TearDown(benchmark::State &)

rclcpp/test/rclcpp/test_executor.cpp

@@ -42,6 +42,8 @@ class DummyExecutor : public rclcpp::Executor
   {
   }
 
+  using Executor::spin;
+
   void spin_nanoseconds(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
   {
     spin_node_once_nanoseconds(node, std::chrono::milliseconds(100));