Add tracking feedback in FollowPath output port

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

@@ -80,6 +80,11 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
   void on_wait_for_result(
     std::shared_ptr<const Action::Feedback> feedback) override;
 
+  /**
+   * @brief Function to set all feedbacks and output ports to be null values
+   */
+  void resetFeedbackAndOutputPorts();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -96,6 +101,8 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
         BT::InputPort<std::string>("goal_checker_id", ""),
         BT::InputPort<std::string>("progress_checker_id", ""),
         BT::InputPort<std::string>("path_handler_id", ""),
+        BT::OutputPort<nav2_msgs::msg::TrackingFeedback>("tracking_feedback",
+          "Tracking feedback from controller server"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The follow path error code"),
         BT::OutputPort<std::string>(

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -189,6 +189,7 @@
       <input_port name="path_handler_id">Path handler</input_port>
       <input_port name="server_name">Action server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <output_port name="tracking_feedback">Tracking feedback from controller server</output_port>
       <output_port name="error_code_id">Follow Path error code</output_port>
       <output_port name="error_msg">Follow Path error message</output_port>
     </Action>

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -39,20 +39,23 @@ void FollowPathAction::on_tick()
 
 BT::NodeStatus FollowPathAction::on_success()
 {
+  resetFeedbackAndOutputPorts();
   setOutput("error_code_id", ActionResult::NONE);
   setOutput("error_msg", "");
   return BT::NodeStatus::SUCCESS;
 }
 
 BT::NodeStatus FollowPathAction::on_aborted()
 {
+  resetFeedbackAndOutputPorts();
   setOutput("error_code_id", result_.result->error_code);
   setOutput("error_msg", result_.result->error_msg);
   return BT::NodeStatus::FAILURE;
 }
 
 BT::NodeStatus FollowPathAction::on_cancelled()
 {
+  resetFeedbackAndOutputPorts();
   // Set empty error code, action was cancelled
   setOutput("error_code_id", ActionResult::NONE);
   setOutput("error_msg", "");
@@ -66,7 +69,7 @@ void FollowPathAction::on_timeout()
 }
 
 void FollowPathAction::on_wait_for_result(
-  std::shared_ptr<const Action::Feedback>/*feedback*/)
+  std::shared_ptr<const Action::Feedback> feedback)
 {
   // Grab the new path
   nav_msgs::msg::Path new_path;
@@ -110,6 +113,16 @@ void FollowPathAction::on_wait_for_result(
     goal_.path_handler_id = new_path_handler_id;
     goal_updated_ = true;
   }
+
+  if (feedback) {
+    setOutput("tracking_feedback", feedback->tracking_feedback);
+  }
+}
+
+void FollowPathAction::resetFeedbackAndOutputPorts()
+{
+  nav2_msgs::msg::TrackingFeedback empty_feedback;
+  setOutput("tracking_feedback", empty_feedback);
 }
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp

@@ -200,6 +200,7 @@ TEST(FollowPathAction, testProgressCheckerIdUpdate)
   config->blackboard->set("progress_checker_id", std::string("new_progress_checker"));
   auto feedback = std::make_shared<nav2_msgs::action::FollowPath::Feedback>();
   auto follow_path_node = dynamic_cast<nav2_behavior_tree::FollowPathAction *>(tree->rootNode());
+  ASSERT_NE(follow_path_node, nullptr);
   follow_path_node->on_wait_for_result(feedback);
 }
 
@@ -239,6 +240,7 @@ TEST(FollowPathAction, testGoalCheckerIdUpdate)
   config->blackboard->set("goal_checker_id", std::string("new_goal_checker"));
   auto feedback = std::make_shared<nav2_msgs::action::FollowPath::Feedback>();
   auto follow_path_node = dynamic_cast<nav2_behavior_tree::FollowPathAction *>(tree->rootNode());
+  ASSERT_NE(follow_path_node, nullptr);
   follow_path_node->on_wait_for_result(feedback);
 }
 
@@ -278,6 +280,7 @@ TEST(FollowPathAction, testControllerIdUpdate)
   config->blackboard->set("controller_id", std::string("new_controller"));
   auto feedback = std::make_shared<nav2_msgs::action::FollowPath::Feedback>();
   auto follow_path_node = dynamic_cast<nav2_behavior_tree::FollowPathAction *>(tree->rootNode());
