From 871f0fc287d113175e5c0c9811a02a3b4bbb52b1 Mon Sep 17 00:00:00 2001
From: Michael Jeronimo <michael.jeronimo@intel.com>
Date: Wed, 5 Jun 2019 16:36:44 -0700
Subject: [PATCH] Only allow one spin_until_future_complete at a time (#804)

---
 .../include/nav2_tasks/bt_action_node.hpp      | 18 +++++++++++++++---
 1 file changed, 15 insertions(+), 3 deletions(-)

diff --git a/nav2_tasks/include/nav2_tasks/bt_action_node.hpp b/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
index 46000b6a0d..54565b957d 100644
--- a/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
+++ b/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
@@ -16,6 +16,7 @@
 #define NAV2_TASKS__BT_ACTION_NODE_HPP_
 
 #include <memory>
+#include <mutex>
 #include <string>
 
 #include "behaviortree_cpp/action_node.h"
@@ -51,7 +52,7 @@ class BtActionNode : public BT::AsyncActionNode
   // but override on_init instead.
   void onInit() final
   {
-    node_ = nav2_util::generate_internal_node();
+    node_ = nav2_util::generate_internal_node(action_name_);
 
     // Initialize the input and output messages
     goal_ = typename ActionT::Goal();
@@ -123,7 +124,12 @@ class BtActionNode : public BT::AsyncActionNode
     auto future_result = goal_handle_->async_result();
     rclcpp::executor::FutureReturnCode rc;
     do {
-      rc = rclcpp::spin_until_future_complete(node_, future_result, node_loop_timeout_);
+      {
+        // Can only have one spin_until_future_complete at a time. Otherwise we get
+        // the "already on a executor" error
+        std::lock_guard<std::mutex> guard(spin_mutex_);
+        rc = rclcpp::spin_until_future_complete(node_, future_result, node_loop_timeout_);
+      }
 
       if (rc == rclcpp::executor::FutureReturnCode::TIMEOUT) {
         on_loop_timeout();
@@ -165,7 +171,10 @@ class BtActionNode : public BT::AsyncActionNode
     if (status() == BT::NodeStatus::RUNNING) {
       action_client_->async_cancel_goal(goal_handle_);
       auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
-      rclcpp::spin_until_future_complete(node_, future_cancel);
+      {
+        std::lock_guard<std::mutex> guard(spin_mutex_);
+        rclcpp::spin_until_future_complete(node_, future_cancel);
+      }
     }
 
     setStatus(BT::NodeStatus::IDLE);
@@ -187,6 +196,9 @@ class BtActionNode : public BT::AsyncActionNode
   // The timeout value while to use in the tick loop while waiting for
   // a result from the server
   std::chrono::milliseconds node_loop_timeout_;
+
+  // A mutex to allow only one spin_until_future_complete at a time
+  std::mutex spin_mutex_;
 };
 
 }  // namespace nav2_tasks