From 83f344b115b2d40c0b326c970e0e871c38d18e6a Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Fri, 16 Jan 2026 12:50:08 +0000
Subject: [PATCH 1/4] Stop replanning when near the goal if path is valid

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 ..._through_poses_w_replanning_and_recovery.xml | 17 +++++++++++++----
 ...vigate_to_pose_w_replanning_and_recovery.xml | 11 ++++++++++-
 ..._w_replanning_goal_patience_and_recovery.xml | 11 ++++++++++-
 3 files changed, 33 insertions(+), 6 deletions(-)

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 3293b52df45..270288d45d9 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
@@ -14,10 +14,19 @@
         <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
         <RateController hz="0.333">
           <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
-            <ReactiveSequence>
-              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
-              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
-            </ReactiveSequence>
+            <Fallback name="FallbackComputePathToPose">
+              <ReactiveSequence name="CheckIfNewPathNeeded">
+                <Inverter>
+                  <GlobalUpdatedGoal/>
+                </Inverter>
+                <IsGoalNearby path="{path}" proximity_threshold="1.0" max_robot_pose_search_dist="1.5"/>
+                <IsPathValid path="{path}"/>
+              </ReactiveSequence>
+              <ReactiveSequence>
+                <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
+                <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
+              </ReactiveSequence>
+            </Fallback>
             <Sequence>
               <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
               <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_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 5a00c52a20f..5cc2ba129e2 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
@@ -15,7 +15,16 @@
         <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
         <RateController hz="1.0">
           <RecoveryNode number_of_retries="1" name="ComputePathToPose">
-            <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
+            <Fallback name="FallbackComputePathToPose">
+              <ReactiveSequence name="CheckIfNewPathNeeded">
+                <Inverter>
+                  <GlobalUpdatedGoal/>
+                </Inverter>
+                <IsGoalNearby path="{path}" proximity_threshold="1.0" max_robot_pose_search_dist="1.5"/>
+                <IsPathValid path="{path}"/>
+              </ReactiveSequence>
+              <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
+            </Fallback>
             <Sequence>
               <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
               <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_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 7ddbf6297dd..be54c01867d 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
@@ -13,7 +13,16 @@
         <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
         <RateController hz="1.0">
           <RecoveryNode number_of_retries="1" name="ComputePathToPose">
-            <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
+            <Fallback name="FallbackComputePathToPose">
+              <ReactiveSequence name="CheckIfNewPathNeeded">
+                <Inverter>
+                  <GlobalUpdatedGoal/>
+                </Inverter>
+                <IsGoalNearby path="{path}" proximity_threshold="1.0" max_robot_pose_search_dist="1.5"/>
+                <IsPathValid path="{path}"/>
+              </ReactiveSequence>
+              <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
+            </Fallback>
             <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
           </RecoveryNode>
         </RateController>

From 35b7beadbc9b8649f5a78ba793c199f58d3627a9 Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Wed, 11 Feb 2026 15:16:37 +0000
Subject: [PATCH 2/4] Update bts

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 ...th_consistent_replanning_and_if_path_becomes_invalid.xml | 6 ++++--
 .../navigate_through_poses_w_replanning_and_recovery.xml    | 3 ++-
 .../navigate_to_pose_w_replanning_and_recovery.xml          | 3 ++-
 ...gate_to_pose_w_replanning_goal_patience_and_recovery.xml | 3 ++-
 4 files changed, 10 insertions(+), 5 deletions(-)

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 54048f1c171..e1aec9b2599 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
@@ -1,5 +1,5 @@
 <!--
-  This Behavior Tree replans the global path once every 15 seconds or if the path becomes invalid. It also has
+  This Behavior Tree replans the global path once every 15 seconds or if the path becomes invalid and not close enough to goal. It also has
   recovery actions specific to planning / control as well as general system issues.
   This will be continuous if a kinematically valid planner is selected.
 -->
@@ -19,7 +19,9 @@
                 <Inverter>
                   <GlobalUpdatedGoal/>
                 </Inverter>
-                <IsPathValid path="{path}"/>
+                <IsGoalNearby path="{path}" proximity_threshold="1.0" max_robot_pose_search_dist="1.5"/>
+                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="2.0" distance_backward="0.0" />
+                <IsPathValid path="{remaining_path}"/>
               </ReactiveSequence>
               <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
             </Fallback>
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 270288d45d9..c83f9dc3f9f 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
@@ -20,7 +20,8 @@
                   <GlobalUpdatedGoal/>
                 </Inverter>
                 <IsGoalNearby path="{path}" proximity_threshold="1.0" max_robot_pose_search_dist="1.5"/>
-                <IsPathValid path="{path}"/>
+                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="2.0" distance_backward="0.0" />
+                <IsPathValid path="{remaining_path}"/>
               </ReactiveSequence>
               <ReactiveSequence>
                 <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
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 5cc2ba129e2..1eb898bb077 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
@@ -21,7 +21,8 @@
                   <GlobalUpdatedGoal/>
                 </Inverter>
                 <IsGoalNearby path="{path}" proximity_threshold="1.0" max_robot_pose_search_dist="1.5"/>
-                <IsPathValid path="{path}"/>
+                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="2.0" distance_backward="0.0" />
+                <IsPathValid path="{remaining_path}"/>
               </ReactiveSequence>
               <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
             </Fallback>
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 be54c01867d..1553a03f043 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
@@ -19,7 +19,8 @@
                   <GlobalUpdatedGoal/>
                 </Inverter>
                 <IsGoalNearby path="{path}" proximity_threshold="1.0" max_robot_pose_search_dist="1.5"/>