+  ASSERT_NE(follow_path_node, nullptr);
   follow_path_node->on_wait_for_result(feedback);
 }
 
@@ -317,6 +320,7 @@ TEST(FollowPathAction, testPathHandlerUpdate)
   config->blackboard->set("path_handler_id", std::string("new_path_handler"));
   auto feedback = std::make_shared<nav2_msgs::action::FollowPath::Feedback>();
   auto follow_path_node = dynamic_cast<nav2_behavior_tree::FollowPathAction *>(tree->rootNode());
+  ASSERT_NE(follow_path_node, nullptr);
   follow_path_node->on_wait_for_result(feedback);
 }
 

nav2_bt_navigator/behavior_trees/follow_point.xml

@@ -16,7 +16,7 @@
         </Sequence>
       </RateController>
       <KeepRunningUntilFailure>
-        <FollowPath path="{truncated_path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+        <FollowPath path="{truncated_path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
       </KeepRunningUntilFailure>
     </PipelineSequence>
   </BehaviorTree>

nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml

@@ -27,7 +27,7 @@
           </RecoveryNode>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
-          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
           <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
         </RecoveryNode>
       </PipelineSequence>

nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml

@@ -65,7 +65,7 @@
           </Sequence>
         </RecoveryNode>
         <RecoveryNode number_of_retries="1" name="FollowPath">
-          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
           <Sequence>
             <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
             <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>

nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml

@@ -25,7 +25,7 @@
           </RecoveryNode>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
-          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" goal_checker_id="{selected_goal_checker}" progress_checker_id="{selected_progress_checker}" path_handler_id="{selected_path_handler}"/>
+          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" goal_checker_id="{selected_goal_checker}" progress_checker_id="{selected_progress_checker}" path_handler_id="{selected_path_handler}" tracking_feedback="{tracking_feedback}"/>
           <Sequence>
             <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
             <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>

nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml

@@ -23,7 +23,7 @@
           </RecoveryNode>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
-          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
           <Sequence>
             <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
             <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>

nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml

@@ -27,7 +27,7 @@
             </RetryUntilSuccessful>
           </PathLongerOnApproach>
           <RecoveryNode number_of_retries="1" name="FollowPath">
-            <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+            <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
             <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
           </RecoveryNode>
         </ReactiveSequence>

nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml

@@ -25,7 +25,7 @@
           </RecoveryNode>
         </RateController>
         <RecoveryNode number_of_retries="1" name="RecoveryFollowPath">
-          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
           <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
         </RecoveryNode>
       </PipelineSequence>

nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml

@@ -10,7 +10,7 @@
       <DistanceController distance="1.0">
         <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
       </DistanceController>
-      <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+      <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
     </PipelineSequence>
   </BehaviorTree>
 </root>

nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml

@@ -10,7 +10,7 @@
       <GoalUpdatedController>
         <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
       </GoalUpdatedController>
-      <FollowPath path="{path}"  controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+      <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
     </PipelineSequence>
   </BehaviorTree>
 </root>

nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml

@@ -17,7 +17,7 @@
           <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
         </Fallback>
       </RateController>
-      <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+      <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
     </PipelineSequence>
   </BehaviorTree>
 </root>

nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml

@@ -10,7 +10,7 @@
       <SpeedController min_rate="0.1" max_rate="1.0" min_speed="0.0" max_speed="0.26">
         <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
       </SpeedController>
-      <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+      <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
     </PipelineSequence>
   </BehaviorTree>
 </root>

nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml

@@ -10,7 +10,7 @@
       <RateController hz="1.0">
         <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
       </RateController>
-      <FollowPath path="{path}"  controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+      <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
     </PipelineSequence>
   </BehaviorTree>
 </root>

nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml

@@ -64,7 +64,7 @@
 
         <!-- Follow the path computed by the planner, with contextual recoveries -->
         <RecoveryNode number_of_retries="1" name="FollowPath">
-          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
           <Sequence>
             <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
             <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>