Behavior Tree

A very basic, but functional, navigator can be seen below.

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <PipelineSequence name="NavigateWithReplanning">
      <DistanceController distance="1.0">
        <ComputePathToPose goal="{goal}" path="{path}"/>
      </DistanceController>
      <FollowPath path="{path}"/>
    </PipelineSequence>
  </BehaviorTree>
</root>

This behavior tree will simply plan a new path to goal every 1 meter (set by DistanceController) using ComputePathToPose. If a new path is computed on the path blackboard variable, FollowPath will take this path and follow it using the server’s default algorithm.

Nodes

Action Nodes

  • ComputePathToPose - ComputePathToPose Action Server Client (Planner Interface)

  • FollowPath - FollowPath Action Server Client (Controller Interface)

  • Spin, Wait, Backup - Behaviors Action Server Client

  • ClearCostmapService - ClearCostmapService Server Clients

Upon completion, these action nodes will return SUCCESS if the action server believes the action has been completed correctly, RUNNING when still running, and will return FAILURE otherwise. Note that in the above list, the ClearCostmapService action node is not an action server client, but a service client.

Condition Nodes

  • GoalUpdated - Checks if the goal on the goal topic has been updated

  • GoalReached - Checks if the goal has been reached

  • InitialPoseReceived - Checks to see if a pose on the intial_pose topic has been received

  • isBatteryLow - Checks to see if the battery is low by listening on the battery topic

The above list of condition nodes can be used to probe particular aspects of the system. Typically they will return SUCCESS if the condition is true and FAILURE otherwise. The key condition that is used in the default Nav2 BT is GoalUpdated which is checked asynchronously within particular subtrees. This condition node allows for the behavior described as “If the goal has been updated, then we must replan”. Condition nodes are typically paired with ReactiveFallback nodes.

Decorator Nodes

  • Distance Controller - Will tick children nodes every time the robot has traveled a certain distance

  • Rate Controller - Controls the ticking of its child node at a constant frequency. The tick rate is an exposed port

  • Goal Updater - Will update the goal of children nodes via ports on the BT

  • Single Trigger - Will only tick its child node once, and will return FAILURE for all subsequent ticks

  • Speed Controller - Controls the ticking of its child node at a rate proportional to the robot’s speed

Control: Pipeline Sequence

The PipelineSequence control node re-ticks previous children when a child returns RUNNING. This node is similar to the Sequence node, with the additional property that the children prior to the “current” are re-ticked, (resembling the flow of water in a pipe). If at any point a child returns FAILURE, all children will be halted and the parent node will also return FAILURE. Upon SUCCESS of the last node in the sequence, this node will halt and return SUCCESS.

To explain this further, here is an example BT that uses PipelineSequence.

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <PipelineSequence>
            <Action_A/>
            <Action_B/>
            <Action_C/>
        </PipelineSequence>
    </BehaviorTree>
</root>

  1. Action_A, Action_B, and Action_C are all IDLE.

  2. When the parent PipelineSequence is first ticked, let’s assume Action_A returns RUNNING. The parent node will now return RUNNING and no other nodes are ticked.

  1. Now, let’s assume Action_A returns SUCCESS, Action_B will now get ticked and will return RUNNING. Action_C has not yet been ticked so will return IDLE.

  1. Action_A gets ticked again and returns RUNNING, and Action_B gets re-ticked and returns SUCCESS and therefore the BT goes on to tick Action_C for the first time. Let’s assume Action_C returns RUNNING. The retick-ing of Action_A is what makes PipelineSequence useful.

  1. All actions in the sequence will be re-ticked. Let’s assume Action_A still returns RUNNING, where as Action_B returns SUCCESS again, and Action_C now returns SUCCESS on this tick. The sequence is now complete, and therefore Action_A is halted, even though it was still RUNNING.

Recall that if Action_A, Action_B, or Action_C returned FAILURE at any point of time, the parent would have returned FAILURE and halted any children as well.

Control: Recovery

The Recovery control node has only two children and returns SUCCESS if and only if the first child returns SUCCESS. If the first child returns FAILURE, the second child will be ticked. This loop will continue until either:

  • The first child returns SUCCESS (which results in SUCCESS of the parent node)

  • The second child returns FAILURE (which results in FAILURE of the parent node)

  • The number_of_retries input parameter is violated

This node is usually used to link together an action, and a recovery action as the name suggests. The first action will typically be the “main” behavior, and the second action will be something to be done in case of FAILURE of the main behavior. Often, the ticking of the second child action will promote the chance the first action will succeed.

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <RecoveryNode number_of_retries="1">
            <ComputePathToPose/>
            <ClearLocalCostmap/>
        </RecoveryNode>
    </BehaviorTree>
</root>

