From eb380153fb22c58714ec8576fcd75a22a7be1b12 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Tue, 2 Apr 2024 13:56:08 +0200 Subject: [PATCH] fix(StaticSingleThreadedExecutor): Fixed regression in spin() Signed-off-by: Janosch Machowinski --- .../static_single_threaded_executor.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 831076c61c..12e2815a5a 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -39,13 +39,18 @@ StaticSingleThreadedExecutor::spin() // except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor // behavior. while (rclcpp::ok(this->context_) && spinning.load()) { - this->spin_once_impl(std::chrono::nanoseconds(-1)); + return this->spin_some_impl(std::chrono::nanoseconds(0), true); } } void StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration) { + if (spinning.exchange(true)) { + throw std::runtime_error("spin_some() called while already spinning"); + } + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false);); + // In this context a 0 input max_duration means no duration limit if (std::chrono::nanoseconds(0) == max_duration) { max_duration = std::chrono::nanoseconds::max(); @@ -56,6 +61,11 @@ StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration) void StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration) { + if (spinning.exchange(true)) { + throw std::runtime_error("spin_some() called while already spinning"); + } + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false);); + if (max_duration < std::chrono::nanoseconds(0)) { throw std::invalid_argument("max_duration must be greater than or equal to 0"); } @@ -72,11 +82,6 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati return spin_forever || (cur_duration < max_duration); }; - if (spinning.exchange(true)) { - throw std::runtime_error("spin_some() called while already spinning"); - } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false);); - while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { // Get executables that are ready now std::lock_guard guard(mutex_);