51 lines
2.8 KiB
XML
51 lines
2.8 KiB
XML
|
|
<!--
|
|
This Behavior Tree replans the global path periodically at 1 Hz and 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.
|
|
-->
|
|
<root BTCPP_format="4" main_tree_to_execute="MainTree">
|
|
<BehaviorTree ID="MainTree">
|
|
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
|
<PipelineSequence name="NavigateWithReplanning">
|
|
<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}"/>
|
|
<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}"/>
|
|
<Sequence>
|
|
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
|
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
|
</Sequence>
|
|
</RecoveryNode>
|
|
</PipelineSequence>
|
|
<Sequence>
|
|
<Fallback>
|
|
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
|
|
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
|
|
</Fallback>
|
|
<ReactiveFallback name="RecoveryFallback">
|
|
<GoalUpdated/>
|
|
<RoundRobin name="RecoveryActions">
|
|
<Sequence name="ClearingActions">
|
|
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
|
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
|
</Sequence>
|
|
<BackUp backup_dist="0.04" backup_speed="0.05" error_code_id="{backup_code_id}"/>
|
|
<BackUp backup_dist="-0.08" backup_speed="0.05" error_code_id="{backup_code_id}"/>
|
|
<BackUp backup_dist="0.012" backup_speed="0.05" error_code_id="{backup_code_id}"/>
|
|
<BackUp backup_dist="-0.16" backup_speed="0.05" error_code_id="{backup_code_id}"/>
|
|
</RoundRobin>
|
|
</ReactiveFallback>
|
|
</Sequence>
|
|
</RecoveryNode>
|
|
</BehaviorTree>
|
|
</root> |