From 42c990599294d0b2584f1a9b9078454cc18ab80f Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Wed, 15 Oct 2025 11:16:20 +0000
Subject: [PATCH 1/3] Add output port in FollowPath

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 .../plugins/action/follow_path_action.hpp       |  9 +++++++++
 nav2_behavior_tree/nav2_tree_nodes.xml          |  1 +
 .../plugins/action/follow_path_action.cpp       | 17 ++++++++++++++++-
 .../behavior_trees/follow_point.xml             |  2 +-
 ...t_replanning_and_if_path_becomes_invalid.xml |  2 +-
 .../navigate_on_route_graph_w_recovery.xml      |  2 +-
 ..._through_poses_w_replanning_and_recovery.xml |  2 +-
 ...vigate_to_pose_w_replanning_and_recovery.xml |  2 +-
 ..._w_replanning_goal_patience_and_recovery.xml |  2 +-
 ..._replanning_only_if_path_becomes_invalid.xml |  2 +-
 .../navigate_w_replanning_distance.xml          |  2 +-
 ...ate_w_replanning_only_if_goal_is_updated.xml |  2 +-
 ..._replanning_only_if_path_becomes_invalid.xml |  2 +-
 .../navigate_w_replanning_speed.xml             |  2 +-
 .../navigate_w_replanning_time.xml              |  2 +-
 ...g_global_planning_and_control_w_recovery.xml |  2 +-
 16 files changed, 39 insertions(+), 14 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
index dda57080644..b3b00a2f4b5 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
@@ -79,6 +79,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
@@ -96,12 +101,16 @@ 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>(
           "error_msg", "The follow path error msg"),
       });
   }
+
+protected:
+  Action::Feedback feedback_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
index b62ec4a487c..e43ea0172ee 100644
--- a/nav2_behavior_tree/nav2_tree_nodes.xml
+++ b/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>
diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp
index 4184e68bf4c..4e7181ff95e 100644
--- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp
+++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp
@@ -39,6 +39,7 @@ void FollowPathAction::on_tick()
 
 BT::NodeStatus FollowPathAction::on_success()
 {
+  resetFeedbackAndOutputPorts();
   setOutput("error_code_id", ActionResult::NONE);
   setOutput("error_msg", "");
   return BT::NodeStatus::SUCCESS;
@@ -46,6 +47,7 @@ BT::NodeStatus FollowPathAction::on_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;
@@ -53,6 +55,7 @@ BT::NodeStatus FollowPathAction::on_aborted()
 
 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,18 @@ void FollowPathAction::on_wait_for_result(
     goal_.path_handler_id = new_path_handler_id;
     goal_updated_ = true;
   }
+
+  if (feedback) {
+    feedback_ = *feedback;
+    setOutput("tracking_feedback", feedback_.tracking_feedback);
+  }
+}
+
+void FollowPathAction::resetFeedbackAndOutputPorts()
+{
+  nav2_msgs::msg::TrackingFeedback empty_feedback;
+  feedback_.tracking_feedback = empty_feedback;
+  setOutput("tracking_feedback", feedback_.tracking_feedback);
 }
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml
index 5d5310f79cd..ab0ff781fbe 100644
--- a/nav2_bt_navigator/behavior_trees/follow_point.xml
+++ b/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>
diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml
index f2b6589d927..54048f1c171 100644
--- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml
+++ b/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>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml
index 44662cb4656..6560dc4c441 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml
+++ b/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"/>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
index 2e2a1b9c456..3293b52df45 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
+++ b/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"/>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
index f6a8e08e8f9..5a00c52a20f 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
+++ b/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"/>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml
index f79984b750e..7ddbf6297dd 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml
+++ b/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>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml
index c13b2629dea..139da7d9a93 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml
+++ b/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>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml
index fa39ff05283..c500a3ca2e9 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml
+++ b/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>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml
index f64063bbdcb..d3735fa9447 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml
+++ b/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>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml
index 1607057c0e2..ad56f441325 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml
+++ b/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>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml
index 9c9342e8bbd..db512b294bd 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml
+++ b/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>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml
index 2e43016775d..645500fe482 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml
+++ b/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>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml
index 4692c9e17b0..e8cb406c8df 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml
+++ b/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"/>

From b924e2799d157ba51e32515c5193339b2d56923d Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Thu, 8 Jan 2026 18:52:46 +0000
Subject: [PATCH 2/3] Lint

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 .../nav2_behavior_tree/plugins/action/follow_path_action.hpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
index b3b00a2f4b5..01f91dd0698 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
@@ -79,7 +79,7 @@ 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
    */
@@ -101,7 +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<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>(

From c717163e699384f2585752a69a2c984a3b8fca7b Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Tue, 13 Jan 2026 14:43:32 +0000
Subject: [PATCH 3/3] Remove storing feedback

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 .../plugins/action/follow_path_action.hpp                   | 3 ---
 nav2_behavior_tree/plugins/action/follow_path_action.cpp    | 6 ++----
 .../test/plugins/action/test_follow_path_action.cpp         | 4 ++++
 3 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
index 01f91dd0698..ebced08ed7a 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
@@ -109,9 +109,6 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
           "error_msg", "The follow path error msg"),
       });
   }
-
-protected:
-  Action::Feedback feedback_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp
index 4e7181ff95e..6db8b3b09ff 100644
--- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp
+++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp
@@ -115,16 +115,14 @@ void FollowPathAction::on_wait_for_result(
   }
 
   if (feedback) {
-    feedback_ = *feedback;
-    setOutput("tracking_feedback", feedback_.tracking_feedback);
+    setOutput("tracking_feedback", feedback->tracking_feedback);
   }
 }
 
 void FollowPathAction::resetFeedbackAndOutputPorts()
 {
   nav2_msgs::msg::TrackingFeedback empty_feedback;
-  feedback_.tracking_feedback = empty_feedback;
-  setOutput("tracking_feedback", feedback_.tracking_feedback);
+  setOutput("tracking_feedback", empty_feedback);
 }
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp
index 2deecf33b53..0ac8837b38e 100644
--- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp
+++ b/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);
 }