diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 23e4079cb3..837fb48c80 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -338,6 +338,10 @@ class Executor void get_next_timer(AnyExecutable & any_exec); + RCLCPP_PUBLIC + void + cache_ready_timers(); + RCLCPP_PUBLIC bool get_next_ready_executable(AnyExecutable & any_executable); @@ -346,7 +350,8 @@ class Executor bool get_next_executable( AnyExecutable & any_executable, - std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1), + bool allow_wait = true); /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. std::atomic_bool spinning; @@ -371,6 +376,7 @@ class Executor std::list weak_nodes_; std::list guard_conditions_; + std::list ready_timer_handles_; }; } // namespace executor diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 00b04ebf01..9530effc64 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -234,13 +234,18 @@ Executor::spin_some(std::chrono::nanoseconds max_duration) throw std::runtime_error("spin_some() called while already spinning"); } RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + // during the first run only, the process is allowed to check for new work + bool allow_wait = true; while (spinning.load() && max_duration_not_elapsed()) { AnyExecutable any_exec; - if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) { + if (get_next_executable(any_exec, std::chrono::milliseconds::zero(), allow_wait)) { execute_any_executable(any_exec); } else { break; } + // after the first run, remaining work may be performed, + // but new work may not be added + allow_wait = false; } } @@ -431,6 +436,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) // Collect the subscriptions and timers to be waited on memory_strategy_->clear_handles(); bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_); + cache_ready_timers(); // Clean up any invalid nodes, if they were detected if (has_invalid_weak_nodes) { @@ -530,8 +536,9 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) } void -Executor::get_next_timer(AnyExecutable & any_exec) +Executor::cache_ready_timers() { + ready_timer_handles_.clear(); for (auto & weak_node : weak_nodes_) { auto node = weak_node.lock(); if (!node) { @@ -547,15 +554,24 @@ Executor::get_next_timer(AnyExecutable & any_exec) return timer->is_ready(); }); if (timer_ref) { - any_exec.timer = timer_ref; - any_exec.callback_group = group; - any_exec.node_base = node; - return; + ready_timer_handles_.push_back(timer_ref); } } } } +void +Executor::get_next_timer(AnyExecutable & any_exec) +{ + for (auto timer : ready_timer_handles_) { + any_exec.timer = timer; + any_exec.callback_group = get_group_by_timer(timer); + any_exec.node_base = get_node_by_group(any_exec.callback_group); + ready_timer_handles_.remove(timer); + return; + } +} + bool Executor::get_next_ready_executable(AnyExecutable & any_executable) { @@ -589,7 +605,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) } bool -Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout) +Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout, bool allow_wait) { bool success = false; // Check to see if there are any subscriptions or timers needing service @@ -598,7 +614,11 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos // If there are none if (!success) { // Wait for subscriptions or timers to work on - wait_for_work(timeout); + if (allow_wait) { + // In some cases, we may not want to wait for additional work, instead + // returning control to the caller + wait_for_work(timeout); + } if (!spinning.load()) { return false; }