Skip to content

Commit 17d750e

Browse files
mini-1235mini-1235
authored
Update behavior tree walkthrough (#707)
* Update behavior tree walkthrough Signed-off-by: Maurice-1235 <mauricepurnawan@gmail.com> * Fix pre-commit Signed-off-by: Maurice-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: Maurice-1235 <mauricepurnawan@gmail.com>
1 parent 5f61435 commit 17d750e

9 files changed

Lines changed: 72 additions & 48 deletions

‎behavior_trees/images/walkthrough/contextual_recoveries.png‎

540 KB
Loading

‎behavior_trees/images/walkthrough/navigation_subtree.png‎

30.8 KB
Loading

‎behavior_trees/images/walkthrough/navigation_subtree_bare.png‎

7.37 KB
Loading

‎behavior_trees/images/walkthrough/overall_bt.png‎

68.3 KB
Loading

‎behavior_trees/images/walkthrough/overall_bt_w_breakdown.png‎

860 KB
Loading

‎behavior_trees/images/walkthrough/recovery_subtree.png‎

31.1 KB
Loading

‎behavior_trees/overview/detailed_behavior_tree_walkthrough.rst‎

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

4848
.. code-block:: xml
4949
50-
<root main_tree_to_execute="MainTree">
50+
<root BTCPP_format="4" main_tree_to_execute="MainTree">
5151
<BehaviorTree ID="MainTree">
5252
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
5353
<PipelineSequence name="NavigateWithReplanning">
54+
<ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
55+
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
5456
<RateController hz="1.0">
5557
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
56-
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
57-
<ReactiveFallback name="ComputePathToPoseRecoveryFallback">
58-
<GoalUpdated/>
58+
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
59+
<Sequence>
60+
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
5961
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
60-
</ReactiveFallback>
62+
</Sequence>
6163
</RecoveryNode>
6264
</RateController>
6365
<RecoveryNode number_of_retries="1" name="FollowPath">
64-
<FollowPath path="{path}" controller_id="FollowPath"/>
65-
<ReactiveFallback name="FollowPathRecoveryFallback">
66-
<GoalUpdated/>
66+
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
67+
<Sequence>
68+
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
6769
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
68-
</ReactiveFallback>
70+
</Sequence>
6971
</RecoveryNode>
7072
</PipelineSequence>
71-
<ReactiveFallback name="RecoveryFallback">
72-
<GoalUpdated/>
73-
<RoundRobin name="RecoveryActions">
74-
<Sequence name="ClearingActions">
73+
<Sequence>
74+
<Fallback>
75+
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
76+
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
77+
</Fallback>
78+
<ReactiveFallback name="RecoveryFallback">
79+
<GoalUpdated/>
80+
<RoundRobin name="RecoveryActions">
81+
<Sequence name="ClearingActions">
7582
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
7683
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
77-
</Sequence>
78-
<Spin spin_dist="1.57"/>
79-
<Wait wait_duration="5"/>
80-
<BackUp backup_dist="0.15" backup_speed="0.025"/>
81-
</RoundRobin>
82-
</ReactiveFallback>
84+
</Sequence>
85+
<Spin spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
86+
<Wait wait_duration="5.0" error_code_id="{wait_error_code}" error_msg="{wait_error_msg}"/>
87+
<BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
88+
</RoundRobin>
89+
</ReactiveFallback>
90+
</Sequence>
8391
</RecoveryNode>
8492
</BehaviorTree>
8593
</root>
@@ -135,21 +143,23 @@ The XML of this subtree is as follows:
135143
.. code-block:: xml
136144
137145
<PipelineSequence name="NavigateWithReplanning">
146+
<ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
147+
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
138148
<RateController hz="1.0">
139149
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
140-
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
141-
<ReactiveFallback name="ComputePathToPoseRecoveryFallback">
142-
<GoalUpdated/>
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}"/>
143153
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
144-
</ReactiveFallback>
154+
</Sequence>
145155
</RecoveryNode>
146156
</RateController>
147157
<RecoveryNode number_of_retries="1" name="FollowPath">
148-
<FollowPath path="{path}" controller_id="FollowPath"/>
149-
<ReactiveFallback name="FollowPathRecoveryFallback">
150-
<GoalUpdated/>
158+
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
159+
<Sequence>
160+
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
151161
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
152-
</ReactiveFallback>
162+
</Sequence>
153163
</RecoveryNode>
154164
</PipelineSequence>
155165
@@ -183,7 +193,7 @@ The below is the ``ComputePathToPose`` subtree:
183193
|
184194
185195
The parent ``RecoveryNode`` controls the flow between the action, and the contextual recovery subtree.
186-
The contextual recoveries for both ``ComputePathToPose`` and ``FollowPath`` involve checking if the goal has been updated, and involves clearing the relevant costmap.
196+
The contextual recoveries for both ``ComputePathToPose`` and ``FollowPath`` involve checking if the recovery could help clear the error code and clearing the relevant costmap.
187197

188198
Consider changing the ``number_of_retries`` parameter in the parent ``RecoveryNode`` control node if your application can tolerate more attempts at contextual recoveries before moving on to system-level recoveries.
189199

@@ -202,6 +212,8 @@ The only differences in the BT subtree of ``ComputePathToPose`` and ``FollowPath
202212
- The ``ComputePathToPose`` subtree clears the global costmap. The global costmap is the relevant costmap in the context of the planner
203213
- The ``FollowPath`` subtree clears the local costmap. The local costmap is the relevant costmap in the context of the controller
204214

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.
216+
205217
Recovery Subtree
206218
================
207219

