Skip to content

Commit 2cdef93

Browse files
mini-1235mini-1235
authored
Stop replanning when near the goal if path is valid (#872)
* Stop replanning when near the goal if path is valid Signed-off-by: Maurice <mauricepurnawan@gmail.com> * Update Signed-off-by: Maurice <mauricepurnawan@gmail.com> --------- Signed-off-by: Maurice <mauricepurnawan@gmail.com>
1 parent 9693282 commit 2cdef93

9 files changed

Lines changed: 100 additions & 43 deletions

‎behavior_trees/images/walkthrough/contextual_recoveries.png‎

-70.8 KB
Loading

‎behavior_trees/images/walkthrough/navigation_subtree.png‎

137 KB
Loading

‎behavior_trees/images/walkthrough/overall_bt.png‎

72.5 KB
Loading

‎behavior_trees/images/walkthrough/overall_bt_w_breakdown.png‎

-511 KB
Loading

‎behavior_trees/overview/detailed_behavior_tree_walkthrough.rst‎

Lines changed: 49 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -47,23 +47,36 @@ BTs are primarily defined in XML. The tree shown above is represented in XML as
4747

4848
.. code-block:: xml
4949
50-
<root BTCPP_format="4" main_tree_to_execute="MainTree">
51-
<BehaviorTree ID="MainTree">
50+
<root BTCPP_format="4" main_tree_to_execute="NavigateToPoseWReplanningAndRecovery">
51+
<BehaviorTree ID="NavigateToPoseWReplanningAndRecovery">
5252
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
5353
<PipelineSequence name="NavigateWithReplanning">
54+
<ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
55+
<GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
56+
<PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector"/>
5457
<ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
5558
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
5659
<RateController hz="1.0">
5760
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
58-
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
61+
<Fallback name="FallbackComputePathToPose">
62+
<ReactiveSequence name="CheckIfNewPathNeeded">
63+
<Inverter>
64+
<GlobalUpdatedGoal/>
65+
</Inverter>
66+
<IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
67+
<TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="-1" distance_backward="0.0" />
68+
<IsPathValid path="{remaining_path}"/>
69+
</ReactiveSequence>
70+
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
71+
</Fallback>
5972
<Sequence>
6073
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
6174
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
6275
</Sequence>
6376
</RecoveryNode>
6477
</RateController>
6578
<RecoveryNode number_of_retries="1" name="FollowPath">
66-
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
79+
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
6780
<Sequence>
6881
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
6982
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
@@ -106,6 +119,8 @@ This can be represented in the following way:
106119
107120
The ``Navigation`` subtree mainly involves actual navigation behavior:
108121

122+
- selecting planners / controllers / goal checkers / path handlers / progress checkers plugins
123+
109124
- calculating a path
110125

111126
- following a path
@@ -143,23 +158,36 @@ The XML of this subtree is as follows:
143158
.. code-block:: xml
144159
145160
<PipelineSequence name="NavigateWithReplanning">
161+
<ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
162+
<GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
163+
<PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector"/>
146164
<ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
147165
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
148166
<RateController hz="1.0">
149-
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
150-
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
151-
<Sequence>
152-
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
153-
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
154-
</Sequence>
155-
</RecoveryNode>
156-
</RateController>
157-
<RecoveryNode number_of_retries="1" name="FollowPath">
158-
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
167+
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
168+
<Fallback name="FallbackComputePathToPose">
169+
<ReactiveSequence name="CheckIfNewPathNeeded">
170+
<Inverter>
171+
<GlobalUpdatedGoal/>
172+
</Inverter>
173+
<IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
174+
<TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="-1" distance_backward="0.0" />
175+
<IsPathValid path="{remaining_path}"/>
176+
</ReactiveSequence>
177+
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
178+
</Fallback>
159179
<Sequence>
160-
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
161-
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
180+
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
181+
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
162182
</Sequence>
183+
</RecoveryNode>
184+
</RateController>
185+
<RecoveryNode number_of_retries="1" name="FollowPath">
186+
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
187+
<Sequence>
188+
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
189+
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
190+
</Sequence>
163191
</RecoveryNode>
164192
</PipelineSequence>
165193
@@ -203,6 +231,10 @@ The only differences in the BT subtree of ``ComputePathToPose`` and ``FollowPath
203231
- The ``ComputePathToPose`` subtree centers around the ``ComputePathToPose`` action.
204232
- The ``FollowPath`` subtree centers around the ``FollowPath`` action.
205233

234+
- The use of conditional flow control (``Fallback``):
235+
- 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.
236+
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.
237+
- 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.
206238
- The ``RateController`` that decorates the ``ComputePathToPose`` subtree
207239
The ``RateController`` decorates the ``ComputePathToPose`` subtree to keep planning at the specified frequency. The default frequency for this BT is 1 hz.
208240
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
212244
- The ``ComputePathToPose`` subtree clears the global costmap. The global costmap is the relevant costmap in the context of the planner
213245
- The ``FollowPath`` subtree clears the local costmap. The local costmap is the relevant costmap in the context of the controller
214246

215-
This subtree also utilizes the ``PlannerSelector`` and ``ControllerSelector`` nodes. These nodes ffer flexibility for applications that need to adjust navigation behavior on the fly.
247+
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.
216248

217249
Recovery Subtree
218250
================

‎behavior_trees/trees/nav_through_poses_recovery.rst‎

Lines changed: 19 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@ By convention we name these by the style of algorithms that they are (e.g. not `
1313
In this behavior tree, we attempt to retry the entire navigation task 6 times before returning to the caller that the task has failed.
1414
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.
1515

16-
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`.
16+
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`.
1717
The planner though is now ``ComputePathThroughPoses`` taking a vector, ``goals``, rather than a single pose ``goal`` to plan to.
1818
The ``RemovePassedGoals`` node is used to cull out ``goals`` that the robot has passed on its path.
1919
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
3838

3939
.. code-block:: xml
4040
41-
<root BTCPP_format="4" main_tree_to_execute="MainTree">
42-
<BehaviorTree ID="MainTree">
41+
<root BTCPP_format="4" main_tree_to_execute="NavigateThroughPosesWReplanningAndRecovery">
42+
<BehaviorTree ID="NavigateThroughPosesWReplanningAndRecovery">
4343
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
4444
<PipelineSequence name="NavigateWithReplanning">
4545
<ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
4646
<GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
47-
<PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector" />
47+
<PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector"/>
4848
<ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
4949
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
5050
<RateController hz="0.333">
5151
<RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
52-
<ReactiveSequence>
53-
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
54-
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
55-
</ReactiveSequence>
52+
<Fallback name="FallbackComputePathToPose">
53+
<ReactiveSequence name="CheckIfNewPathNeeded">
54+
<Inverter>
55+
<GlobalUpdatedGoal/>
56+
</Inverter>
57+
<IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
58+
<TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="-1" distance_backward="0.0" />
59+
<IsPathValid path="{remaining_path}"/>
60+
</ReactiveSequence>
61+
<ReactiveSequence>
62+
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
63+
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
64+
</ReactiveSequence>
65+
</Fallback>
5666
<Sequence>
5767
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
5868
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
5969
</Sequence>
6070
</RecoveryNode>
6171
</RateController>
6272
<RecoveryNode number_of_retries="1" name="FollowPath">
63-
<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}"/>
73+
<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}"/>
6474
<Sequence>
6575
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
6676
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>

‎behavior_trees/trees/nav_to_pose_recovery.rst‎

Lines changed: 13 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ By convention we name these by the style of algorithms that they are (e.g. not `
1414
In this behavior tree, we attempt to retry the entire navigation task 6 times before returning to the caller that the task has failed.
1515
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.
1616

17-
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`.
17+
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`.
1818
However, this time, if the planner fails, it will trigger contextually aware recovery behaviors in its subtree, clearing the global costmap.
1919
Additional recovery behaviors can be added here for additional context-specific recoveries, such as trying another algorithm.
2020

@@ -45,15 +45,25 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
4545
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
4646
<RateController hz="1.0">
4747
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
48-
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
48+
<Fallback name="FallbackComputePathToPose">
49+
<ReactiveSequence name="CheckIfNewPathNeeded">
50+
<Inverter>
51+
<GlobalUpdatedGoal/>
52+
</Inverter>
53+
<IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
54+
<TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="-1" distance_backward="0.0" />
55+
<IsPathValid path="{remaining_path}"/>
56+
</ReactiveSequence>
57+
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
58+
</Fallback>
4959
<Sequence>
5060
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
5161
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
5262
</Sequence>
5363
</RecoveryNode>
5464
</RateController>
5565
<RecoveryNode number_of_retries="1" name="FollowPath">
56-
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
66+
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
5767
<Sequence>
5868
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
5969
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>

0 commit comments

Comments
 (0)