Update on my learning experiences with behavior trees

[Michael Wimble]

Ignore if you aren’t interested in the use of behavior trees in ROS.

I been reading a lot, but then you have to read a lot if you actually want to use this library in ROS 2. The documentation is reasonable except for “all that glue” stuff, which we don’t like to talk about. Even one, small, real-world example would have really, really helped.

It seems like this will be a useful tool for creating driving behavior for the “fetch me a beer” scenario, with hooks for handling all the failure scenarios I can think up. Doing this in linear code would be, uhm, “taxing”, and hard to keep track of visually.

I’ve been able to create a basic behavior tree that corresponds to the following XML

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Repeat name="do_alot" num_cycles="1000">
      <ReactiveSequence name="level_1">
        <GetHallwayWidthDistances name="get_hallway_width_distances" />
        <FindBackWall name="find_back_wall" />
        <ReactiveSequence name="goto_front_wall">
          <GetHallwayWidthDistances name="get_hallway_width_distances_w/in_goto_front_wall" />
          <MoveForwardsCentered name="move_forwards_centered" />
          <FindFrontWall name="find_front_wall" />
          <FrontWallIsCloseEnough name="front_wall_is_close_enough" />
        </ReactiveSequence>
        <ReactiveSequence name="goto_back_wall">
          <GetHallwayWidthDistances name="get_hallway_width_distances_w/in_goto_back_wall" />
          <MoveBackwardsCentered name="move_backwards_centered" />
          <FindBackWall name="find_back_wall" />
          <BackWallIsCloseEnough name="back_wall_is_close_enough" />
        </ReactiveSequence>
      </ReactiveSequence>
    </Repeat>
  </BehaviorTree>
</root>

I say “corresponds to” because, while the XML is used directly in the framework, for the tree to be used, you must then code up all the non built-in node kinds. For instance, I have c++ files for GetHallwayWidthDistances, FindBackWall, etc. The nodes Repeat and ReactiveSequence are built-in, and the outer XML is part of the scaffolding to contain the tree.

In case that XML doesn’t mean anything to you, it basically says,

• Begin by finding the distances to the left and right wall. If that fails, stop the whole process. This is a precondition.
• Then find the distance to the back wall. If that fails, stop the whole process as this is also a precondition.
• Now repeat these steps to go to the front wall. If these steps fail, stop as something is very wrong.
• Read the wall distances
• Move forwards a bit, centered between the walls.
• Read the front wall distance.
• See if we are “close enough” to the front wall.
• When the previous sequence success and we are at the front wall, do the following sequence repeatedly until we return to the back wall.
• Read the wall distances.
• Move backwards a bit, centered between the walls.
• Read the back wall distance.
• See if we are “close enough” to the back wall.

There are some obvious improvements to be made to this, which I’ll leave you to discover if you want. But this will get me started with Puck. Now I need to replace my placeholder code with code that actually reads sensors and moves the robot.

My nodes currently are just placeholders. E.g.: find_back_wall.cpp

#include "find_back_wall.h"
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"

BT::NodeStatus FindBackWall() {
  std::cout << "[ FindBackWall: OK ]" << std::endl;
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  return BT::NodeStatus::SUCCESS;
}

And then I needed to create the main node which loads the XML, registers my custom nodes and adds in fun loggers to print the tree, print the tree transitions to the console and interact with the fun but frustrating Groot visualizer.

#include <cstdio>

#include <rclcpp/rclcpp.hpp>

#include "back_wall_is_close_enough.h"

#include "behaviortree_cpp_v3/bt_factory.h"

#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"

#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"

#include "find_back_wall.h"

#include "find_front_wall.h"

#include "front_wall_is_close_enough.h"

#include "get_hallway_width_distances.h"

#include "goto_back_wall.h"

#include "move_backwards_centered.h"

#include "move_forwards_centered.h"

rclcpp::Node::SharedPtr g_node;

inline void SleepMS(int ms)

{

    std::this_thread::sleep_for(std::chrono::milliseconds(ms));

}

int main(int argc, char** argv) {

    rclcpp::init(argc, argv);

    g_node = rclcpp::Node::make_shared("floorbot_1_node");

  std::string xml_path;

  g_node->declare_parameter<std::string>("xml_path", "foo");

  g_node->get_parameter("xml_path", xml_path);

  BT::BehaviorTreeFactory factory;

  factory.registerSimpleCondition("BackWallIsCloseEnough", std::bind(BackWallIsCloseEnough));

  factory.registerSimpleCondition("FindBackWall", std::bind(FindBackWall));

  factory.registerSimpleCondition("FindFrontWall", std::bind(FindFrontWall));

  factory.registerSimpleCondition("FrontWallIsCloseEnough", std::bind(FrontWallIsCloseEnough));

  factory.registerSimpleCondition("GetHallwayWidthDistances", std::bind(GetHallwayWidthDistances));

  factory.registerNodeType<GotoBackWall>("GotoBackWall");

  factory.registerNodeType<MoveBackwardsCentered>("MoveBackwardsCentered");

  factory.registerNodeType<MoveForwardsCentered>("MoveForwardsCentered");

  auto tree = factory.createTreeFromFile(xml_path);

  BT::PublisherZMQ publisher_zmq(tree);

  BT::StdCoutLogger logger_cout(tree);

  BT::printTreeRecursively(tree.rootNode());

  {

    BT::NodeStatus status = BT::NodeStatus::RUNNING;

    while (status == BT::NodeStatus::RUNNING) {

      status = tree.tickRoot();

      SleepMS(5000);

    }

    SleepMS(2000);

  }

  printf("Floorbot Level 1 Challenge completed\n");

  return 0;

}

With this, I was able to launch the code and visualize the tree with Groot as the various nodes int he tree because active. When this actually does something useful, I’ll be putting a more detailed article in my blog.