From 0a478e52333238bb4bbcbeaab7216f4970175e69 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 12 Nov 2015 16:29:17 -0800 Subject: [PATCH 1/7] Implement cancel --- rclcpp/include/rclcpp/executor.hpp | 10 ++++++++ rclcpp/include/rclcpp/utilities.hpp | 1 - rclcpp/src/rclcpp/executor.cpp | 23 ++++++++++++++++--- .../executors/multi_threaded_executor.cpp | 3 ++- .../executors/single_threaded_executor.cpp | 3 ++- 5 files changed, 34 insertions(+), 6 deletions(-) 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); } From 5bd71c1f8096d39a9b2c3b3f8bcd54065f51f25a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 12 Nov 2015 17:20:05 -0800 Subject: [PATCH 2/7] 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); } From c469a8a2e99cd035ae07bbf80010a5c3cda46378 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 13 Nov 2015 10:17:45 -0800 Subject: [PATCH 3/7] Cancel instead of cancels --- rclcpp/include/rclcpp/executor.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index d72b549..1e6bd95 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -180,7 +180,7 @@ public: return FutureReturnCode::INTERRUPTED; } - /// Cancels any running spin* function, causing it to return. + /// Cancel any running spin* function, causing it to return. /* This function can be called asynchonously from any thread. */ RCLCPP_PUBLIC void From 9dce2808ea227cc47f830f6526b896987a155002 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 16 Nov 2015 17:13:32 -0800 Subject: [PATCH 4/7] add implementation of scope exit, aka scope guard --- rclcpp/include/rclcpp/macros.hpp | 3 ++ rclcpp/src/rclcpp/scope_exit.hpp | 52 ++++++++++++++++++++++++++++++++ 2 files changed, 55 insertions(+) create mode 100644 rclcpp/src/rclcpp/scope_exit.hpp diff --git a/rclcpp/include/rclcpp/macros.hpp b/rclcpp/include/rclcpp/macros.hpp index b22067d..b6cc888 100644 --- a/rclcpp/include/rclcpp/macros.hpp +++ b/rclcpp/include/rclcpp/macros.hpp @@ -82,6 +82,9 @@ __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ __RCLCPP_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) +#define RCLCPP_STRING_JOIN(arg1, arg2) RCLCPP_DO_STRING_JOIN(arg1, arg2) +#define RCLCPP_DO_STRING_JOIN(arg1, arg2) arg1 ## arg2 + #define RCLCPP_INFO(Args) std::cout << Args << std::endl; #endif // RCLCPP__MACROS_HPP_ diff --git a/rclcpp/src/rclcpp/scope_exit.hpp b/rclcpp/src/rclcpp/scope_exit.hpp new file mode 100644 index 0000000..334b790 --- /dev/null +++ b/rclcpp/src/rclcpp/scope_exit.hpp @@ -0,0 +1,52 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Based on: http://the-witness.net/news/2012/11/scopeexit-in-c11/ +// But I changed the lambda to include by reference rather than value, see: +// http://the-witness.net/news/2012/11/scopeexit-in-c11/comment-page-1/#comment-86873 + +#ifndef RCLCPP__SCOPE_EXIT_HPP_ +#define RCLCPP__SCOPE_EXIT_HPP_ + +#include + +#include "rclcpp/macros.hpp" + +namespace rclcpp +{ + +template +struct ScopeExit +{ + explicit ScopeExit(Callable callable) + : callable_(callable) {} + ~ScopeExit() {callable_(); } + +private: + Callable callable_; +}; + +template +ScopeExit +make_scope_exit(Callable callable) +{ + return ScopeExit(callable); +} + +} // namespace rclcpp + +#define RCLCPP_SCOPE_EXIT(code) \ + auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code; }) + +#endif // RCLCPP__SCOPE_EXIT_HPP_ From 2b342357d9b0d69b2bc00a40780ca7c8d8ee8198 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 16 Nov 2015 17:13:46 -0800 Subject: [PATCH 5/7] use scope exit to ensure spinning is reset --- rclcpp/src/rclcpp/executor.cpp | 4 ++++ rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp | 3 +++ rclcpp/src/rclcpp/executors/single_threaded_executor.cpp | 3 +++ 3 files changed, 10 insertions(+) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index b4f3793..48ac9c9 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -16,6 +16,8 @@ #include "rcl_interfaces/msg/intra_process_message.hpp" +#include "./scope_exit.hpp" + using rclcpp::executor::AnyExecutable; using rclcpp::executor::Executor; @@ -108,6 +110,7 @@ Executor::spin_node_some(rclcpp::node::Node::SharedPtr node) void Executor::spin_some() { + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); } @@ -121,6 +124,7 @@ Executor::spin_some() void Executor::spin_once(std::chrono::nanoseconds timeout) { + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); if (spinning.exchange(true)) { throw std::runtime_error("spin_once() called while already spinning"); } diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index c59987c..e62dbcc 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -20,6 +20,8 @@ #include "rclcpp/utilities.hpp" +#include "../scope_exit.hpp" + using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor; MultiThreadedExecutor::MultiThreadedExecutor(rclcpp::memory_strategy::MemoryStrategy::SharedPtr ms) @@ -36,6 +38,7 @@ MultiThreadedExecutor::~MultiThreadedExecutor() {} void MultiThreadedExecutor::spin() { + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index 962221b..00ce809 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -14,6 +14,8 @@ #include +#include "../scope_exit.hpp" + using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; SingleThreadedExecutor::SingleThreadedExecutor( @@ -26,6 +28,7 @@ SingleThreadedExecutor::~SingleThreadedExecutor() {} void SingleThreadedExecutor::spin() { + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); } From f1e7ea5ca08e135b5a736c53de5d6a9887545f4e Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 16 Nov 2015 17:30:01 -0800 Subject: [PATCH 6/7] remove redundant calls to set spinning false --- rclcpp/src/rclcpp/executor.cpp | 2 -- rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp | 1 - rclcpp/src/rclcpp/executors/single_threaded_executor.cpp | 1 - 3 files changed, 4 deletions(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 48ac9c9..5e0f290 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -118,7 +118,6 @@ Executor::spin_some() while ((any_exec = get_next_executable(std::chrono::milliseconds::zero())) && spinning.load()) { execute_any_executable(any_exec); } - spinning.store(false); } void @@ -132,7 +131,6 @@ Executor::spin_once(std::chrono::nanoseconds timeout) if (any_exec) { execute_any_executable(any_exec); } - spinning.store(false); } void diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index e62dbcc..e7bcc63 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -55,7 +55,6 @@ MultiThreadedExecutor::spin() for (auto & thread : threads) { thread.join(); } - spinning.store(false); } size_t diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index 00ce809..87c25bd 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -36,5 +36,4 @@ SingleThreadedExecutor::spin() auto any_exec = get_next_executable(); execute_any_executable(any_exec); } - spinning.store(false); } From 1c9eb0b36798cc202bc584cae5c0bce78d2ca307 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 17 Nov 2015 11:32:58 -0800 Subject: [PATCH 7/7] only set scope exit after it was determined that spinning was changed to true --- rclcpp/src/rclcpp/executor.cpp | 4 ++-- rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp | 2 +- rclcpp/src/rclcpp/executors/single_threaded_executor.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 5e0f290..981934c 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -110,10 +110,10 @@ Executor::spin_node_some(rclcpp::node::Node::SharedPtr node) void Executor::spin_some() { - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); } + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); AnyExecutable::SharedPtr any_exec; while ((any_exec = get_next_executable(std::chrono::milliseconds::zero())) && spinning.load()) { execute_any_executable(any_exec); @@ -123,10 +123,10 @@ Executor::spin_some() void Executor::spin_once(std::chrono::nanoseconds timeout) { - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); if (spinning.exchange(true)) { throw std::runtime_error("spin_once() called while already spinning"); } + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); auto any_exec = get_next_executable(timeout); if (any_exec) { execute_any_executable(any_exec); diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index e7bcc63..63d555f 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -38,10 +38,10 @@ MultiThreadedExecutor::~MultiThreadedExecutor() {} void MultiThreadedExecutor::spin() { - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); std::vector threads; { 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 87c25bd..204d422 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -28,10 +28,10 @@ SingleThreadedExecutor::~SingleThreadedExecutor() {} void SingleThreadedExecutor::spin() { - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); } + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); while (rclcpp::utilities::ok() && spinning.load()) { auto any_exec = get_next_executable(); execute_any_executable(any_exec);