Behaviortree in ros2

BehaviorTree.CPP It's an open source C++ Behavior Tree Library . In the field of games , Behavior trees are already popular . It is mainly used to maintain various actions and states of game characters . But it is rarely used in the field of robotics .Navigation2 The behavior tree is introduced to organize the workflow and action execution of the robot .

The behavior tree is a tree structure , Its logic flow is composed of xml The document describes . We can use its matching tools Groot To visualize the behavior tree . As shown in the figure below ：

The behavior tree itself does not specifically implement the execution content of the robot , It is only responsible for arranging the execution content . With Navigation2 For example , The specific implementation content is placed in each server Medium . The nodes on the behavior tree are related to server communicate , Request specific execution contents , Then get feedback . According to the feedback result, another execution content can be requested . The jump between these different execution contents is controlled by the behavior tree .

Comparison between behavior tree and state machine

Another common way to organize robot behavior is the state machine .ROS1 Medium move_base Is based on the state machine . The most significant difference between it and the behavior tree is that the state and execution content are bound together . When the execution content needs to be executed in multiple states , In each state, you need to place the logic of the execution content . When the business logic code is scattered everywhere, it is not easy to maintain , Especially for complex robot systems .

A good software has the following characteristics （ quote Official documents A passage from ）：

A “good” software architecture should have the following characteristics:

• Modularity.

• Reusability of componens.

• Composability.

• Good separation of concerns.

Main advantages :

• They are intrinsically Hierarchical
• Their graphical representation has a semantic meaning
• They are more expressive

Behavior Tree VS FSM(Finite State Machines)：

The business logic becomes “spread” in many locations and it is hard for the developer to reason about it and to debug errors in the control flow.
Business logic becomes “ Dispersed “ In many places , It is difficult for developers to reason about it and debug errors in the control flow .

To achieve strong separation of concerns it is better to centralize the business logic in a single location.
To achieve strong separation of concerns , It is better to put the business logic in one place .

Behavior tree first experience

Here is the official demo video , You can feel it .

Compilation and installation Groot

Compilation and installation behaviortree-cpp-v3

sudo apt-get install libzmq3-dev libboost-dev
git clone https://ghproxy.com/https://github.com/BehaviorTree/BehaviorTree.CPP
cd BehaviorTree.CPP
mkdir build
cd build
cmake ..
make -j8
sudo make install

Installation dependency

sudo apt install qtbase5-dev libqt5svg5-dev libzmq3-dev libdw-dev

compile Groot

   git clone https://ghproxy.com/https://github.com/BehaviorTree/Groot.git
   cd Groot
   git submodule update --init --recursive
   mkdir build
   cd build
   cmake ..
   make

install Groot

sudo vim /etc/ld.so.conf  # Add dynamic library link address 
 add to  /usr/local/lib/  To /etc/ld.so.conf In the document 
sudo ldconfig

sudo mv /usr/local/lib/groot/Groot /usr/local/bin/

 And then you can   use Groot Command to open  Groot

The library contains some sample programs . After compiling according to the above steps , You can go to build/examples See the execution file of the sample program in .

You can run one to see . By default, you have entered build/examples Directory .

./t05_cross_door loop

Groot

About the concept of behavior tree , I suggest you look at the official documents ：

BehaviorTree.CPP

https://github.com/BehaviorTree/BehaviorTree.CPP

BehaviorTree Official documents ：

https://www.behaviortree.dev/bt_basics/

Use Groot

Use Groot Edit the behavior tree

1. open Groot

Groot

1. Load custom behavior tree nodes .

The following figure shows the loading of Navigation2 Custom leaf nodes in , That is, the blue part , The black one is BehaviorTree.CPP Kuri brought it with him . The loaded file is nav2_behavior_tree/nav2_tree_nodes.xml.

1. Load a behavior tree

The above loads some available nodes . When we combine these available nodes to form a tree , It can realize various functions .

The picture below is Navigation2 in , A behavior tree for single point navigation .

