Behavior Trees

Objectives in MoveIt Pro are represented as Behavior Trees that perform a specific function, such as picking an object or opening a door. In MoveIt Pro, Behaviors are the building blocks used to create Objectives. They define a discrete sensing, planning, motion execution, or decision-making step.

The MoveIt Pro SDK is built around the BehaviorTree.CPP library's behavior tree implementation.

If you are not already familiar with BehaviorTree.CPP, we strongly recommend that you review its introductory documentation before continuing.

To see the list of Behaviors included in MoveIt Pro, refer to the Technical Documentation.

Creating Custom Behaviors

Overview

MoveIt Pro Behaviors are C++ classes that derive from BehaviorTree.CPP BT::TreeNode classes. There are four main types of nodes in BehaviorTree.CPP that inherit from this base class:

• BT::ActionNode: The most common type of node that performs a task. Can return SUCCESS, FAILURE, or RUNNING as a status.
• BT::ConditionNode: A simplified case of action nodes that performs a simple check, and as such cannot return RUNNING.
• BT::ControlNode: Controls the flow of execution based on child or sibling nodes.
• BT::DecoratorNode: Modifies the result of its child node.

In practice, most of your Behaviors will be implemented as some type of BT::ActionNode or BT::ConditionNode. For most applications, the standard set of control nodes (Sequence, Fallback, Parallel, etc.) and decorators (Inverter, Repeat, ForceSuccess, etc.) will be sufficient. Refer to the Nodes Library documentation for more details.

Writing a New Behavior in C++

As a developer, you can create custom Behaviors within your own packages and have the Objective Server load them at runtime from plugins.

To do this, you must complete three main steps:

1. Create a new C++ class defining your custom Behavior.
2. Create a Behavior loader plugin to register your custom Behavior with the Objective Server's Behavior tree factory.
3. Add the name of the plugin library to your robot configuration package's YAML file to allow that type of robot to load those Behaviors when running Objectives.

If you do not need ROS

Behaviors that do not need to interact with ROS can derive from one of the BehaviorTree.CPP action node types, such as BT::SyncActionNode or BT::StatefulActionNode.

If you do need ROS

Behaviors that need to interact with ROS need to derive from the SharedResourcesNode class provided in the moveit_studio_behavior_interface package. This class adds an additional constructor parameter that allows pointing it to an instance of a ROS node (along with some additional shared resources) when it is created.

The SharedResourcesNode class is templated, can take any type based on BT::TreeNode as the template parameter. The type provided as the template parameter will be used as the base class for SharedResourcesNode -- for example, SharedResourcesNode<BT::SyncActionNode> or SharedResourcesNode<BT::StatefulActionNode>.

Additionally, we provide a set of helpful Behavior base classes you can use to interact with ROS interfaces:

• GetMessageFromTopicBehaviorBase: Gets the latest message from a ROS topic and sets its value to an output data port.
• ServiceClientBehaviorBase: Sends a request to a ROS service server and waits for a result.
• ActionClientBehaviorBase: Sends an goal to a ROS action server and waits for a result.

For more information on the custom Behavior base classes provided by MoveIt Pro, refer to the Technical Documentation for the MoveIt Pro Behavior Interface.

Implementing the Behavior

Your Behavior class needs to implement the virtual functions in the BehaviorTree.CPP node class it is derived from. Refer to the BehaviorTree.CPP documentation to see what you specifically need to do (for example, read here for BT::StatefulActionNode).

Important Behavior Design Guidelines

Here are a few key things to keep in mind when creating your own Behaviors:

Behaviors must not block while ticking

When the Behavior tree is ticked, it is important that the state of each Behavior can be evaluated quickly. This happens in the Behavior's tick() function. It is important that tick() can finish very quickly, so that Behaviors derived from BT::SyncActionNode do not perform long-running processes within the Behavior's implementation of tick().

As a rule of thumb, a process that runs longer than 1 millisecond should be implemented as an asynchronous action node.

One approach for implementing asynchronous Behaviors is to handle long-running processes in a separate thread or using an asynchronous task. This way, ticking the Behavior just checks if the long-running process has completed yet.

Inheriting from BT::StatefulActionNode provides a simple way to allow your Behavior to check the progress of this long-running process. Equivalently, the onStarted() and onRunning() functions for these derived classes must also finish quickly.

Alternatively, you can use the AsyncBehaviorBase class included in MoveIt Pro to define the contents of such a process with less boilerplate code.

Creating a Behavior Loader Plugin

Custom Behaviors are registered with the Objective Server Behavior tree factory through a Behavior loader plugin. This is a class that is loaded at runtime using pluginlib, instantiated within the Objective Server, and called to register the Behaviors.

Behavior loader plugins are derived from the SharedResourcesNodeLoaderBase class.

An example implementation of a Behavior loader plugin would look like this:

#include <behaviortree_cpp/bt_factory.h>
#include <moveit_studio_behavior_interface/behavior_context.hpp>
#include <moveit_studio_behavior_interface/shared_resources_node_loader.hpp>
#include <pluginlib/class_list_macros.hpp>

// Include headers for your custom Behaviors
#include <my_package/my_basic_behavior.hpp>
#include <my_package/my_shared_resources_behavior.hpp>
namespace my_package
{
class MyBehaviorLoader : public SharedResourcesNodeLoaderBase
{
public:
  void registerBehaviors(BT::BehaviorTreeFactory& factory,
                     const std::shared_ptr<BehaviorContext>& shared_resources) override
  {
    // Register a Behavior derived from basic BehaviorTree.CPP node types
    registerBehavior<MyBasicBehavior>(factory, "MyBasicBehavior");
     // Register a Behavior derived from SharedResourcesNode
    registerBehavior<MySharedResourcesBehavior>(factory, "MySharedResourcesBehavior", shared_resources);
  }
};
}
PLUGINLIB_EXPORT_CLASS(my_package::MyBehaviorLoader,
                       moveit_studio::behaviors::SharedResourcesNodeLoaderBase);

Adding the loader plugin to the robot configuration package

When you Create a new Behavior package, a file named behavior_plugin.yaml will be created in that package. It should look as follows:

objectives:
behavior_loader_plugins:
  my_package:
    - "my_package::MyPackageBehaviorsLoader"

As long as this file exists in your Behavior package, it will be automatically added to the list of Behavior plugins.

note

Finished reference Behaviors can be found in the MoveIt Pro Example Behaviors repository.

Creating a Behavior to process a series of data points

When creating a Behavior whose desired functionality is to process a series of data points, such as filtering, it is best to have the Behavior inherit from BT::StatefulActionNode. BT::StatefulActionNode allows a Behavior to retain state between ticks, so multiple data points can be collected and processed. The Behavior can return SUCCESS if processing a finite number of data points or return RUNNING if continuously processing a number of data points.

An implementation of a Behavior that processes data points using BT::StatefulActionNode is Average Pose Stamped.