diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 2d5ca2149a..592fe59839 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -637,6 +637,9 @@ class Executor /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. std::atomic_bool spinning; + /// Flag specifying that the executor has been cancelled and should not spin + std::atomic_bool executor_canceled{false}; + /// Guard condition for signaling the rmw layer to wake up for special events. std::shared_ptr interrupt_guard_condition_; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 3deb2d1706..2bae0be701 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -500,6 +500,8 @@ void Executor::cancel() { spinning.store(false); + executor_canceled.store(true); + try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index e7f311c147..452f29d9cb 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -27,6 +27,10 @@ SingleThreadedExecutor::~SingleThreadedExecutor() {} void SingleThreadedExecutor::spin() { + if (executor_canceled.exchange(false)) { + return; + } + if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 3c14b37b45..1d5225b9e2 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -41,6 +41,10 @@ StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() void StaticSingleThreadedExecutor::spin() { + if (executor_canceled.exchange(false)) { + return; + } + if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } diff --git a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp index 64b07c0814..1822bbe06a 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -106,6 +106,10 @@ EventsExecutor::~EventsExecutor() void EventsExecutor::spin() { + if (executor_canceled.exchange(false)) { + return; + } + if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index fe509511e8..d85288b76d 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -373,6 +373,25 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) spinner.join(); } +TYPED_TEST(TestExecutors, testCancel) +{ + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + ExecutorType executor; + + auto executor_thread = std::thread([&](){ executor.spin();}); + executor.cancel(); + // This should not timeout + executor_thread.join(); +} + class TestWaitable : public rclcpp::Waitable { public: