diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index d620334..7e69db6 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -180,6 +180,13 @@ public: return FutureReturnCode::INTERRUPTED; } + /// Stop everything + /** + */ + RCLCPP_PUBLIC + void + cancel(); + /// Support dynamic switching of the memory strategy. /** * Switching the memory strategy while the executor is spinning in another threading could have @@ -253,6 +260,9 @@ protected: AnyExecutable::SharedPtr get_next_executable(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + /// For cancelling execution mid-spin. + std::atomic_bool canceled; + /// Guard condition for signaling the rmw layer to wake up for special events. rmw_guard_condition_t * interrupt_guard_condition_; diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 1006152..dbcac72 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -46,7 +46,6 @@ RCLCPP_PUBLIC void shutdown(); - /// Get a handle to the rmw guard condition that manages the signal handler. RCLCPP_PUBLIC rmw_guard_condition_t * diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index bfe27a3..c49d925 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -21,6 +21,7 @@ using rclcpp::executor::Executor; Executor::Executor(rclcpp::memory_strategy::MemoryStrategy::SharedPtr ms) : interrupt_guard_condition_(rmw_create_guard_condition()), + canceled(false), memory_strategy_(ms) { } @@ -108,9 +109,9 @@ Executor::spin_node_some(rclcpp::node::Node::SharedPtr node) void Executor::spin_some() { - while (AnyExecutable::SharedPtr any_exec = - get_next_executable(std::chrono::milliseconds::zero())) - { + canceled = false; + AnyExecutable::SharedPtr any_exec; + while ((any_exec = get_next_executable(std::chrono::milliseconds::zero())) && !canceled) { execute_any_executable(any_exec); } } @@ -124,6 +125,16 @@ Executor::spin_once(std::chrono::nanoseconds timeout) } } +void +Executor::cancel() +{ + canceled = true; + rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); + if (status != RMW_RET_OK) { + throw std::runtime_error(rmw_get_error_string_safe()); + } +} + void Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) { @@ -503,9 +514,15 @@ Executor::get_next_executable(std::chrono::nanoseconds timeout) if (!any_exec) { // Wait for subscriptions or timers to work on wait_for_work(timeout); + if (canceled) { + return nullptr; + } // Try again any_exec = get_next_ready_executable(); } + if (canceled) { + return nullptr; + } // At this point any_exec should be valid with either a valid subscription // or a valid timer, or it should be a null shared_ptr if (any_exec) { diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 5684213..50523f1 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -36,6 +36,7 @@ MultiThreadedExecutor::~MultiThreadedExecutor() {} void MultiThreadedExecutor::spin() { + canceled = false; std::vector threads; { std::lock_guard wait_lock(wait_mutex_); @@ -61,7 +62,7 @@ void MultiThreadedExecutor::run(size_t this_thread_number) { thread_number_by_thread_id_[std::this_thread::get_id()] = this_thread_number; - while (rclcpp::utilities::ok()) { + while (rclcpp::utilities::ok() && !canceled) { executor::AnyExecutable::SharedPtr any_exec; { std::lock_guard wait_lock(wait_mutex_); diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index 39f04bf..81373b2 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -26,7 +26,8 @@ SingleThreadedExecutor::~SingleThreadedExecutor() {} void SingleThreadedExecutor::spin() { - while (rclcpp::utilities::ok()) { + canceled = false; + while (rclcpp::utilities::ok() && !canceled) { auto any_exec = get_next_executable(); execute_any_executable(any_exec); }