Allow access to action feedback in nav2_behavior_tree::BtActionNode::on_wait_for_result

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -132,9 +132,12 @@ class BtActionNode : public BT::ActionNodeBase
 
   /**
    * @brief Function to perform some user-defined operation after a timeout
-   * waiting for a result that hasn't been received yet
+   * waiting for a result that hasn't been received yet. Also provides access to
+   * the latest feedback message from the action server. Feedback will be nullptr
+   * in subsequent calls to this function if no new feedback is received while waiting for a result.
+   * @param feedback shared_ptr to latest feedback message, nullptr if no new feedback was received
    */
-  virtual void on_wait_for_result()
+  virtual void on_wait_for_result(std::shared_ptr<const typename ActionT::Feedback>/*feedback*/)
   {
   }
 
@@ -206,7 +209,10 @@ class BtActionNode : public BT::ActionNodeBase
       // The following code corresponds to the "RUNNING" loop
       if (rclcpp::ok() && !goal_result_available_) {
         // user defined callback. May modify the value of "goal_updated_"
-        on_wait_for_result();
+        on_wait_for_result(feedback_);
+
+        // reset feedback to avoid stale information
+        feedback_.reset();
 
         auto goal_status = goal_handle_->get_status();
         if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
@@ -340,6 +346,11 @@ class BtActionNode : public BT::ActionNodeBase
           result_ = result;
         }
       };
+    send_goal_options.feedback_callback =
+      [this](typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
+        const std::shared_ptr<const typename ActionT::Feedback> feedback) {
+        feedback_ = feedback;
+      };
 
     future_goal_handle_ = std::make_shared<
       std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>(
@@ -406,6 +417,9 @@ class BtActionNode : public BT::ActionNodeBase
   typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle_;
   typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult result_;
 
+  // To handle feedback from action server
+  std::shared_ptr<const typename ActionT::Feedback> feedback_;
+
   // The node that will be used for any ROS operations
   rclcpp::Node::SharedPtr node_;
   rclcpp::CallbackGroup::SharedPtr callback_group_;

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp

@@ -16,6 +16,7 @@
 #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
 
 #include <string>
+#include <memory>
 
 #include "nav2_msgs/action/follow_path.hpp"
 #include "nav2_behavior_tree/bt_action_node.hpp"
@@ -48,8 +49,10 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
   /**
    * @brief Function to perform some user-defined operation after a timeout
    * waiting for a result that hasn't been received yet
+   * @param feedback shared_ptr to latest feedback message
    */
-  void on_wait_for_result() override;
+  void on_wait_for_result(
+    std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback> feedback) override;
 
   /**
    * @brief Creates list of BT ports

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -35,7 +35,8 @@ void FollowPathAction::on_tick()
   getInput("goal_checker_id", goal_.goal_checker_id);
 }
 
-void FollowPathAction::on_wait_for_result()
+void FollowPathAction::on_wait_for_result(
+  std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback>/*feedback*/)
 {
   // Grab the new path
   nav_msgs::msg::Path new_path;