-                <IsPathValid path="{path}"/>
+                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="2.0" distance_backward="0.0" />
+                <IsPathValid path="{remaining_path}"/>
               </ReactiveSequence>
               <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
             </Fallback>

From 6c21f3f65bd67f8f739183076bc225e4eb92e538 Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Tue, 17 Feb 2026 08:03:07 +0000
Subject: [PATCH 3/4] Update proximity threshold

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 ...e_with_consistent_replanning_and_if_path_becomes_invalid.xml | 2 +-
 .../navigate_through_poses_w_replanning_and_recovery.xml        | 2 +-
 .../navigate_to_pose_w_replanning_and_recovery.xml              | 2 +-
 ...navigate_to_pose_w_replanning_goal_patience_and_recovery.xml | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

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 e1aec9b2599..b374ee9112d 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
@@ -19,7 +19,7 @@
                 <Inverter>
                   <GlobalUpdatedGoal/>
                 </Inverter>
-                <IsGoalNearby path="{path}" proximity_threshold="1.0" max_robot_pose_search_dist="1.5"/>
+                <IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
                 <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="2.0" distance_backward="0.0" />
                 <IsPathValid path="{remaining_path}"/>
               </ReactiveSequence>
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 c83f9dc3f9f..c8761e7222e 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
@@ -19,7 +19,7 @@
                 <Inverter>
                   <GlobalUpdatedGoal/>
                 </Inverter>
-                <IsGoalNearby path="{path}" proximity_threshold="1.0" max_robot_pose_search_dist="1.5"/>
+                <IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
                 <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="2.0" distance_backward="0.0" />
                 <IsPathValid path="{remaining_path}"/>
               </ReactiveSequence>
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 1eb898bb077..7109370c694 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
@@ -20,7 +20,7 @@
                 <Inverter>
                   <GlobalUpdatedGoal/>
                 </Inverter>
-                <IsGoalNearby path="{path}" proximity_threshold="1.0" max_robot_pose_search_dist="1.5"/>
+                <IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
                 <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="2.0" distance_backward="0.0" />
                 <IsPathValid path="{remaining_path}"/>
               </ReactiveSequence>
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 1553a03f043..f0e1ddd8cb8 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
@@ -18,7 +18,7 @@
                 <Inverter>
                   <GlobalUpdatedGoal/>
                 </Inverter>
-                <IsGoalNearby path="{path}" proximity_threshold="1.0" max_robot_pose_search_dist="1.5"/>
+                <IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
                 <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="2.0" distance_backward="0.0" />
                 <IsPathValid path="{remaining_path}"/>
               </ReactiveSequence>

From f5e07f3b6007795917aae541ccec00e63261d18e Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Wed, 18 Feb 2026 09:49:27 +0000
Subject: [PATCH 4/4] Set distance forward to -1

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 .../plugins/action/truncate_path_local_action.cpp              | 3 +++
 ..._with_consistent_replanning_and_if_path_becomes_invalid.xml | 2 +-
 .../navigate_through_poses_w_replanning_and_recovery.xml       | 2 +-
 .../navigate_to_pose_w_replanning_and_recovery.xml             | 2 +-
 ...avigate_to_pose_w_replanning_goal_patience_and_recovery.xml | 2 +-
 5 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp
index 7cae783bb9f..991629361dc 100644
--- a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp
+++ b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp
@@ -58,6 +58,9 @@ inline BT::NodeStatus TruncatePathLocal::tick()
   getInput("max_robot_pose_search_dist", max_robot_pose_search_dist);
 
   bool path_pruning = std::isfinite(max_robot_pose_search_dist);
+  if (distance_forward < 0.0) {
+    distance_forward = std::numeric_limits<double>::max();
+  }
   nav_msgs::msg::Path new_path;
   getInput("input_path", new_path);
   if (!path_pruning || new_path != path_) {
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 b374ee9112d..ca5a616b9f9 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
@@ -20,7 +20,7 @@
                   <GlobalUpdatedGoal/>
                 </Inverter>
                 <IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
-                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="2.0" distance_backward="0.0" />
+                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="-1" distance_backward="0.0" />
                 <IsPathValid path="{remaining_path}"/>
               </ReactiveSequence>
               <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
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 c8761e7222e..1468b28201c 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
@@ -20,7 +20,7 @@
                   <GlobalUpdatedGoal/>
                 </Inverter>
                 <IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
-                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="2.0" distance_backward="0.0" />
+                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="-1" distance_backward="0.0" />
                 <IsPathValid path="{remaining_path}"/>
               </ReactiveSequence>
               <ReactiveSequence>
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 7109370c694..ae8aca33011 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
@@ -21,7 +21,7 @@
                   <GlobalUpdatedGoal/>
                 </Inverter>
                 <IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
-                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="2.0" distance_backward="0.0" />
+                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="-1" distance_backward="0.0" />
                 <IsPathValid path="{remaining_path}"/>
               </ReactiveSequence>
               <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
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 f0e1ddd8cb8..a7cd1fab36b 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
@@ -19,7 +19,7 @@
                   <GlobalUpdatedGoal/>
                 </Inverter>
                 <IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
-                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="2.0" distance_backward="0.0" />
+                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="-1" distance_backward="0.0" />
                 <IsPathValid path="{remaining_path}"/>
               </ReactiveSequence>
               <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>