Manually Create a HelloWorld Behavior

This how-to guide shows how to create a new package containing a custom Behavior from the command line using only default ROS 2 command line tools and an IDE or text editor.

Note

The completed Behavior from this tutorial, and other example Behaviors, can be found in the MoveIt Studio Example Behaviors repository. If you’d like test against a finished example Behavior, clone this repository to your workspace’s src directory and skip to Build the Package.

1. Create a New Package

Create a new package that will contain your new Behaviors using this command:

ros2 pkg create --build-type ament_cmake --dependencies moveit_studio_behavior_interface pluginlib --library-name hello_world my_behaviors

This will create a new ROS package called my_behaviors containing an empty class named HelloWorld. The package depends on the moveit_studio_behavior_interface package.

2. Create the HelloWorld Behavior’s Header File

Open my_behaviors/include/my_behaviors/hello_world.hpp in your editor of choice, and paste in the following code:

#pragma once

#include <behaviortree_cpp/action_node.h>
#include <my_behaviors/visibility_control.h>

namespace my_behaviors
{
class HelloWorld : public BT::SyncActionNode
{
public:
  HelloWorld(const std::string& name, const BT::NodeConfiguration& config);

  static BT::PortsList providedPorts();

  BT::NodeStatus tick() override;
};
}  // namespace my_behaviors

Explanation of the Header File

Includes

#include <my_behaviors/visibility_control.h> is needed to build this package on Windows.

#include <behaviortree_cpp/action_node.h> includes the core BehaviorTree.Cpp tree node types, which we will inherit from to create a new Behavior.

Creating the HelloWorld Class

namespace my_behaviors
{
class HelloWorld : public BT::SyncActionNode

This defines a new class named HelloWorld, which inherits from the BT::SyncActionNode class, and puts it within the namespace my_behaviors.

A SyncActionNode performs some task synchronously and blocks until it either succeeds or fails. This type of action node is suitable for simple processes that will always finish very quickly.

Declaring Class Member Functions

public:
  HelloWorld(const std::string& name, const BT::NodeConfiguration& config);

  static BT::PortsList providedPorts();

  BT::NodeStatus tick() override;

These lines declare the public functions which must be implemented by classes inheriting from BT::SyncActionNode.

We provide a constructor, which takes two arguments:

  • The name of the Behavior. This will be set by the Behavior tree factory when it creates a Behavior tree that contains an instance of this Behavior.