@@ -219,20 +231,30 @@ And the XML snippet:
219231

220232
.. code-block:: xml
221233
222-
<ReactiveFallback name="RecoveryFallback">
223-
<GoalUpdated/>
224-
<RoundRobin name="RecoveryActions">
225-
<Sequence name="ClearingActions">
234+
<Sequence>
235+
<Fallback>
236+
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
237+
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
238+
</Fallback>
239+
<ReactiveFallback name="RecoveryFallback">
240+
<GoalUpdated/>
241+
<RoundRobin name="RecoveryActions">
242+
<Sequence name="ClearingActions">
226243
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
227244
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
228-
</Sequence>
229-
<Spin spin_dist="1.57"/>
230-
<Wait wait_duration="5"/>
231-
<BackUp backup_dist="0.15" backup_speed="0.025"/>
232-
</RoundRobin>
233-
</ReactiveFallback>
245+
</Sequence>
246+
<Spin spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
247+
<Wait wait_duration="5.0" error_code_id="{wait_error_code}" error_msg="{wait_error_msg}"/>
248+
<BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
249+
</RoundRobin>
250+
</ReactiveFallback>
251+
</Sequence>
252+
253+
At the top level, a ``Sequence`` ensures the following steps are executed in order:
254+
255+
- A ``Fallback`` node first checks whether planner or controller recoveries might help resolve the issue. If either returns ``SUCCESS``, the fallback succeeds and the sequence proceeds to the next step.
234256

235-
The top most parent, ``ReactiveFallback`` controls the flow between the rest of the system wide recoveries, and asynchronously checks if a new goal has been received.
257+
- A ``ReactiveFallback`` that controls the flow between the rest of the system wide recoveries, and asynchronously checks if a new goal has been received.
236258
If at any point the goal gets updated, this subtree will halt all children and return ``SUCCESS``. This allows for quick reactions to new goals and preempt currently executing recoveries.
237259
This should look familiar to the contextual recovery portions of the ``Navigation`` subtree. This is a common BT pattern to handle the situation "Unless 'this condition' happens, Do action A".
238260

@@ -263,7 +285,7 @@ For example, let's say the robot is stuck and the ``Navigation`` subtree returns
263285

264286
4. Let's say the next action, ``Wait`` returns ``SUCCESS``. The robot will then move on to the ``Navigation`` subtree
265287

266-
5. Assume the ``Navigation`` subtree returns ``FAILURE`` (clearing the costmaps, attempting a spin, and waiting were *still* not sufficient to recover the system. The robot will move onto the ``Recovery`` subtree and attempt the ``BackUp`` action. Let's say that the robot attempts the ``BackUp`` action and was able to successfully complete the action. The ``BackUp`` action node returns ``SUCCESS`` and so now we move on to the Navigation subtree again.
288+
5. Assume the ``Navigation`` subtree returns ``FAILURE`` (clearing the costmaps, attempting a spin, and waiting were *still* not sufficient to recover the system). The robot will move onto the ``Recovery`` subtree and attempt the ``BackUp`` action. Let's say that the robot attempts the ``BackUp`` action and was able to successfully complete the action. The ``BackUp`` action node returns ``SUCCESS`` and so now we move on to the Navigation subtree again.
267289

268290
6. In this hypothetical scenario, let's assume that the ``BackUp`` action allowed the robot to successfully navigate in the ``Navigation`` subtree, and the robot reaches the goal. In this case, the overall BT will still return ``SUCCESS``.
269291

‎behavior_trees/trees/nav_through_poses_recovery.rst‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -38,10 +38,12 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
3838

3939
.. code-block:: xml
4040
41-
<root main_tree_to_execute="MainTree">
41+
<root BTCPP_format="4" main_tree_to_execute="MainTree">
4242
<BehaviorTree ID="MainTree">
4343
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
4444
<PipelineSequence name="NavigateWithReplanning">
45+
<ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
46+
<GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
4547
<ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
4648
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
4749
<RateController hz="0.333">
@@ -57,7 +59,7 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
5759
</RecoveryNode>
5860
</RateController>
5961
<RecoveryNode number_of_retries="1" name="FollowPath">
60-
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
62+
<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}"/>
6163
<Sequence>
6264
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
6365
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
@@ -76,9 +78,9 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
7678
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
7779
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
7880
</Sequence>
79-
<Spin spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
80-
<Wait wait_duration="5.0"/>
81-
<BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
81+
<Spin name="SpinRecovery" spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
82+
<Wait name="WaitRecovery" wait_duration="5.0" error_code_id="{wait_error_code}" error_msg="{wait_error_msg}"/>
83+
<BackUp name="BackUpRecovery" backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
8284
</RoundRobin>
8385
</ReactiveFallback>
8486
</Sequence>

‎behavior_trees/trees/nav_to_pose_recovery.rst‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
3434

3535
.. code-block:: xml
3636
37-
<root main_tree_to_execute="MainTree">
37+
<root BTCPP_format="4" main_tree_to_execute="MainTree">
3838
<BehaviorTree ID="MainTree">
3939
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
4040
<PipelineSequence name="NavigateWithReplanning">
@@ -70,8 +70,8 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
7070
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
7171
</Sequence>
7272
<Spin spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
73-
<Wait wait_duration="5.0"/>
74-
<BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
73+
<Wait wait_duration="5.0" error_code_id="{wait_error_code}" error_msg="{wait_error_msg}"/>
74+
<BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
7575
</RoundRobin>
7676
</ReactiveFallback>
7777
</Sequence>

0 commit comments

Comments
 (0)