Revert back to CoroActionNodes instead of AsyncActionNodes (#836)

nav2_tasks/include/nav2_tasks/bt_action_node.hpp

@@ -16,7 +16,6 @@
 #define NAV2_TASKS__BT_ACTION_NODE_HPP_
 
 #include <memory>
-#include <mutex>
 #include <string>
 
 #include "behaviortree_cpp/action_node.h"
@@ -27,16 +26,16 @@ namespace nav2_tasks
 {
 
 template<class ActionT>
-class BtActionNode : public BT::AsyncActionNode
+class BtActionNode : public BT::CoroActionNode
 {
 public:
   explicit BtActionNode(const std::string & action_name)
-  : BT::AsyncActionNode(action_name), action_name_(action_name)
+  : BT::CoroActionNode(action_name), action_name_(action_name)
   {
   }
 
   BtActionNode(const std::string & action_name, const BT::NodeParameters & params)
-  : BT::AsyncActionNode(action_name, params), action_name_(action_name)
+  : BT::CoroActionNode(action_name, params), action_name_(action_name)
   {
   }
 
@@ -52,7 +51,7 @@ public:
   // but override on_init instead.
   void onInit() final
   {
-    node_ = nav2_util::generate_internal_node(action_name_);
+    node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
 
     // Initialize the input and output messages
     goal_ = typename ActionT::Goal();
@@ -124,13 +123,7 @@ new_goal_received:
     auto future_result = goal_handle_->async_result();
     rclcpp::executor::FutureReturnCode rc;
     do {
-      {
-        // 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_);
-      }
-
+      rc = rclcpp::spin_until_future_complete(node_, future_result, node_loop_timeout_);
       if (rc == rclcpp::executor::FutureReturnCode::TIMEOUT) {
         on_loop_timeout();
 
@@ -140,6 +133,9 @@ new_goal_received:
           goal_updated_ = false;
           goto new_goal_received;
         }
+
+        // Yield to any other CoroActionNodes (coroutines)
+        setStatusRunningAndYield();
       }
     } while (rc != rclcpp::executor::FutureReturnCode::SUCCESS);
 
@@ -171,18 +167,15 @@ new_goal_received:
     if (status() == BT::NodeStatus::RUNNING) {
       action_client_->async_cancel_goal(goal_handle_);
       auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
-      {
-        std::lock_guard<std::mutex> guard(spin_mutex_);
-        if (rclcpp::spin_until_future_complete(node_, future_cancel) !=
-          rclcpp::executor::FutureReturnCode::SUCCESS)
-        {
-          RCLCPP_ERROR(node_->get_logger(),
-            "Failed to cancel action server for %s", action_name_.c_str());
-        }
+      if (rclcpp::spin_until_future_complete(node_, future_cancel) !=
+        rclcpp::executor::FutureReturnCode::SUCCESS) {
+        RCLCPP_ERROR(node_->get_logger(),
+          "Failed to cancel action server for %s", action_name_.c_str());
       }
     }
 
     setStatus(BT::NodeStatus::IDLE);
+    CoroActionNode::halt();
   }
 
 protected:
@@ -201,9 +194,6 @@ protected:
   // 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