From 5bd71c1f8096d39a9b2c3b3f8bcd54065f51f25a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 12 Nov 2015 17:20:05 -0800 Subject: [PATCH] refactor executor::cancel to use a spinning state --- rclcpp/include/rclcpp/any_executable.hpp | 3 +++ rclcpp/include/rclcpp/executor.hpp | 9 +++---- rclcpp/src/rclcpp/any_executable.cpp | 10 ++++++++ rclcpp/src/rclcpp/executor.cpp | 25 +++++++++++-------- .../executors/multi_threaded_executor.cpp | 9 ++++--- .../executors/single_threaded_executor.cpp | 7 ++++-- 6 files changed, 43 insertions(+), 20 deletions(-) diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 6f886fd..656b2c7 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -33,6 +33,9 @@ struct AnyExecutable RCLCPP_PUBLIC AnyExecutable(); + RCLCPP_PUBLIC + virtual ~AnyExecutable(); + // Only one of the following pointers will be set. rclcpp::subscription::SubscriptionBase::SharedPtr subscription; rclcpp::subscription::SubscriptionBase::SharedPtr subscription_intra_process; diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 7e69db6..d72b549 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -180,9 +180,8 @@ public: return FutureReturnCode::INTERRUPTED; } - /// Stop everything - /** - */ + /// Cancels any running spin* function, causing it to return. + /* This function can be called asynchonously from any thread. */ RCLCPP_PUBLIC void cancel(); @@ -260,8 +259,8 @@ protected: AnyExecutable::SharedPtr get_next_executable(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - /// For cancelling execution mid-spin. - std::atomic_bool canceled; + /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. + std::atomic_bool spinning; /// Guard condition for signaling the rmw layer to wake up for special events. rmw_guard_condition_t * interrupt_guard_condition_; diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 0566140..0cef731 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -25,3 +25,13 @@ AnyExecutable::AnyExecutable() callback_group(nullptr), node(nullptr) {} + +AnyExecutable::~AnyExecutable() +{ + // Make sure that discarded (taken but not executed) AnyExecutable's have + // their callback groups reset. This can happen when an executor is canceled + // between taking an AnyExecutable and executing it. + if (callback_group) { + callback_group->can_be_taken_from().store(true); + } +} diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index c49d925..b4f3793 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -20,8 +20,7 @@ using rclcpp::executor::AnyExecutable; using rclcpp::executor::Executor; Executor::Executor(rclcpp::memory_strategy::MemoryStrategy::SharedPtr ms) -: interrupt_guard_condition_(rmw_create_guard_condition()), - canceled(false), +: spinning(false), interrupt_guard_condition_(rmw_create_guard_condition()), memory_strategy_(ms) { } @@ -109,26 +108,33 @@ Executor::spin_node_some(rclcpp::node::Node::SharedPtr node) void Executor::spin_some() { - canceled = false; + if (spinning.exchange(true)) { + throw std::runtime_error("spin_some() called while already spinning"); + } AnyExecutable::SharedPtr any_exec; - while ((any_exec = get_next_executable(std::chrono::milliseconds::zero())) && !canceled) { + while ((any_exec = get_next_executable(std::chrono::milliseconds::zero())) && spinning.load()) { execute_any_executable(any_exec); } + spinning.store(false); } void Executor::spin_once(std::chrono::nanoseconds timeout) { + if (spinning.exchange(true)) { + throw std::runtime_error("spin_once() called while already spinning"); + } auto any_exec = get_next_executable(timeout); if (any_exec) { execute_any_executable(any_exec); } + spinning.store(false); } void Executor::cancel() { - canceled = true; + spinning.store(false); 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()); @@ -147,7 +153,7 @@ Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr void Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec) { - if (!any_exec) { + if (!any_exec || !spinning.load()) { return; } if (any_exec->timer) { @@ -514,15 +520,12 @@ 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) { + if (!spinning.load()) { 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) { @@ -534,6 +537,8 @@ Executor::get_next_executable(std::chrono::nanoseconds timeout) // It should not have been taken otherwise assert(any_exec->callback_group->can_be_taken_from().load()); // Set to false to indicate something is being run from this group + // This is reset to true either when the any_exec is executed or when the + // any_exec is destructued any_exec->callback_group->can_be_taken_from().store(false); } } diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 50523f1..c59987c 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -36,7 +36,9 @@ MultiThreadedExecutor::~MultiThreadedExecutor() {} void MultiThreadedExecutor::spin() { - canceled = false; + if (spinning.exchange(true)) { + throw std::runtime_error("spin() called while already spinning"); + } std::vector threads; { std::lock_guard wait_lock(wait_mutex_); @@ -50,6 +52,7 @@ MultiThreadedExecutor::spin() for (auto & thread : threads) { thread.join(); } + spinning.store(false); } size_t @@ -62,11 +65,11 @@ 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() && !canceled) { + while (rclcpp::utilities::ok() && spinning.load()) { executor::AnyExecutable::SharedPtr any_exec; { std::lock_guard wait_lock(wait_mutex_); - if (!rclcpp::utilities::ok()) { + if (!rclcpp::utilities::ok() || !spinning.load()) { return; } any_exec = get_next_executable(); diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index 81373b2..962221b 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -26,9 +26,12 @@ SingleThreadedExecutor::~SingleThreadedExecutor() {} void SingleThreadedExecutor::spin() { - canceled = false; - while (rclcpp::utilities::ok() && !canceled) { + if (spinning.exchange(true)) { + throw std::runtime_error("spin_some() called while already spinning"); + } + while (rclcpp::utilities::ok() && spinning.load()) { auto any_exec = get_next_executable(); execute_any_executable(any_exec); } + spinning.store(false); }