MoveIt Pro Behavior Interface

This package contains the base classes used to create new Behavior plugins that can be executed as part of Objectives.

Refer to the Behavior Trees documentation for more information.

Testing

The test directory contains various helper classes and macros for Behavior testing. These helpers can:

• Initialize a test Behavior
• Help with useful operations in tests, like setting transforms or test data directories.
• Provide accessors to various components: the Behavior, its node, its shared resources and mock logger if any, etc.
• Automatically test that the Behavior creates exactly a set of input and output ports.
• Automatically generate tests that check if the Behavior handles well a situation in which one input port is not set in the blackboard.

The test file is the source of truth about the Behavior, and the Behavior code will have to comply with the port definition in the test file, so the first thing to do is defining the input and output ports of the Behavior under test.

The following example defines a map of input ports named "myExpectedInputPorts" and a map of output ports named "myExpectedOutputPorts". In each line of the map definition, a port ID is associated with the value the port will take in that map. Note that, if a port is both an input and an output, we just need to include it in both maps.

BEGIN_BEHAVIOR_PORT_SETTER_MAP(myExpectedInputPorts)
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDPointCloud, sensor_msgs::msg::PointCloud2())
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDMasks3D, std::vector<Mask3D>())
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDBaseFrame, "")
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDPlaneInlierThreshold, 0.0)
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDMinimumFaceArea, 0.0)
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDFaceSeparationThreshold, 0.0)
END_BEHAVIOR_PORT_SETTER_MAP()
 
BEGIN_BEHAVIOR_PORT_SETTER_MAP(myExpectedOutputPorts)
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDGraspableObjects, GraspableObject())
END_BEHAVIOR_PORT_SETTER_MAP()

Now, to test the Behavior, our test fixture will have to inherit from a helper class (in addition to its base test fixture). There are helper classes for Behaviors with and without context, also known as shared resources. If the Behavior has context, the most common case, our fixture must inherit from WithBehavior<>. In either case, we then must call initBehavior() from the fixture's SetUp(), or from any other setup function the fixture offers. The following example shows this case. Note how initBehavior() gets the expected input and output ports we defined above.

#include <moveit_studio_behavior_interface/test_behavior.hpp>
#include <moveit_studio_common/test_utils/ros_test.hpp>
 
...
 
class MyBehaviorWithContextTest
  : public moveit_studio::test_utils::RosTest, public moveit_studio::test_utils::WithBehavior<MyBehaviorWithContext>
{
public:
  void SetUp() override {
    initBehavior(/*name=*/"MyBehaviorWithContext", myExpectedInputPorts, myExpectedOutputPorts);
 
    // TODO: set up other things for your tests here.
  }
};

For behaviors without context, the pattern is the same, but uses the WithBehaviorWithoutContext helper class instead.

#include <moveit_studio_behavior_interface/test_behavior.hpp>
#include <moveit_studio_common/test_utils/ros_test.hpp>
 
...
 
class MyBehaviorWithoutContextTest
  : public moveit_studio::test_utils::RosTest, public moveit_studio::test_utils::WithBehaviorWithoutContext<MyBehaviorWithoutContext>
{
public:
  void SetUp() override {
    initBehavior(/*name=*/"MyBehaviorWithoutContext", myExpectedInputPorts, myExpectedOutputPorts);
 
    // TODO: set up other things for your tests here.
  }
};

There are macros to automatically generate tests to check if your Behavior handles the situation in which one port has no value set in the blackboard when it is run. There are macros for synchronous and asynchronous Behaviors, with and without context.

If the Behavior is synchronous and has context:

#include <moveit_studio_behavior_interface/test_behavior_input_port_not_set.hpp>
 
...
 
INSTANTIATE_SYNC_BEHAVIOR_PORT_NOT_SET_TESTS(MyBehaviorWithContext, myExpectedInputPorts,
                                             myExpectedOutputPorts)

If the Behavior is asynchronous and has context:

#include <moveit_studio_behavior_interface/test_behavior_input_port_not_set.hpp>
 
...
 
INSTANTIATE_ASYNC_BEHAVIOR_PORT_NOT_SET_TESTS(MyBehaviorWithContext, myExpectedInputPorts,
                                              myExpectedOutputPorts)

If the Behavior is synchronous and does not have context:

#include <moveit_studio_behavior_interface/test_behavior_without_context_input_port_not_set.hpp>
 
...
 
INSTANTIATE_SYNC_BEHAVIOR_WITHOUT_CONTEXT_PORT_NOT_SET_TESTS(MyBehaviorWithContext, myExpectedInputPorts,
                                             myExpectedOutputPorts)

If the Behavior is asynchronous and does not have context:

#include <moveit_studio_behavior_interface/test_behavior_without_context_input_port_not_set.hpp>
 
...
 
INSTANTIATE_ASYNC_BEHAVIOR_WITHOUT_CONTEXT_PORT_NOT_SET_TESTS(MyBehaviorWithContext, myExpectedInputPorts,
                                              myExpectedOutputPorts)

These macros generate one test per input port, in which the port is the only one not set. The test expects the Behavior to fail after logging a failure message pointing at the port.

But what if a Behavior is ok with an input port not being set? We can simply define the port as optional as below. Optional ports must be created by the Behavior, but no specific outcome will be enforced by the "port no set" tests.

DEFINE_OPTIONAL_BEHAVIOR_PORT_SETTER(kPortIDOptionalInput, 0.0)

Let's put everything together in an example where we test an asynchronous Behavior with context:

#include <moveit_studio_behavior_interface/test_behavior.hpp>
#include <moveit_studio_behavior_interface/test_behavior_input_port_not_set.hpp>
#include <moveit_studio_common/test_utils/ros_test.hpp>
 
