Only check for new work once in spin_some (#471)

To prevent the Executor::spin_some() method for potentially running
indefinitely long, a flag only allows the method which loads new
work to execute once per cycle.

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

rclcpp/include/rclcpp/executor.hpp

@@ -337,7 +337,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_new_work = true);
 
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;

rclcpp/src/rclcpp/executor.cpp

@@ -234,13 +234,15 @@ 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); );
+  bool allow_new_work = 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_new_work)) {
       execute_any_executable(any_exec);
     } else {
       break;
     }
+    allow_new_work = false;
   }
 }
 
@@ -561,14 +563,14 @@ 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_new_work)
 {
   bool success = false;
   // Check to see if there are any subscriptions or timers needing service
   // TODO(wjwwood): improve run to run efficiency of this function
   success = get_next_ready_executable(any_executable);
   // If there are none
-  if (!success) {
+  if (!success && allow_new_work) {
     // Wait for subscriptions or timers to work on
     wait_for_work(timeout);
     if (!spinning.load()) {