From f97568b772888ae8262277c2ee2aae61afccecf6 Mon Sep 17 00:00:00 2001
From: DongheeYe <45094526+DongheeYe@users.noreply.github.com>
Date: Fri, 12 Jun 2020 02:25:02 +0900
Subject: [PATCH] Fix spin_until_future_complete: check spinning value (#1023)

Signed-off-by: Donghee Ye <donghee.ye@samsung.com>
Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
---
 rclcpp/include/rclcpp/executor.hpp | 14 ++++++++++++--
 rclcpp/src/rclcpp/executor.cpp     | 14 ++++++++++----
 2 files changed, 22 insertions(+), 6 deletions(-)

diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
index cbeda82a24..dd24bc378d 100644
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/rclcpp/include/rclcpp/executor.hpp
@@ -35,6 +35,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
 {
@@ -244,9 +245,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) {
@@ -360,6 +366,10 @@ class Executor
 private:
   RCLCPP_DISABLE_COPY(Executor)
 
+  RCLCPP_PUBLIC
+  void
+  spin_once_impl(std::chrono::nanoseconds timeout);
+
   std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
   std::list<const rcl_guard_condition_t *> guard_conditions_;
 };
diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
index c8ee23752b..4fcce4e8fb 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