  • The config of the Behavior. This defines runtime configuration info for each instance of this Behavior. It will be set by the Behavior tree factory when this Behavior is created within a new Behavior tree.

The BehaviorTree.Cpp library requires that Behaviors must implement the static providedPorts() function. This defines the Behavior’s input and output data ports. It is used internally by the BehaviorTree.Cpp library.

The tick() function is where all useful work happens within a SyncActionNode Behavior. This function must return very quickly after it is called to avoid blocking execution of other Behaviors in the Behavior tree. As a rule of thumb, a task that requires more than about 1 millisecond to finish should not use a SyncActionNode.

3. Create the HelloWorld Behavior’s Source File

Open my_behaviors/src/hello_world.cpp, and paste in the following code:

#include "my_behaviors/hello_world.hpp"

namespace my_behaviors
{
HelloWorld::HelloWorld(const std::string& name, const BT::NodeConfiguration& config)
  : BT::SyncActionNode(name, config)
{
}

BT::PortsList HelloWorld::providedPorts()
{
  // Returns an empty BT::PortsList, which declares that HelloWorld does not have any ports whatsoever.
  return BT::PortsList({});
}

BT::NodeStatus HelloWorld::tick()
{
  // Do HelloWorld's useful work
  std::cout << "Hello, world!" << std::endl;

  // Return SUCCESS once the work has been completed.
  return BT::NodeStatus::SUCCESS;
}
}  // namespace my_behaviors

Explanation of the Source File

Includes

#include "my_behaviors/hello_world.hpp"

namespace my_behaviors
{

This includes the header file we created previously, and puts the implementations of the functions defined in that header within the my_behaviors namespace.

HelloWorld Constructor

HelloWorld::HelloWorld(const std::string& name, const BT::NodeConfiguration& config)
  : BT::SyncActionNode(name, config)
{
}

This is a standard constructor for Behaviors that inherit from SyncActionNode. The constructor arguments are passed on to the base class.

HelloWorld::providedPorts()

BT::PortsList HelloWorld::providedPorts()
{
  // Returns an empty BT::PortsList, which declares that HelloWorld does not have any ports whatsoever.
  return BT::PortsList({});
}

We provide an implementation of the required providedPorts() function. As explained in the inline comment, since the HelloWorld Behavior does not use any input or output data ports, this function returns an empty BT::PortsList.

HelloWorld::tick()

BT::NodeStatus HelloWorld::tick()
{
  // Do HelloWorld's useful work
  std::cout << "Hello, world!" << std::endl;

  // Return SUCCESS once the work has been completed.
  return BT::NodeStatus::SUCCESS;
}

We implement the tick() function, which is where HelloWorld does all its useful work. This simply prints the text "Hello, world!" to the console and then returns BT::NodeStatus::SUCCESS to show that the Behavior finished successfully.

4. Create a Behavior Loader Plugin

Create a new .cpp file in my_behaviors/src/ named my_behaviors_loader.cpp, then open it in your editor.

touch my_behaviors/src/my_behaviors_loader.cpp

Paste the following code into this file:

#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 <my_behaviors/hello_world.hpp>

namespace my_behaviors
{
class MyBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase
{
public:
  void registerBehaviors(BT::BehaviorTreeFactory& factory,
                         const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& /*shared_resources*/) override
  {
    moveit_studio::behaviors::registerBehavior<HelloWorld>(factory, "HelloWorld");
  }
};
}

PLUGINLIB_EXPORT_CLASS(my_behaviors::MyBehaviorsLoader,
                       moveit_studio::behaviors::SharedResourcesNodeLoaderBase);

Explanation of the Behavior Loader Plugin

Includes

#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 <my_behaviors/hello_world.hpp>

First, this includes header files needed to implement the Behavior loader class and then export it as a plugin.

Then, this includes the header file for our HelloWorld Behavior which we created earlier.

MyBehaviorsLoader Class

namespace my_behaviors
{
class MyBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase
{

The MyBehaviorsLoader class inherits from moveit_studio::behaviors::SharedResourcesNodeLoaderBase, which is the base class used for all Behavior loader plugins in the MoveIt Studio SDK.

MyBehaviorsLoader::registerBehaviors

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

Implementations of SharedResourcesNodeLoaderBase must override the registerBehaviors function. This function is called within the Agent to register the custom Behaviors with the Behavior tree factory so new instances of them can be created when running objectives.

Within this function, the registerBehavior function handles the work of registering our custom HelloWorld Behavior. The string "HelloWorld" will be used as the name of this Behavior when creating new objectives. This name should match the Behavior’s class name.

Exporting MyBehaviorsLoader as a Plugin

PLUGINLIB_EXPORT_CLASS(my_behaviors::MyBehaviorsLoader,
                       moveit_studio::behaviors::SharedResourcesNodeLoaderBase);

The PLUGINLIB_EXPORT_CLASS macro registers the MyBehaviorsLoader class with pluginlib so it can be dynamically loaded at runtime.

Within this macro, we declare provide the fully-namespaced names of both MyBehaviorsLoader and its base class.

5. Create a Plugin Description XML File

Create a new XML file at my_behaviors/my_behaviors_plugin_description.xml:

touch my_behaviors/my_behaviors_plugin_description.xml

Copy the following text into this file:

<library path="my_behaviors">
  <class type="my_behaviors::MyBehaviorsLoader"
         base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase">
  </class>
</library>

This file tells pluginlib where to find the compiled library that contains the MyBehaviorsLoader plugin.

6. Create a Behavior Loader Unit Test

Create a new test directory in the my_behaviors package:

mkdir my_behaviors/test

Create a new .cpp file within this directory named test_load_my_behaviors.cpp:

touch my_behaviors/test/test_load_my_behaviors.cpp

Open this file in your editor, and paste the code provided below into this file:

#include <gtest/gtest.h>
#include <behaviortree_cpp/bt_factory.h>
#include <moveit_studio_behavior_interface/shared_resources_node_loader.hpp>
#include <pluginlib/class_loader.hpp>
#include <rclcpp/node.hpp>

namespace
{
constexpr auto kBehaviorInterfacePackageName = "moveit_studio_behavior_interface";
constexpr auto kBehaviorBaseClassName = "moveit_studio::behaviors::SharedResourcesNodeLoaderBase";
}  // namespace

/**
 * @brief This test makes sure that the Behaviors provided in this package can be successfully registered with an
 * instantiated by the Behavior tree factory.
 * @details It is recommended to include a test like this one in packages that add new Behaviors, since Behaviors
 * are only loaded at runtime and there are several possible points of failure that could prevent them from being
 * loaded, such as typos in the package's plugin description XML file or within the plugin loader function.
 */
TEST(MyBehaviors, load_my_behaviors)
{
  pluginlib::ClassLoader<moveit_studio::behaviors::SharedResourcesNodeLoaderBase> class_loader(kBehaviorInterfacePackageName,
                                                                                kBehaviorBaseClassName);

  auto node = std::make_shared<rclcpp::Node>("test_node");
  auto shared_resources = std::make_shared<moveit_studio::behaviors::BehaviorContext>(node);

  // Test creating an instance of our Behavior using the Behavior loader plugin
  BT::BehaviorTreeFactory factory;
  {
    auto plugin_instance = class_loader.createUniqueInstance("my_behaviors::MyBehaviorsLoader");
    ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources));
  }

