From ddbe473a4512127f517a33ae2d011dbc9b60d363 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Tue, 2 Apr 2024 18:35:23 +0200 Subject: [PATCH] fix(StaticSingleThreadedExecutor): Fixed regression in spin() Signed-off-by: Janosch Machowinski --- .../static_single_threaded_executor.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index d517ccafd0..a703480de3 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -35,11 +35,19 @@ StaticSingleThreadedExecutor::spin() } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - // This is essentially the contents of the rclcpp::Executor::wait_for_work method, - // 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)); + while (rclcpp::ok(context_) && spinning.load()) { + // Get executables that are ready now + std::lock_guard guard(mutex_); + + // wait forever until the wait returns + auto wait_result = this->collect_and_wait(std::chrono::nanoseconds(-1)); + if (wait_result.has_value()) { + // Execute ready executables + this->execute_ready_executables( + current_collection_, + wait_result.value(), + false); + } } }