ros-navigation · SteveMacenski · Feb 18, 2026 · Feb 12, 2026 · Feb 18, 2026
diff --git a/behavior_trees/images/walkthrough/contextual_recoveries.png b/behavior_trees/images/walkthrough/contextual_recoveries.png
diff --git a/behavior_trees/images/walkthrough/navigation_subtree.png b/behavior_trees/images/walkthrough/navigation_subtree.png
diff --git a/behavior_trees/images/walkthrough/overall_bt.png b/behavior_trees/images/walkthrough/overall_bt.png
diff --git a/behavior_trees/images/walkthrough/overall_bt_w_breakdown.png b/behavior_trees/images/walkthrough/overall_bt_w_breakdown.png
diff --git a/behavior_trees/overview/detailed_behavior_tree_walkthrough.rst b/behavior_trees/overview/detailed_behavior_tree_walkthrough.rst
@@ -47,23 +47,36 @@ BTs are primarily defined in XML. The tree shown above is represented in XML as
 
 .. code-block:: xml
 
-    <root BTCPP_format="4" main_tree_to_execute="MainTree">
-        <BehaviorTree ID="MainTree">
+    <root BTCPP_format="4" main_tree_to_execute="NavigateToPoseWReplanningAndRecovery">
+        <BehaviorTree ID="NavigateToPoseWReplanningAndRecovery">
             <RecoveryNode number_of_retries="6" name="NavigateRecovery">
                 <PipelineSequence name="NavigateWithReplanning">
+                    <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
+                    <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
+                    <PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector"/>
                     <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
                     <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"/>
+                                    <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>
                             <Sequence>
                                 <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
                                 <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
                             </Sequence>
                         </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"/>
@@ -106,6 +119,8 @@ This can be represented in the following way:
 
 The ``Navigation`` subtree mainly involves actual navigation behavior:
 
+- selecting planners / controllers / goal checkers / path handlers / progress checkers plugins
+
 - calculating a path
 
 - following a path
@@ -143,23 +158,36 @@ The XML of this subtree is as follows:
 .. code-block:: xml
 
     <PipelineSequence name="NavigateWithReplanning">
+        <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
+        <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
+        <PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector"/>
         <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
         <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}"/>
-                <Sequence>
-                    <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
-                    <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
-                </Sequence>
-            </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}"/>
+          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
+            <Fallback name="FallbackComputePathToPose">
+              <ReactiveSequence name="CheckIfNewPathNeeded">
+                <Inverter>
+                  <GlobalUpdatedGoal/>
+                </Inverter>
+                <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>
             <Sequence>
-                <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
-                <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+              <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
+              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
             </Sequence>
+          </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}" tracking_feedback="{tracking_feedback}"/>
+          <Sequence>
+            <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
+            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+          </Sequence>
         </RecoveryNode>
     </PipelineSequence>
 
@@ -203,6 +231,10 @@ The only differences in the BT subtree of ``ComputePathToPose`` and ``FollowPath
     - The ``ComputePathToPose`` subtree centers around the ``ComputePathToPose`` action.
     - The ``FollowPath`` subtree centers around the ``FollowPath`` action.
 
+- The use of conditional flow control (``Fallback``):
+    - The ``ComputePathToPose`` subtree incorporates logic to handle the robot's behavior as it nears the goal. When using feasible planners, re-planning within a small radius (e.g., < 1.0m) can be detrimental due to state estimation drift or path-tracking errors, often resulting in unnecessary "looping" behaviors.
+      To prevent this, the subtree uses a ``ReactiveSequence`` with the ``IsGoalNearby`` node. If the robot is within a specified proximity threshold and the current path remains valid (i.e., no new obstacles), the subtree will skip the re-planning request. This allows the robot to smoothly transition into its final approach using its current path without unnecessary re-planning.
+    - The ``FollowPath`` subtree, by contrast, does not typically use this conditional gating. Once a path is available, the controller is invoked directly to produce velocity commands.
 - The ``RateController`` that decorates the ``ComputePathToPose`` subtree
     The ``RateController`` decorates the ``ComputePathToPose`` subtree to keep planning at the specified frequency. The default frequency for this BT is 1 hz.
     This is done to prevent the BT from flooding the planning server with too many useless requests at the tree update rate (100Hz). Consider changing this frequency to something higher or lower depending on the application and the computational cost of
