Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Created path segmenter controller

Browse Source
This commit is contained in:
pimpest
2026-01-11 22:57:26 +01:00
committed by Pimpest
parent 3a6907a1fe
commit eca8d2532f
19 changed files with 4411 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
toid_navigation/behaviors/navigate_to_pose_w_backtracking.xml Normal file
View File

@@ -0,0 +1,51 @@
<!--
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>