Making a New Behavior

This how-to guide walks you through how to extend the functionality of MoveIt Pro by creating a new custom Behavior plugin that can be loaded and run within your MoveIt Pro Objectives.

The end result of this how-to guide will be a new ROS package, which contains a Behavior that succeeds immediately.

Creating a New Behavior Package

You can create a new Behavior package from one of our templates using a graphical or command line approach.

A brief summary of the node types are:

• A SyncAction node is expected to tick once and exit very quickly.
• A StatefulAction node is intended for longer running processes that the Behavior tree can continually tick until the Behavior status causes it to do otherwise.
• The WithSharedResources suffix provides the node with access to ROS 2 resources, allowing it to create publishers, subscribers, loggers, and other ROS 2 constructs.
• An AsyncBehaviorBase node is a specialization of the StatefulActionWithSharedResources class with convenience functions for asynchronous tasks.

For further detail, consult the API documentation or check out some examples.

Creating a New Behavior Package (Graphical Method)

For the graphical approach, start MoveIt Pro and create a new objective (or edit an existing one). While editing an objective, click + Behavior to bring up the Behavior creation dialog where you can give it a name, type, and description. If you don't know which type to choose, pick AsyncBehavior.

A popup will appear containing the path to the newly-created Behavior package. The location provided is the path that is used inside the Docker container. The contents of the container's ${HOME}/user_ws is shared with the container's host (your system), at the location of your user workspace.

Creating a New Behavior Package (Command Line)

To create a new Behavior package using cookiecutter, you must first start a MoveIt Pro development image using moveit_pro dev. Once started, you can invoke the package creation script using:

cookiecutter $(ros2 pkg prefix --share moveit_studio_behavior_template) --output-dir ~/user_ws/src

You will them be prompted to:

• Enter a package name (this will be the ROS 2 package name that gets created)
• Enter a Behavior name (this will be the name of the Behavior when using it in MoveIt Pro)
• Select a Behavior type (pick AsyncBehavior if you don't know which type to choose)
• Give your project a description
• Provide the package author's name
• Provide the package author's email

Once complete you will now have a package in your workspace. Before you compile with colcon build, remove the COLCON_IGNORE file from the directory.

Inspect the New Behavior Package Directory

Open the directory where the new Behavior package was created. Within the MoveIt Pro container it is located at ${HOME}/user_ws/, or on the host it can be found in the src folder of your user workspace.

Your new package contains the following files:

.
├── behavior_plugin.yaml
├── CMakeLists.txt
├── config
│   └── tree_nodes_model.xml
├── include
│   └── package_name
│       └── my_behavior.hpp
├── package_name_plugin_description.xml
├── package.xml
├── src
│   ├── my_behavior.cpp
│   └── register_behaviors.cpp
└── test
    ├── CMakeLists.txt
    └── test_behavior_plugins.cpp

Build the Behavior Package

Run the following command:

moveit_pro build user_workspace

Building the workspace should succeed; if not, you will see a description of the issue in the terminal window.

Write Unit Tests for your New Behavior

Even though we haven't added any additional functionality to our new Behavior, we can confirm that the basic functionality in our Behavior works as expected. Let's add unit tests. By default the test_load_behavior_plugins test checks two important things:

• The Behavior loader plugin that contains the Behavior can be loaded at runtime using its name and registered with the Behavior tree factory to make its Behaviors available.
• The Behavior tree factory can create an instance of the Behavior.

Next we will add a simple test to make sure the Behavior can be ticked, and will return success after the process completes.

Open test/test_behavior_plugins.cpp and add the following test:

TEST(BehaviorTests, test_behavior_success)
{
  pluginlib::ClassLoader<moveit_studio::behaviors::SharedResourcesNodeLoaderBase> class_loader(
      "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase");

  // Set up and instantiate a new Behavior
  auto node = std::make_shared<rclcpp::Node>("BehaviorTests");
  auto shared_resources = std::make_shared<moveit_studio::behaviors::BehaviorContext>(node);
  BT::BehaviorTreeFactory factory;
  {
    auto plugin_instance = class_loader.createUniqueInstance("package_name::PackageNameBehaviorsLoader");
    ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources));
  }
  auto my_behavior = factory.instantiateTreeNode("test_behavior_name", "MyBehavior",
                                                           BT::NodeConfiguration());

  // Try calling tick() on the Behavior, the first time it should return RUNNING.
  EXPECT_EQ(my_behavior->executeTick(), BT::NodeStatus::RUNNING);

  // Simulate a 100Hz tick() rate
  std::this_thread::sleep_for(std::chrono::milliseconds(10));

  // The next time the Behavior is ticked, it should return SUCCESS.
  EXPECT_EQ(my_behavior->executeTick(), BT::NodeStatus::SUCCESS);

}

The test_behavior_success test checks if the Behavior can be instantiated and ticked, as well as confirms the expected behavior. When the Behavior is ticked, it starts an asynchronous process and returns RUNNING. The process completes immediately, but we simulate a 100Hz Behavior Tree tick rate by sleeping for 10 milliseconds. The next time the Behavior is ticked it should return SUCCESS.

Run the new unit test by entering the following command. This will build the user workspace to ensure that all packages are up to date, and then run all of the tests in the workspace:

moveit_pro build user_workspace && \
moveit_pro test"

At the end of the console output, you should see something similar to this:

1: [==========] Running 2 tests from 1 test suite.
1: [----------] Global test environment set-up.
1: [----------] 2 tests from BehaviorTests
1: [ RUN      ] BehaviorTests.test_load_behavior_plugins
1: [       OK ] BehaviorTests.test_load_behavior_plugins (30 ms)
1: [ RUN      ] BehaviorTests.test_behavior_success
1: [       OK ] BehaviorTests.test_behavior_success (24 ms)
1: [----------] 2 tests from BehaviorTests (54 ms total)
1: 
1: [----------] Global test environment tear-down
1: [==========] 2 tests from 1 test suite ran. (54 ms total)
1: [  PASSED  ] 2 tests.         

If you'd like to take a closer look at the test output for the particular test that we just modified, go to your user workspace and open log/latest_test/package_name/stdout.log.

Running Your New Behavior in MoveIt Pro

If you've already built and tested your Behavior, it is ready for use in MoveIt Pro. If you have not built your new package, use moveit_pro build before you moveit_pro run.

Adding a New Behavior to an Existing Package

note

Throughout the remainder of this how-to guide, we will refer to package files via relative paths such as include/..., src/..., and test/.... These paths are relative to a package directory, such as ~/user_ws/src/package_name/.

You do not have to create a new package for each Behavior. Additional Behaviors can be added to an existing package using the following steps:

Create .cpp and .hpp files

Following the existing structure, place your .cpp file in src/ and .hpp file in include/package_name and add content.

Add source files to CMake

Add the new source file to the CMake library target in CMakeLists.txt. It should look something like this:

add_library(
  package_name
  SHARED
  src/my_behavior.cpp
  src/my_other_behavior.cpp
  src/register_behaviors.cpp)

Register the Behavior Plugin

Edit src/register_behaviors.cpp to include your .hpp file, and register additional Behaviors like so:

  void registerBehaviors(BT::BehaviorTreeFactory& factory,
       const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources) override
  {
    moveit_studio::behaviors::registerBehavior<MyBehavior>(factory, "MyBehavior", shared_resources);
    moveit_studio::behaviors::registerBehavior<MyOtherBehavior>(factory, "MyOtherBehavior", shared_resources);
  }