fix(StaticSingleThreadedExecutor): Fixed rergession in spin()

Signed-off-by: Janosch Machowinski <[email protected]>

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -39,13 +39,18 @@ StaticSingleThreadedExecutor::spin()
   // 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));
+    return this->spin_some_impl(std::chrono::nanoseconds(0), true);
   }
 }
 
 void
 StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration)
 {
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin_some() called while already spinning");
+  }
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false););
+
   // In this context a 0 input max_duration means no duration limit
   if (std::chrono::nanoseconds(0) == max_duration) {
     max_duration = std::chrono::nanoseconds::max();
@@ -56,6 +61,11 @@ StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration)
 void
 StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration)
 {
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin_some() called while already spinning");
+  }
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false););
+
   if (max_duration < std::chrono::nanoseconds(0)) {
     throw std::invalid_argument("max_duration must be greater than or equal to 0");
   }
@@ -72,11 +82,6 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati
       return spin_forever || (cur_duration < max_duration);
     };
 
-  if (spinning.exchange(true)) {
-    throw std::runtime_error("spin_some() called while already spinning");
-  }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false););
-
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
     // Get executables that are ready now
     std::lock_guard<std::mutex> guard(mutex_);