Building a Behaviour Tree with Groot

We can use Groot to help build our behaviour tree, rather than write out the xml by hand. Run Groot, include the Nav2 node palette, and we are ready to begin. If you already have Groot open viewing a tree, go to File->Clear to start fresh.

Mobile robot navigation consists of two main steps, path planning followed by navigation (i.e. following that path). The simplest behaviour we can build is there for a ComputePathToPose action node followed by a FollowPath action node, mediated by a Sequence control node.

Place the nodes onto the GUI, and connect the nodes by grabbing the lower dot on the parent node and dragging (with the mouse) a connection to the leaf node (see the picture below).

Once you have placed the nodes, it should look something like the image below. You can click on the various fields to populate them.

In the sequence, once the ComputePathToPose returns SUCCESS it moves onto the FollowPath Node and the robot starts tracking its trajectory. A path is generated, then it is followed.

Let us unpick the different parts of the ComputePathToPose and FollowPath action nodes.

ComputePathToPose Action Node

The ComputePathToPose action node has multiple inputs and one output. This will interface with the planner_server (with NavFn as a plugin) we have setup in previous tutorials.

• [IN] goal: The goal pose provided by the user or an autonomous agent, the curly braces {goal} indicates that this varible is on the Blackboard for the behaviour tree

• [IN] planner_id: In the config file for the planner (planner.yaml), we state the plugin is called GridBased, this term is a ROS2/Nav2 convention and should not be changed

• [IN] server_name: The planner server, when running, will have an action server to perform the path planning, in this case the action server is called compute_path_to_pose - this will be discussed later

• [IN] server_timeout: How long the behaviour tree node should wait for a response from a ROS node (Nav2 uses 20 ms by default)

• [IN] start: It is possible to use a different starting pose for path planning (rather than the current position of the robot)

• [OUT] path: This is the path which will be used by the navigation algorithm to generate trajectories

FollowPath Action Node

The FollowPath action node accepts the path from the path planner, then passes this to the controller plugin to handle all the trajectory generation and tracking (e.g. using DWB).

• [IN] controller_id: In the config file for the controller (controller.yaml), we state the plugin is called FollowPath, this term is a ROS2/Nav2 convention and should not be changed

• [IN] goal_checker_id: In the config file for the controller (controller.yaml), we state the plugin for goal checking is called goal_checker, therefore, we use this - this will be discussed later

• [IN] path: This is the path variable on the Blackboard, hence the curly braces {path}, generated by the path planning action server

• [IN] server_name: The controller server, when running, will have an action server to perform the trajectory control, in this case the action server is called follow_path - this will be discussed later

• [IN] server_timeout: How long the behaviour tree node should wait for a response from a ROS node (Nav2 uses 20 ms by default)

Saving the Behaviour Tree

Once the tree has been generated, it is possible to save it as a .xml file in your bt_demos/behavior_tree_xml directory. Save the file as bt_simple_nav.xml.

You will notice that the .xml file looks different to most of the example behaviour tree files you will see. This is because Groot uses “Explicit” notation rather than “Compact” notation, and as a ROS user it is somewhat of a pain…

Explicit Vs Compact Notation

The BehaviorTree.CPP documentation covers the difference between Explicit and Compact notation. The short version is that the .xml file can be written in two ways, either explicitly stating the node and its ID (Action and FollowPath), or using the node ID only (e.g. FollowPath).

The compact notation references existing known nodes, the bonus is any default values are automatically used. In the explicit form, all fields must be completed, or manually removed from the .xml file.

An example is the ComputePathToPose action node. If using the explicit notation, then the server_name must be stated or you must manually delete this portion from the .xml file. If using compact notation, Nav2/ROS is clever enough to know you mean the compute_path_to_pose action server. This is the same for the FollowPath goal_checker_id and server_name. There are pros and cons to both, and likely you will only stumble on a preference for your use case.

Furthermore, you will notice that Groot adds information about nodes from the palette to the file in the <TreeNodesModel> tag, which is a bit cumbersome. You can delete this portion and it will still work with ROS. Groot will also be able to open it, but will need the Nav2 palette open. If you leave this information in, you no longer need to open the palette every time.

Note

My personal preference would be for Groot to offer a compact notation option. Groot2 may solve some of this.

The files below show the two same trees, but using the compact and explicit notation. The explicit version has been clipped to remove most of the <TreeNodesModel> Actions.

<root main_tree_to_execute="BehaviorTree">
    <BehaviorTree ID="BehaviorTree">
        <Sequence name="Navigate">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            <FollowPath controller_id="FollowPath" path="{path}"/>
        </Sequence>
    </BehaviorTree>
</root>

<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
    <!-- ////////// -->
    <BehaviorTree ID="BehaviorTree">
        <Sequence name="Navigate">
            <Action ID="ComputePathToPose" goal="{goal}" path="{path}" planner_id="GridBased" server_name="compute_path_to_pose" server_timeout="" start=""/>
            <Action ID="FollowPath" controller_id="FollowPath" goal_checker_id="goal_checker" path="{path}" server_name="follow_path" server_timeout=""/>
        </Sequence>
    </BehaviorTree>
    <!-- ////////// -->
    <TreeNodesModel>
        <Action ID="AssistedTeleop">
            <input_port name="is_recovery">If true recovery count will be incremented</input_port>
            <input_port name="server_name">Service name</input_port>
            <input_port name="server_timeout">Server timeout</input_port>
            <input_port name="time_allowance">Allowed time for spinning</input_port>
        </Action>
 ...
        <Action ID="Wait">
            <input_port name="server_name">Server name</input_port>
            <input_port name="server_timeout">Server timeout</input_port>
            <input_port name="wait_duration">Wait time</input_port>
        </Action>
    </TreeNodesModel>
    <!-- ////////// -->
</root>