A mission as a behavior tree

Write the tree

In the Quickstart project folder, create trees/patrol.xml:

<?xml version="1.0"?>
<root BTCPP_format="4">
  <BehaviorTree ID="Patrol">
    <Sequence>
      <WaitForEvent topic_name="start" result="{start}">
        <AlwaysSuccess/>
      </WaitForEvent>
      <Arm service_name="set_arming_state"/>
      <Offboard service_name="set_offboard_mode"/>
      <TakeOff height="1.5" speed="0.5"/>
      <Fallback>
        <Repeat num_cycles="2">
          <Sequence>
            <GoTo pose="2;0;1.5" max_speed="1.0" yaw_mode="1" yaw_angle="0.0"/>
            <GoTo pose="2;2;1.5" max_speed="1.0" yaw_mode="1" yaw_angle="0.0"/>
            <GoTo pose="0;2;1.5" max_speed="1.0" yaw_mode="1" yaw_angle="0.0"/>
            <GoTo pose="0;0;1.5" max_speed="1.0" yaw_mode="1" yaw_angle="0.0"/>
          </Sequence>
        </Repeat>
        <Echo data="Patrol failed, landing"/>
      </Fallback>
      <Land speed="0.5"/>
    </Sequence>
  </BehaviorTree>
</root>

Read it from the top:

1. WaitForEvent holds everything until a message arrives on the drone's start topic.
2. Arm, Offboard and TakeOff get the drone in the air. If any of them fails, the whole sequence fails and nothing else runs.
3. Repeat flies the square twice. A GoTo pose is x;y;z in metres, in earth, and yaw_mode="1" turns the drone to face each point.
4. If a GoTo fails, the Repeat fails, and the Fallback moves on to Echo, which prints a message and succeeds. Either way the Fallback succeeds, so the sequence goes on to Land.

Load it at launch

The project starts a behavior tree with the drone, in the mission_execution window. Its command is in tmuxinator/aerostack2.yaml. Point it at your tree:

  # Mission execution
  - mission_execution:
      layout:
      panes:
        # Behavior tree
        - ros2 launch as2_behavior_tree behavior_trees.launch.py
            drone_id:=<%= drone_namespace %>
            tree:=./trees/patrol.xml

Then start the drone:

./launch_as2.bash

The tree loads and stops at WaitForEvent. Nothing flies yet.

Start the patrol

The project's mission_behavior_tree.py publishes one empty message on /drone0/start. Run it in a second terminal:

python3 mission_behavior_tree.py

The drone takes off, flies the square twice and lands. Press Ctrl-b and the mission_execution window's number to watch the tree. Every node logs its changes of state as it runs:

GoTo                      IDLE -> RUNNING
GoTo                      RUNNING -> SUCCESS
...
Repeat                    RUNNING -> SUCCESS
Fallback                  RUNNING -> SUCCESS
Land                      IDLE -> RUNNING
Land                      RUNNING -> SUCCESS
Sequence                  RUNNING -> SUCCESS

When the tree finishes, the node exits. To fly again, run the launch command again in that pane.

Change the tree

Try a few changes, relaunching the tree after each:

• Set num_cycles="-1" to patrol until something fails.
• Give the Fallback a GoTo back to the origin before the Echo, so a failed patrol returns home before landing.

To see the tree as it runs, start it with groot_logger:=true and connect Groot to it. Groot can also draw and edit the XML.