// TODO: Add your header files.
 
namespace moveit_studio {
namespace {
using ::moveit_studio::test_utils::RosTest;
using ::moveit_studio::test_utils::WithBehavior;
 
// TODO: Add your anonymous elements.
}  // namespace
 
BEGIN_BEHAVIOR_PORT_SETTER_MAP(myExpectedInputPorts)
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDPointCloud, sensor_msgs::msg::PointCloud2())
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDMasks3D, std::vector<Mask3D>())
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDBaseFrame, "")
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDPlaneInlierThreshold, 0.0)
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDMinimumFaceArea, 0.0)
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDFaceSeparationThreshold, 0.0)
DEFINE_OPTIONAL_BEHAVIOR_PORT_SETTER(kPortIDOptionalInput, 0.0)
END_BEHAVIOR_PORT_SETTER_MAP()
 
BEGIN_BEHAVIOR_PORT_SETTER_MAP(myExpectedOutputPorts)
DEFINE_BEHAVIOR_PORT_SETTER(kPortIDGraspableObjects, GraspableObject())
END_BEHAVIOR_PORT_SETTER_MAP()
 
class MyBehaviorWithContextTest
  : public RosTest, public WithBehavior<MyBehaviorWithContext>
{
public:
  void SetUp() override {
    initBehavior(/*name=*/"MyBehaviorWithContext", myExpectedInputPorts, myExpectedOutputPorts);
 
    // TODO: set up other things for your tests here.
  }
};
 
INSTANTIATE_ASYNC_BEHAVIOR_PORT_NOT_SET_TESTS(MyBehaviorWithContext, myExpectedInputPorts,
                                              myExpectedOutputPorts)
 
// TODO: Add your Behavior tests.
 
}  // namespace moveit_studio

Testing Behaviors that communicate with action servers

Behaviors communicating with action servers normally mock those servers in the test fixture. The WithActionBehavior base class lets us pass a mock when initializing the action Behavior:

namespace {
using ::moveit_studio::behaviors::mockup::MockServiceClientInterface;
}
 
class MyActionBehaviorTest
  : public moveit_studio::test_utils::RosTest,
    public moveit_studio::test_utils::WithActionBehavior<MyActionBehavior, MockServiceClientInterface<MyActionType>> {
public:
  void SetUp() override {
    auto mock_client_interface = std::make_unique<MockServiceClientInterface<MyActionType>>();
    initBehavior(/*name=*/"MyActionBehavior", myExpectedInputPorts, myExpectedOutputPorts, std::move(mock_client_interface));
 
    // TODO: set up other things for your tests here.
  }
};

The macros that automatically instantiate "port not set" tests use their own test fixture internally and won't set up any action server mocks. In that case, we can use another flavor of the macro:

INSTANTIATE_ACTION_BEHAVIOR_PORT_NOT_SET_TESTS(MyActionBehaviorTest, MyActionBehavior, myExpectedInputPorts, myExpectedOutputPorts)

Note that the only novelty is that the first parameter is the test fixture itself. The macro will instantiate all tests as usual, but they will use the passed fixture, which will set up all action server mocks appropriately before each test.

Behaviors to load data types from YAML

We provide generic LoadFromYaml and LoadMultipleFromYaml templates that can be used to create behaviors that need to load data types from a YAML file.

All that is needed is to:

1. Write a YAML conversion function for your type, if there isn't one already.
2. Register the Behavior for your type.

Add a YAML conversion function.

To use the LoadFromYaml and LoadMultipleFromYaml Behavior templates, a YAML conversion function needs to exist for the data type being loaded.

YAML conversion functions for common types, including generic ROS messages can be found in yaml_parsing_tools.hpp.

We offer a ROS_MESSAGE_YAML_PARSER macro to automatically add support for any ROS message. It just needs to be instantiated for you particular type. For instance, adding this line will create a YAML conversion function for trajectory_msgs::msg::JointTrajectory messages:

#include <moveit_studio_behavior_interface/yaml_parsing_tools.hpp>
 
ROS_MESSAGE_YAML_PARSER(trajectory_msgs, JointTrajectory)

After this, LoadFromYaml and LoadMultipleFromYaml can be used with this type.

Register a YAML-loading Behavior.

Once a YAML conversion function exists for the type you want to load, you can register your Behavior using the provided LoadFromYaml or LoadMultipleFromYaml templates.

For instance, this line will register a new Behavior, called LoadJointTrajectoryFromYaml, to load a trajectory_msgs::msg::JointTrajectory ROS message from a YAML file.

#include <moveit_studio_behavior_interface/load_from_yaml.hpp>
 
registerBehavior<LoadFromYaml<trajectory_msgs::msg::JointTrajectory>>(factory, "LoadJointTrajectoryFromYaml", shared_resources);

If you need a Behavior to load a vector of a given data type, use LoadMultipleFromYaml instead. For instance, this line will register a new Behavior, called LoadPoseStampedVectorFromYaml, to load a vector of geometry_msgs::msg::PoseStamped messages from a YAML file:

#include <moveit_studio_behavior_interface/load_from_yaml.hpp>
 
registerBehavior<LoadMultipleFromYaml<geometry_msgs::msg::PoseStamped>>(factory, "LoadPoseStampedVectorFromYaml", shared_resources);

In this case, the YAML file needs to be formatted as multi-document YAML, e.g.:

---
position:
  x: 1.0
  y: 2.0
  z: 3.0
orientation:
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0
---
position:
  x: 1.0
  y: 0.5
  z: 0.2
orientation:
  x: 0.707
  y: 0.0
  z: 0.0
  w: 0.707