diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 39acba3ec4..77fe4df7d9 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -37,6 +37,7 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/scope_exit.hpp" namespace rclcpp { @@ -212,9 +213,14 @@ class Executor } std::chrono::nanoseconds timeout_left = timeout_ns; - while (rclcpp::ok(this->context_)) { + if (spinning.exchange(true)) { + throw std::runtime_error("spin_until_future_complete() called while already spinning"); + } + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + while (rclcpp::ok(this->context_) && spinning.load()) { // Do one item of work. - spin_once(timeout_left); + spin_once_impl(timeout_left); + // Check if the future is set, return SUCCESS if it is. status = future.wait_for(std::chrono::seconds(0)); if (status == std::future_status::ready) { @@ -336,6 +342,10 @@ class Executor RCLCPP_DISABLE_COPY(Executor) + RCLCPP_PUBLIC + void + spin_once_impl(std::chrono::nanoseconds timeout); + std::list weak_nodes_; std::list guard_conditions_; }; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 878628d901..83a8baf5bd 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -244,6 +244,15 @@ Executor::spin_some(std::chrono::nanoseconds max_duration) } } +void +Executor::spin_once_impl(std::chrono::nanoseconds timeout) +{ + AnyExecutable any_exec; + if (get_next_executable(any_exec, timeout)) { + execute_any_executable(any_exec); + } +} + void Executor::spin_once(std::chrono::nanoseconds timeout) { @@ -251,10 +260,7 @@ Executor::spin_once(std::chrono::nanoseconds timeout) throw std::runtime_error("spin_once() called while already spinning"); } RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); - AnyExecutable any_exec; - if (get_next_executable(any_exec, timeout)) { - execute_any_executable(any_exec); - } + spin_once_impl(timeout); } void