Detailed Behavior Tree Walkthrough

Overview

This document serves as a reference guide to the main behavior tree (BT) used in Nav2.

There are many example behavior trees provided in nav2_bt_navigator/behavior_trees, but these sometimes have to be re-configured based on the application of the robot. The following document will walk through the current main default BT navigate_to_pose_w_replanning_and_recovery.xml in great detail.

Prerequisites

  • Become familiar with the concept of a behavior tree before continuing with this walkthrough

    • Read the short explanation in navigation concepts

    • Read the general tutorial and guide (not Nav2 specific) on the BehaviorTree CPP V4 website. Specifically, the “Basic Concepts” section on the BehaviorTree CPP V4 website explains the basic generic nodes that will be used that this guide will build upon.

  • Become familiar with the custom Nav2 specific BT nodes

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


../../_images/recovery_subtree.png

And the XML snippet:

<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>
            <Spin spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
            <Wait wait_duration="5.0" error_code_id="{wait_error_code}" error_msg="{wait_error_msg}"/>
            <BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
        </RoundRobin>
    </ReactiveFallback>
</Sequence>

At the top level, a Sequence ensures the following steps are executed in order:

  • A Fallback node first checks whether planner or controller recoveries might help resolve the issue. If either returns SUCCESS, the fallback succeeds and the sequence proceeds to the next step.

  • A ReactiveFallback that controls the flow between the rest of the system wide recoveries, and asynchronously checks if a new goal has been received.

If at any point the goal gets updated, this subtree will halt all children and return SUCCESS. This allows for quick reactions to new goals and preempt currently executing recoveries. This should look familiar to the contextual recovery portions of the Navigation subtree. This is a common BT pattern to handle the situation “Unless ‘this condition’ happens, Do action A”.

These condition nodes can be extremely powerful and are typically paired with ReactiveFallback. It can be easy to imagine wrapping this whole navigate_to_pose_w_replanning_and_recovery tree in a ReactiveFallback with a isBatteryLow condition – meaning the navigate_to_pose_w_replanning_and_recovery tree will execute unless the battery becomes low (and then enter a different subtree for docking to recharge).

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.

If the BackUp action was not sufficient enough to allow the robot to become un-stuck, the above logic will go on indefinitely until the number_of_retries in the parent of the Navigate subtree and Recovery subtree is exceeded, or if all the system-wide recoveries in the Recovery subtree return FAILURE (this is unlikely, and likely points to some other system failure).