Limit spin_some to only service individual timers and waitables once …

…per execution (#471)

Distribution Statement A; OPSEC #2893

Signed-off-by: Roger Strain <rstrain@swri.org>

rclcpp/include/rclcpp/executor.hpp

@@ -337,6 +337,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);
@@ -345,7 +349,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;
@@ -367,6 +372,7 @@ class Executor
 
   std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
   std::list<const rcl_guard_condition_t *> guard_conditions_;
+  std::list<rclcpp::TimerBase::SharedPtr> ready_timer_handles_;
 };
 
 }  // namespace executor

rclcpp/src/rclcpp/executor.cpp

@@ -232,13 +232,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;
   }
 }
 
@@ -426,6 +431,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) {
@@ -523,8 +529,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) {
@@ -538,16 +545,25 @@ Executor::get_next_timer(AnyExecutable & any_exec)
       for (auto & timer_ref : group->get_timer_ptrs()) {
         auto timer = timer_ref.lock();
         if (timer && timer->is_ready()) {
-          any_exec.timer = timer;
-          any_exec.callback_group = group;
-          any_exec.node_base = node;
-          return;
+          ready_timer_handles_.push_back(timer);
         }
       }
     }
   }
 }
 
+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)
 {
@@ -581,7 +597,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
@@ -590,7 +606,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;
     }