  // Test that ClassLoader is able to find and instantiate each Behavior using the package's plugin description info.
  auto hello_world_behavior = factory.instantiateTreeNode("say_hello_behavior", "HelloWorld", BT::NodeConfiguration());

  // Try ticking the HelloWorld Behavior to print its friendly text output when we run this test
  EXPECT_EQ(hello_world_behavior->executeTick(), BT::NodeStatus::SUCCESS);
}

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}

As described in the comments in this file, this test will try registering the Behavior loader plugin and instantiating the HelloWorld Behavior.

7. Update CMakeLists.txt

Open my_behaviors/CMakeLists.txt in your editor. This file has already been filled in with some default CMake code from when we created the my_behaviors package. Update this file so it matches the code below:

cmake_minimum_required(VERSION 3.22.1)
project(my_behaviors LANGUAGES CXX)

if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
    add_compile_options(-Werror -Wall -Wextra -Wpedantic -Wshadow)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_ros REQUIRED)
find_package(moveit_studio_behavior_interface REQUIRED)
find_package(pluginlib REQUIRED)

add_library(my_behaviors
  src/hello_world.cpp
  src/my_behaviors_loader.cpp)
target_compile_features(my_behaviors PUBLIC cxx_std_17)
target_include_directories(my_behaviors PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
ament_target_dependencies(
  my_behaviors
  moveit_studio_behavior_interface
  pluginlib
)

# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.
target_compile_definitions(my_behaviors PRIVATE "MY_BEHAVIORS_BUILDING_LIBRARY")

install(
  DIRECTORY include/
  DESTINATION include
)
install(
  TARGETS my_behaviors
  EXPORT export_${PROJECT_NAME}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

if(BUILD_TESTING)
  find_package(ament_cmake_gtest REQUIRED)
  ament_add_gtest(test_load_example_behavior_plugins
    test/test_load_my_behaviors.cpp)
  ament_target_dependencies(test_load_example_behavior_plugins
    moveit_studio_behavior_interface
    pluginlib)
endif()

# Export the Behavior plugins defined in this package so they are available to plugin loaders that load the
# Behavior base class library from the moveit_studio_behavior_interface package.
pluginlib_export_plugin_description_file(moveit_studio_behavior_interface my_behaviors_plugin_description.xml)

ament_export_include_directories(
  include
)
ament_export_libraries(
  my_behaviors
)
ament_export_targets(
  export_${PROJECT_NAME} HAS_LIBRARY_TARGET
)

ament_package()

This will compile hello_world.cpp into a library named my_behaviors, then export this library to pluginlib using the pluginlib description XML file we created earlier, and finally install the library.

8. Update package.xml

Open my_behaviors/package.xml in your editor. This file was already filled in when we created the my_behaviors package.

Paste this text into the package.xml file:

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>my_behaviors</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="[email protected]">Your Name Here</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake_ros</buildtool_depend>

  <depend>moveit_studio_behavior_interface</depend>
  <depend>pluginlib</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
  <test_depend>ament_cmake_gtest</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

The most functionally-important parts of this file are the dependencies on the moveit_studio_behavior_interface and pluginlib packages, since these packages are required to build and run the my_behaviors package.

9. Build the Package

From the root directory of your workspace, run the following command to build your new package:

colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install

10. Run the Behavior Loader Unit Tests

Run unit tests for the my_behaviors package:

colcon test --event-handlers console_direct+ --packages-select my_behaviors

The test should succeed, and the output printed to the console should contain some text like this:

1: [==========] Running 1 test from 1 test suite.
1: [----------] Global test environment set-up.
1: [----------] 1 test from RobotTask
1: [ RUN      ] RobotTask.load_example_behavior_plugins
1: Hello, world!
1: [       OK ] RobotTask.load_example_behavior_plugins (20 ms)
1: [----------] 1 test from RobotTask (20 ms total)
1:
1: [----------] Global test environment tear-down
1: [==========] 1 test from 1 test suite ran. (20 ms total)
1: [  PASSED  ] 1 test.

...

100% tests passed, 0 tests failed out of 1

This output shows that the HelloWorld Behavior can be successfully loaded through pluginlib, instantiated by the Behavior tree factory, and then ticked to make it print its message.

These are the minimal criteria needed to demonstrate that this Behavior can be loaded and run by the MoveIt Studio Agent as part of a custom objective.