In the above example, let’s assume ComputePathToPose fails. ClearLocalCostmap will be ticked in response, and return SUCCESS. Now that we have cleared the costmap, let’s say the robot is correctly able to compute the path and ComputePathToPose now returns SUCCESS. Then, the parent RecoveryNode will also return SUCCESS and the BT will be complete.

Control: Round Robin

The RoundRobin control node ticks its children in a round robin fashion until a child returns SUCCESS, in which the parent node will also return SUCCESS. If all children return FAILURE so will the parent RoundRobin.

Here is an example BT we will use to walk through the concept.

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <RoundRobin>
            <Action_A/>
            <Action_B/>
            <Action_C/>
        </RoundRobin>
    </BehaviorTree>
</root>

  1. All the nodes start at IDLE

  1. Upon tick of the parent node, the first child (Action_A) is ticked. Let’s assume on tick the child returns RUNNING. In this case, no other children are ticked and the parent node returns RUNNING as well.

  1. Upon the next tick, let’s assume that Action_A returns FAILURE. This means that Action_B will get ticked next, and Action_C remains unticked. Let’s assume Action_B returns RUNNING this time. That means the parent RoundRobin node will also return RUNNING.

  1. Upon this next tick, let’s assume that Action_B returns SUCCESS. The parent RoundRobin will now halt all children and return SUCCESS. The parent node retains this state information, and will tick Action_C upon the next tick rather than start from Action_A like Step 2 did.

  1. On this tick, let’s assume Action_C returns RUNNING, and so does the parent RoundRobin. No other nodes are ticked.

  1. On this last tick, let’s assume Action_C returns FAILURE. The parent will circle and tick Action_A again. Action_A returns RUNNING and so will the parent RoundRobin node. This pattern will continue indefinitely unless all children return FAILURE.

Navigate To Pose With Replanning and Recovery

The following section will describe in detail the concept of the main and default BT currently used in Nav2, navigate_to_pose_w_replanning_and_recovery.xml. 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>

These smaller subtrees are the children of the top-most RecoveryNode. From this point forward the NavigateWithReplanning subtree will be referred to as the Navigation subtree, and the RecoveryFallback subtree will be known as the Recovery subtree. This can be represented in the following way:

The Navigation subtree mainly involves actual navigation behavior:

  • calculating a path

  • following a path

  • contextual recovery behaviors for each of the above primary navigation behaviors

Navigation Subtree

Now that we have gone over the control flow between the Navigation subtree and the Recovery subtree, let’s focus on the Navigation subtree.

<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>

This subtree has two primary actions ComputePathToPose and FollowPath. If either of these two actions fail, they will attempt to clear the failure contextually. The crux of the tree can be represented with only one parent and two children nodes like this:

The parent PipelineSequence node allows the ComputePathToPose to be ticked, and once that succeeds, FollowPath to be ticked. While the FollowPath subtree is being ticked, the ComputePathToPose subtree will be ticked as well. This allows for the path to be recomputed as the robot moves around.

Both the ComputePathToPose and the FollowPath follow the same general structure.

  • Do the action

  • If the action fails, try to see if we can contextually recover

The below is the ComputePathToPose subtree:

The parent RecoveryNode controls the flow between the action, and the contextual recovery subtree. The contextual recoveries for both ComputePathToPose and FollowPath involve checking if the goal has been updated, and involves clearing the relevant costmap.

Recovery Subtree

The Recovery subtree is the second big “half” of the Nav2 default navigate_to_pose_w_replanning_and_recovery.xml tree. In short, this subtree is triggered when the Navigation subtree returns FAILURE and controls the recoveries at the system level (in the case the contextual recoveries in the Navigation subtree were not sufficient).

<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>

If the goal is never updated, the behavior tree will go on to the RoundRobin node. These are the default four system-level recoveries in the BT are:

  • A sequence that clears both costmaps (local, and global)

  • Spin action

  • Wait action

  • BackUp action

Upon SUCCESS of any of the four children of the parent RoundRobin, the robot will attempt to renavigate in the Navigation subtree. If this renavigation was not successful, the next child of the RoundRobin will be ticked.

For example, let’s say the robot is stuck and the Navigation subtree returns FAILURE: (for the sake of this example, let’s assume that the goal is never updated).

  1. The Costmap clearing sequence in the Recovery subtree is attempted, and returns SUCCESS. The robot now moves to Navigation subtree again

  2. Let’s assume that clearing both costmaps was not sufficient, and the Navigation subtree returns FAILURE once again. The robot now ticks the Recovery subtree

  3. In the Recovery subtree, the Spin action will be ticked. If this returns SUCCESS, then the robot will return to the main Navigation subtree BUT let’s assume that the Spin action returns FAILURE. In this case, the tree will remain in the Recovery subtree

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

  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.

  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.

PreviousIssac_simNextGalactic to Humble

Last updated