BT action node try 2 with commits based on current master

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -81,7 +81,7 @@ class BtActionNode : public BT::CoroActionNode
   }
 
   // Derived classes can override any of the following methods to hook into the
-  // processing for the action: on_tick, on_server_timeout, and on_success
+  // processing for the action: on_tick, on_wait_for_result, and on_success
 
   // Could do dynamic checks, such as getting updates to values on the blackboard
   virtual void on_tick()
@@ -90,7 +90,7 @@ class BtActionNode : public BT::CoroActionNode
 
   // There can be many loop iterations per tick. Any opportunity to do something after
   // a timeout waiting for a result that hasn't been received yet
-  virtual void on_server_timeout()
+  virtual void on_wait_for_result()
   {
   }
 
@@ -106,7 +106,17 @@ class BtActionNode : public BT::CoroActionNode
     on_tick();
 
 new_goal_received:
-    auto future_goal_handle = action_client_->async_send_goal(goal_);
+
+    bool goal_result_available = false;
+    auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
+    send_goal_options.result_callback =
+      [&](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
+        goal_result_available = true;
+        result_ = result;
+      };
+
+    auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
+
     if (rclcpp::spin_until_future_complete(node_, future_goal_handle) !=
       rclcpp::executor::FutureReturnCode::SUCCESS)
     {
@@ -118,28 +128,18 @@ class BtActionNode : public BT::CoroActionNode
       throw std::runtime_error("Goal was rejected by the action server");
     }
 
-    auto future_result = action_client_->async_get_result(goal_handle_);
-    rclcpp::executor::FutureReturnCode rc;
-    do {
-      rc = rclcpp::spin_until_future_complete(node_, future_result, server_timeout_);
-      if (rc == rclcpp::executor::FutureReturnCode::TIMEOUT) {
-        on_server_timeout();
-
-        // We can handle a new goal if we're still executing
-        auto status = goal_handle_->get_status();
-        if (goal_updated_ && (status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
-          status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ))
-        {
-          goal_updated_ = false;
-          goto new_goal_received;
-        }
-
-        // Yield to any other CoroActionNodes (coroutines)
-        setStatusRunningAndYield();
+    while (rclcpp::ok() && !goal_result_available) {
+      on_wait_for_result();
+      if (goal_updated_) {
+        goal_updated_ = false;
+        goto new_goal_received;
       }
-    } while (rc != rclcpp::executor::FutureReturnCode::SUCCESS);
 
-    result_ = future_result.get();
+      // Yield to any other CoroActionNodes (coroutines)
+      setStatusRunningAndYield();
+      rclcpp::spin_some(node_);
+    }
+
     switch (result_.code) {
       case rclcpp_action::ResultCode::SUCCEEDED:
         on_success();

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -92,7 +92,7 @@ class BtServiceNode : public BT::CoroActionNode
     } else if (rc == rclcpp::executor::FutureReturnCode::TIMEOUT) {
       RCLCPP_WARN(node_->get_logger(),
         "Node timed out while executing service call to %s.", service_name_.c_str());
-      on_server_timeout();
+      on_wait_for_result();
     }
     return BT::NodeStatus::FAILURE;
   }
@@ -105,7 +105,7 @@ class BtServiceNode : public BT::CoroActionNode
 
   // An opportunity to do something after
   // a timeout waiting for a result that hasn't been received yet
-  virtual void on_server_timeout()
+  virtual void on_wait_for_result()
   {
   }
 

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -42,7 +42,7 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
     getInput("controller_id", goal_.controller_id);
   }
 
-  void on_server_timeout() override
+  void on_wait_for_result() override
   {
     // Check if the goal has been updated
     if (config().blackboard->get<bool>("path_updated")) {

nav2_util/include/nav2_util/parameter_events_subscriber.hpp

@@ -19,6 +19,7 @@
 #include <utility>
 #include <unordered_map>
 #include <vector>
+#include <list>
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/parameter_events_filter.hpp"