EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

behavior trees are added

This commit is contained in:
Rooholla-KhorramBakht 2024-07-28 13:16:20 -04:00
parent 7e1f4501c5
commit e625b7090a
3 changed files with 107 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
deploy/ros2_nodes/sportmode_nav2/params/nav2_params.yaml
View File

@ -50,6 +50,8 @@ bt_navigator:
wait_for_service_timeout: 1000
action_server_result_timeout: 900.0
navigators: ["navigate_to_pose", "navigate_through_poses"]
default_nav_to_pose_bt_xml: "/home/nav2_ws/src/sportmode_nav2/params/navigate_to_pose_w_replanning_and_recovery.xml"
default_nav_through_poses_bt_xml: "/home/nav2_ws/src/sportmode_nav2/params/navigate_through_poses_w_replanning_and_recovery.xml"
navigate_to_pose:
plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
navigate_through_poses:
@ -104,10 +106,10 @@ controller_server:
vx_std: 0.5
vy_std: 0.2
wz_std: 0.4
vx_max: 1.5
vx_max: 1.0
vx_min: -0.8
vy_max: 1.5
wz_max: 3.9
vy_max: 1.0
wz_max: 2.0
ax_max: 1004.0
ax_min: -1004.0
ay_max: 1004.0
@ -118,7 +120,7 @@ controller_server:
temperature: 0.3
gamma: 0.015
motion_model: "DiffDrive"
visualize: true
visualize: false
# reset_period: 1.0 # (only in Humble)
regenerate_noises: false
TrajectoryVisualizer:
@ -298,7 +300,7 @@ planner_server:
ros__parameters:
use_sim_time: False
planner_plugins: ["GridBased"]
expected_planner_frequency: 1.0
GridBased:
plugin: "nav2_smac_planner::SmacPlannerHybrid"
tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters

51
deploy/ros2_nodes/sportmode_nav2/params/navigate_through_poses_w_replanning_and_recovery.xml Normal file
View File

@ -0,0 +1,51 @@
<!--
This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
and it also has recovery actions specific to planning / control as well as general system issues.
-->
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="4" 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="0.333">
<RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
<ReactiveSequence>
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
</ReactiveSequence>
<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>
<Wait wait_duration="2.0"/>
<BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}"/>
</RoundRobin>
</ReactiveFallback>
</Sequence>
</RecoveryNode>
</BehaviorTree>
</root>

49
deploy/ros2_nodes/sportmode_nav2/params/navigate_to_pose_w_replanning_and_recovery.xml Normal file
View File

@ -0,0 +1,49 @@
<!--
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="4" 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>
<Wait wait_duration="5.0"/>
<BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_code_id}"/>
</RoundRobin>
</ReactiveFallback>
</Sequence>
</RecoveryNode>
</BehaviorTree>
</root>