diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index b4f1b9d8f9..5bcedc37cd 100644 --- 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(std::function exception_handler); + /// Add a callback group to an executor. /** * An executor can have zero or more callback groups which provide work during `spin` functions. diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index b63ea2c41b..f620e7ec55 100644 --- 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 exception_handler); + spin(std::function exception_handler) override; RCLCPP_PUBLIC size_t diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp index 9dc6dec57b..cccddf8c8e 100644 --- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp +++ b/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) }; diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 5294605eaf..56f3ce9566 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/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 diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp index dd5b1ebe63..d17caea97c 100644 --- 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,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 diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 416a82be7c..e8d07beecf 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -402,6 +402,20 @@ Executor::remove_node(std::shared_ptr node_ptr, bool notify) this->remove_node(node_ptr->get_node_base_interface(), notify); } +void +Executor::spin(std::function 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, diff --git a/rclcpp/test/benchmark/benchmark_parameter_client.cpp b/rclcpp/test/benchmark/benchmark_parameter_client.cpp index 0d9a6fd9c9..b59c029dd0 100644 --- a/rclcpp/test/benchmark/benchmark_parameter_client.cpp +++ b/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(&rclcpp:: + executors::SingleThreadedExecutor::spin), remote_executor); } void TearDown(benchmark::State &) diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index 706b80aef1..3cdf5f2b90 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/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));