behavior_tree

ROS2 package for managing the AUV's behavior using BTCPP behavior trees. BTCPP documentation can be found at https://www.behaviortree.dev. The github repository which contains examples and source code can be found here. To visualize the behavior trees checkout Groot and download the latest AppImage under releases.

For a good reference implementation of BTCPP with ROS2 checkout navigation2.

Overview

This package provides abstractions that make building a extensible behavior trees with ROS2 easy. The general development flow for adding new behaviors and functionality to the AUV can be summarized in the following steps:

1. Implement the task or process as a ROS2 service or action server completely independent of the behavior tree.
2. Create a new behavior tree node inheriting from either BtService or BtAction
3. Implement the virtual functions in a C++ plugin and override any other functions to get the desired behavior when the node is called in the tree.
4. Compile the plugin and and add the node to the tree.

BtService and BtAction are both contained entirely in header files so no static linking is required to work with them.

BtEngine

This is the only executable in this repository and is responsible for loading the behavior tree xml and plugins and then executing the behavior tree. BtEngine is run as a ROS node and has the default name bt_engine not to be confused with the additional node that this process will create which is used by BtService and BtAction with the default name bt_node.

Program Flow

• Configure Parameters
• Load the plugins specified by the plugins parameter
• Load tree specified by the bt_file_path parameter and create a second ROS node that lives in the blackboard and is accessible to all nodes in the tree
• Check if run_groot_monitoring is true and if so start groot monitoring on publisher_port connected to server_port
• while rclcpp::ok() tick the tree and sleep to match tick rate with 1/loop_timeout.

Parameters

Note that all plugins specified in the plugins parameter should be pointed to by the LD_LIBRARY_PATH environment variable. This should be handled automatically when sourcing the ROS2 workspace but if issues come up you can check to make sure LD_LIBRARY_PATH is set correctly by running echo $LD_LIBRARY_PATH in the terminal where you are having issues running the behavior tree.

BtService

Provides the framework for a standard ROS2 service client. Implementation is done through overriding the ServiceT::Request::SharedPtr populate_request() and BT::NodeStatus handle_response(ServiceT::Response::SharedPtr response) methods. Adding additional ports can be done by overriding BT::PortsList providedPorts().

Program Flow

On tick

• If node status is idle wait for service with timeout server_timeout
• If node status is idle call service server with message generated by populate_request
• Return Running and wait for the service to complete
• Upon service call completion run handle_response and return the status code returned by handle response

Basic Ports

BtAction

Provides the framework for a standard ROS2 action client. Implementation is done through overriding ActionT::Goal populate_goal(). Adding additional ports can be done by overriding BT::PortsList providedPorts(). Canceling actions is done by halting the node which can be accomplished by using something like a reactive sequence with an action or condition node preceding the BtAction node which, upon returning FAILURE, will cause the reactive sequence to halt the BtAction node which will subsequently cancel the action over ROS2.

Program Flow

On tick

• If node status is idle or a current goal is in progress as determined in goal_in_progress based on the status of the _goal_handle then send a goal, return RUNNING.
• Else spin the node and check to see if the goal status has changed, return the value returned by process_goal_status

send_goal

• Set the done flag to false
• wait for action server
• Get the goal from the populate goal method
• Send goal and wait for acceptance
• return status running if goal is accepted

process_goal_status

• If done flag is true then process _result.code and return corresponding BT::NodeStatu
• Else return running

Basic Ports

Creating a custom ROS2 action/service client node

Action Service Example

Action Client Example

1. Create a new .cpp file for the desired node, typically this will be in plugins/action within a ROS2 package
2. The cpp file should contain a class that inherits public BtAction<ActionType> or public BtService<ServiceType>, we will override the necessary methods to generate a fully implemented behavior tree node.
3. For action client override the ActionType::Goal populate_goal() method, for a service client override ServiceType::Request::SharedPtr populate_request() and BT::NodeStatus handle_response(ServiceType::Response::SharedPtr response).
4. (Optional) Override the static PortsList providedPorts() method to add additional ports, the code should look similar to the below example code segment

   static PortsList providedPorts() {
    return providedBasicPorts({InputPort<geometry_msgs::msg::Vector3>("distances")});
   }

5. Add the following code segment, changing the Type and ClientNodeName based on your setup. The name that you set here is the name that will be used in the tree.xml.

   BT_REGISTER_NODES(factory) {
    factory.registerNodeType<Type>("ClientNodeName");
   }

6. See the Building Nodes section

Building Nodes

Add package dependencies to the CMakeLists.txt and add the following snippet adapted to your behavior tree node

add_library(name_bt_node SHARED plugins/action/name.cpp)
list(APPEND plugin_libs name_bt_node)

The CMakeLists.txt should include the following supporting code

set(dependencies
        rclcpp
        ...
        )

<<<List of plugin specific segments resembling the above code snippet>>>

foreach(bt_plugin ${plugin_libs})
  ament_target_dependencies(${bt_plugin} ${dependencies})
  target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
endforeach()

install(TARGETS ${plugin_libs}
        ARCHIVE DESTINATION lib
        LIBRARY DESTINATION lib
        RUNTIME DESTINATION bin
        )