Behavior Tree XML Nodes
The nav2_behavior_tree package provides several navigation-specific nodes that are pre-registered and can be included in Behavior Trees.
Check this introduction to learn how behavior trees work and the difference between actions, conditions, controls and decorators.
Consider checking out the Groot - Interacting with Behavior Trees tutorial for using Groot to visualize and modify behavior trees.
Action Plugins
- Wait
- Spin
- BackUp
- DriveOnHeading
- AssistedTeleop
- ComputePathToPose
- FollowPath
- NavigateToPose
- ClearEntireCostmap
- ClearCostmapExceptRegion
- ClearCostmapAroundRobot
- ReinitializeGlobalLocalization
- TruncatePath
- TruncatePathLocal
- PlannerSelector
- ControllerSelector
- SmootherSelector
- GoalCheckerSelector
- ProgressCheckerSelector
- NavigateThroughPoses
- ComputePathThroughPoses
- ComputeCoveragePath
- CancelCoverage
- RemovePassedGoals
- RemoveInCollisionGoals
- CancelControl
- CancelBackUp
- CancelSpin
- CancelWait
- CancelDriveOnHeading
- CancelAssistedTeleop
- SmoothPath
- GetPoseFromPath
- DockRobot
- UndockRobot
Condition Plugins
Control Plugins
Decorator Plugins
Example
This Behavior Tree replans the global path periodically at 1 Hz and it also has recovery actions.
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
<ReactiveFallback name="ComputePathToPoseRecoveryFallback">
<GoalUpdated/>
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
</ReactiveFallback>
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="FollowPath"/>
<ReactiveFallback name="FollowPathRecoveryFallback">
<GoalUpdated/>
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
</ReactiveFallback>
</RecoveryNode>
</PipelineSequence>
<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>
<Spin spin_dist="1.57"/>
<Wait wait_duration="5"/>
<BackUp backup_dist="0.15" backup_speed="0.025"/>
</RoundRobin>
</ReactiveFallback>
</RecoveryNode>
</BehaviorTree>
</root>