stay Navigation2 in , Describe the behavior tree xml The documents are stored in nav2_bt_navigator/behavior_trees Under the table of contents .

You can drag the node you want from the left to the right , Then modify the parameters of the node , Then connect it to the tree . After the modification is completed, it can be saved Navigation2 Used .

Use Groot Monitor the behavior tree in real time

1. open Groot Back check Monitor.

1. When the program runs , Click on the left connect Button connection to display the currently running behavior tree .

It should be noted that , If the behavior tree status of the machine is viewed remotely , It's in Server IP Filled with the machine IP Address .

Behavior tree log Save and playback of

1. Save the behavior tree log

The behavior Tree Library has the following 4 Kind of log Interface . Where is saved as *.fbl Format file log Yes, it can be. Groot` Loaded and played back .

//4 Kind of log Storage publishing method 

    // Important: when the object tree goes out of scope, all the TreeNodes are destroyed
    auto tree = factory.createTreeFromText(xml_text);

    // This logger prints state changes on console
    StdCoutLogger logger_cout(tree);

    // This logger saves state changes on file
    FileLogger logger_file(tree, "bt_trace.fbl");// This file can be loaded into Groot And play back 

    // This logger stores the execution time of each node
    MinitraceLogger logger_minitrace(tree, "bt_trace.json");

#ifdef ZMQ_FOUND
    // This logger publish status changes using ZeroMQ. Used by Groot
    PublisherZMQ publisher_zmq(tree); // Send to in real time Groot Show 
#endif

1. Play back the behavior tree log

open Groot, Choose Log Replay.

Click on the left Load Log load fbl Format file to see log.

Use BT Precautions for

1. When customizing a leaf node , You need to inherit to the appropriate base class . stay Navigation2 in , Yes BT The leaf node type of .

The synchronization node encapsulates the service client , In this way, the corresponding node can act through the service request .

class BtServiceNode : public BT::SyncActionNode

The asynchronous node encapsulates the action client , In this way, the corresponding server can execute business logic through action requests .

class BtActionNode : public BT::ActionNodeBase

1. Data flow in the behavior tree

The common data in the behavior tree is stored in Blackboard Medium .Blackboard It stores data in the form of key value pairs . Leaf nodes of each behavior tree can pass through key Access to the required data .

Ports Is used to exchange data between nodes . A behavior tree leaf node can define the input Ports And the output of Ports. When different leaf nodes Port Have the same key When the name , It can be considered that they are interlinked .

When these are declared in the leaf node of the behavior tree Ports when , Also need to be in xml These are described in the document Ports.

The official interpretation of data flow is also included below .

For the time being, it is important to know that:

• A Blackboard is a key/value storage shared by all the Nodes of a Tree.
• Ports are a mechanism that Nodes can use to exchange information between each other.
• Ports are “connected” using the same key of the blackboard.
• The number, name and kind of ports of a Node must be known at compilation-time (C++); connections between ports are done at deployment-time (XML).

Let's explain Blackboard How it is used .

action server Set the blackboard Value

blackboard->set<geometry_msgs::msg::PoseStamped>("goal", goal->pose);

// Put items on the blackboard
blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_);  // NOLINT
blackboard_->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);  // NOLINT
blackboard_->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);  // NOLINT

In the leaf node, you can set ports To set up blackboard The value in

static BT::PortsList providedPorts()
  {
    return providedBasicPorts(
      {
        BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
        BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
        BT::InputPort<geometry_msgs::msg::PoseStamped>(
          "start", "Start pose of the path if overriding current robot pose"),
        BT::InputPort<std::string>("planner_id", ""),
      });
  }

A leaf node needs to use a port You need to be in providedPorts It is declared here , Equivalent to the list Join in first .

Be careful ：portslist Not found in key It doesn't work getInput or etOutput To operate the .

obtain port Value

getInput("goal", goal_.goal);
getInput("planner_id", goal_.planner_id);
if (getInput("start", goal_.start)) {
    goal_.use_start = true;
}

Set up port Value

setOutput("path", result_.result->path);

stay xml Declaration in the document ports

The file path is nav2_behavior_tree/nav2_tree_nodes.xml.

    <Action ID="ComputePathToPose">
      <input_port name="goal">Destination to plan to</input_port>
      <input_port name="start">Start pose of the path if overriding current robot pose</input_port>
      <output_port name="path">Path created by ComputePathToPose node</output_port>
      <input_port name="planner_id"/>
    </Action>

1. Every time tickRoot() Function execution will traverse the entire tree , about asynActionNode, Because it has a cycle （ In a separate thread ）, So when the loop does not end, it will return RUNNING state . Different control flow node pairs RUNNING It's not the same . For this point, you can see the description of the control flow node in the official document .

2. Navigation2 Of libbehaviortree_cpp_v3 The dependent libraries are installed in by default /opt/ros/galactic/lib/libbehaviortree_cpp_v3.so . If you want to use the latest behavior Tree Library , You can pull the latest code by yourself , Replace it after compilation . Pay attention to the interface changes of the new version ！ To avoid incompatibility .

3. BT Does not execute specific function code , It just organizes the business logic code . Like Lego blocks, each business logic code block can be organized according to different ideas to achieve different functions . Different functions can be realized through different organization methods and a small amount of code modification .

stay ROS2 Use in Behavior Tree

stay nav2_behavior_tree Many plug-ins are maintained in . These plug-ins are divided into 4 Categories ：action,condition,control and decorator.

action

Action nodes usually implement service clients and action clients , It can also be some simple execution programs . They pass on to Planner server,Controller server,Recovery server Send a request to start the corresponding function program .action Usually used as a leaf node in the behavior tree , Responsible for the implementation of specific behaviors and functions . But these specific function codes are not in the leaf node, but in the corresponding server .

condition

This is the conditional control node . For example, judge the battery level , A certain switch signal, etc .

control

This is the control flow in the behavior tree . similar c++ In language if else,switch wait . It is responsible for building the logical structure of the behavior tree .sequeence,fallback And so on belong to this category .

decorator

decorator Is a node decorator . It can only have one child node . Be responsible for decorating the results of child nodes . For example, reverse the results of child nodes , Constraining the execution times of child nodes, etc .

When we have implemented enough full-featured server programs , You can write the corresponding behavior tree plug-in . Through these plug-ins, we arrange and combine various functions or program execution blocks , Form a complete function . The logical modification may only need to modify the description file of the behavior tree without changing the source code . Product construction based on behavior tree , Maybe more people can participate in product development, not just R & D personnel .

BehaviorTreeEngine The plug-in set in the parameter file will be loaded . and nav2_bt_navigator The behavior tree description file of the navigation system is maintained in .

Here is turtlebot3 The configuration file fragment of can be used as a reference . The file in turtlebot3/turtlebot3_navigation2/param Directory .

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    bt_loop_duration: 10
    default_server_timeout: 20
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node
    - nav2_single_trigger_bt_node
    - nav2_is_battery_low_condition_bt_node
    - nav2_navigate_through_poses_action_bt_node
    - nav2_navigate_to_pose_action_bt_node
    - nav2_remove_passed_goals_action_bt_node
    - nav2_planner_selector_bt_node
    - nav2_controller_selector_bt_node
    - nav2_goal_checker_selector_bt_node

Robot development and use BT The advantages of

Here is a summary of the use of BT Advantages ！

1. Debug is convenient , Changes in machine behavior can be traced back to . Changes in machine behavior can be recorded and played back . Behavior changes can also be monitored in real time .
2. High code reuse rate . Different functions require only a small amount of code modification and reorganization of machine behavior .

BehaviorTree Related materials

Behavior trees for AI: How they work

navigation2

• navigation2 Medium nav2_behavior_tree The module is right BehaviorTree.CPP Encapsulation of Libraries
• nav2_bt_navigator The module is used to load the behavior tree file and start

Groot Is a visual behavior Tree Editor and debugger