@@ -212,7 +244,7 @@ The only differences in the BT subtree of ``ComputePathToPose`` and ``FollowPath
     - The ``ComputePathToPose`` subtree clears the global costmap. The global costmap is the relevant costmap in the context of the planner
     - The ``FollowPath`` subtree clears the local costmap. The local costmap is the relevant costmap in the context of the controller
 
-This subtree also utilizes the ``PlannerSelector`` and ``ControllerSelector`` nodes. These nodes ffer flexibility for applications that need to adjust navigation behavior on the fly.
+This subtree also utilizes the ``PlannerSelector``, ``ControllerSelector``, ``GoalCheckerSelector``, ``ProgressCheckerSelector``, and ``PathHandlerSelector`` nodes. These nodes offer flexibility for applications that need to adjust navigation behavior on the fly.
 
 Recovery Subtree
 ================

diff --git a/behavior_trees/trees/nav_through_poses_recovery.rst b/behavior_trees/trees/nav_through_poses_recovery.rst
@@ -13,7 +13,7 @@ By convention we name these by the style of algorithms that they are (e.g. not `
 In this behavior tree, we attempt to retry the entire navigation task 6 times before returning to the caller that the task has failed.
 This allows the navigation system ample opportunity to try to recovery from failure conditions or wait for transient issues to pass, such as crowding from people or a temporary sensor failure.
 
-In nominal execution, this will replan the path at every 3 seconds and pass that path onto the controller, similar to the behavior tree in :ref:`behavior_trees`.
+In nominal execution, this will replan the path at every 3 seconds if not close enough to goal and pass that path onto the controller, similar to the behavior tree in :ref:`behavior_trees`.
 The planner though is now ``ComputePathThroughPoses`` taking a vector, ``goals``, rather than a single pose ``goal`` to plan to.
 The ``RemovePassedGoals`` node is used to cull out ``goals`` that the robot has passed on its path.
 In this case, it is set to remove a pose from the poses when the robot is within ``0.5`` of the goal and it is the next goal in the list.
@@ -38,29 +38,39 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
 
 .. code-block:: xml
 
-  <root BTCPP_format="4" main_tree_to_execute="MainTree">
-    <BehaviorTree ID="MainTree">
+  <root BTCPP_format="4" main_tree_to_execute="NavigateThroughPosesWReplanningAndRecovery">
+    <BehaviorTree ID="NavigateThroughPosesWReplanningAndRecovery">
       <RecoveryNode number_of_retries="6" name="NavigateRecovery">
         <PipelineSequence name="NavigateWithReplanning">
           <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
           <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
-          <PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector" />
+          <PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector"/>
           <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
           <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"/>
+                  <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}"/>
+                  <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"/>
               </Sequence>
             </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/behavior_trees/trees/nav_to_pose_recovery.rst b/behavior_trees/trees/nav_to_pose_recovery.rst
@@ -14,7 +14,7 @@ By convention we name these by the style of algorithms that they are (e.g. not `
 In this behavior tree, we attempt to retry the entire navigation task 6 times before returning to the caller that the task has failed.
 This allows the navigation system ample opportunity to try to recovery from failure conditions or wait for transient issues to pass, such as crowding from people or a temporary sensor failure.
 
-In nominal execution, this will replan the path at every second and pass that path onto the controller, similar to the behavior tree in :ref:`behavior_trees`.
+In nominal execution, this will replan the path at every second if not close enough to goal and pass that path onto the controller, similar to the behavior tree in :ref:`behavior_trees`.
 However, this time, if the planner fails, it will trigger contextually aware recovery behaviors in its subtree, clearing the global costmap.
 Additional recovery behaviors can be added here for additional context-specific recoveries, such as trying another algorithm.
 
@@ -45,15 +45,25 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
           <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"/>
+                  <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>
               <Sequence>
                 <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
                 <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
               </Sequence>
             </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"/>