Adding Behavior Ports in MoveIt Pro
This guide assumes an understanding of how to create Objectives and Behaviors.
For preliminary reading, please refer to:
Behavior Ports are the main mechanism in which data is passed to and from a Behavior from the Blackboard. In the previous how-to guide, Create a New Behavior, we made a Behavior that returns immediately. In this guide we will add ports so that we can log a given message after a specified duration. We will also write unit tests for our Behavior that cover a number of use cases.
Create a Delayed Message Behavior Package
If you use the Graphical Interface, create a new AsyncBehavior called delayed_message.
If you use the Command Line, create a new AsyncBehavior called DelayedMessage in a package called delayed_message.
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/delayed_message/.
Add Ports to the Behavior
Open src/delayed_message.cpp in the text editor of your choice.
Replace return BT::PortsList({}); with the following:
return {
BT::InputPort<std::string>("message", "Hello, World!", "The message to log after a given duration."),
BT::InputPort<double>("delay_duration", "5", "The duration, in seconds, to wait before logging a message."),
};
Check the Port Values
Behavior ports can point to blackboard variables that have not been set.
For that reason alone port values should be validated, however additional data validation should be performed when appropriate.
At the beginning of the doWork() method, add the following lines:
// Get all the required port inputs, or an error message indicating what went wrong.
const auto ports =
moveit_studio::behaviors::getRequiredInputs(getInput<std::string>("message"),
getInput<double>("delay_duration"));
// If a port was not set, return the error
if (!ports.has_value())
{
auto error_message = fmt::format("Failed to get required values from input data ports:\n{}", ports.error());
return tl::make_unexpected(error_message);
}
// Assign port values to local variables
const auto& [message_string, delay_duration] = ports.value();
// Perform additional data validation if necessary
if (delay_duration < 0)
{
return tl::make_unexpected("Only positive delays are supported.");
}
Use the Port Values
In the doWork() method, add the following before return { true };
// Sleep for a given duration
std::this_thread::sleep_for(std::chrono::duration<double>(delay_duration));
// Log a Info message
shared_resources_->logger->publishInfoMessage(name(), "message_string");
Build, Test, and Run your Behavior
Add Unit Tests
As with all code, writing tests ensures your code functions, and continues to function, as expected.
The first test we add will check that the Behavior can be loaded, instantiated, and ticked.
When a Behavior is instantiated by MoveIt Pro, its ports are set to their default value.
When the node is instantiated in a test, its port values must be set.
In the following example we initialize the ports in two different ways.
The delay_duration port is set to a blackboard key, so the blackboard must have that value set.
The message port is set explicitly to Hello, World!, there is no blackboard key for it.
We will also check if the Behavior returns RUNNING after 1 second, and SUCCESS after more than 1.6 seconds.
Add the following tests to test/test_behavior_plugins.cpp:
/**
* @brief This test makes sure that the Behavior in this package waits the appropriate amount
* of time before returning SUCCESS
*/
TEST(BehaviorTests, test_behavior_success)
{
pluginlib::ClassLoader<moveit_studio::behaviors::SharedResourcesNodeLoaderBase>
class_loader("moveit_studio_behavior_interface",
"moveit_studio::behaviors::SharedResourcesNodeLoaderBase");
auto node = std::make_shared<rclcpp::Node>("test_node");
auto shared_resources = std::make_shared<moveit_studio::behaviors::BehaviorContext>(node);
BT::BehaviorTreeFactory factory;
{
auto plugin_instance = class_loader.createUniqueInstance(
"delayed_message::DelayedMessageBehaviorsLoader");
ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources));
}
// Create a blackboard for the test Behavior to use
BT::NodeConfiguration config;
config.blackboard = BT::Blackboard::create();
// Assign the blackboard key {delay_duration} a value
config.blackboard->set("delay_duration", "1.6");
// Set the input port value to the blackboard key {delay_duration}
config.input_ports.insert(std::make_pair("delay_duration", "{delay_duration}"));
// For the message input port, we will just set the value directly
config.input_ports.insert(std::make_pair("message", "Hello, World!"));
auto delayed_message_behavior = factory.instantiateTreeNode("test_behavior_name",
"DelayedMessage", config);
// Try calling tick() on the DelayedMessage Behavior to check if it is running.
ASSERT_EQ(delayed_message_behavior->executeTick(), BT::NodeStatus::RUNNING);
std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // Sleep for 1 second
// After 1 second, the node should still be running.
ASSERT_EQ(delayed_message_behavior->executeTick(), BT::NodeStatus::RUNNING);
// Sleep for a little more than the time remaining
std::this_thread::sleep_for(std::chrono::milliseconds(610));
// After more than 1.6 seconds has passed the node should return SUCCESS.
ASSERT_EQ(delayed_message_behavior->executeTick(), BT::NodeStatus::SUCCESS);
}
This test ensure a variety of incorrect port settings are caught
/**
* @brief This test makes sure that the Behavior can handle empty or incorrectly set ports
*/
TEST(BehaviorTests, test_bad_ports)
{
pluginlib::ClassLoader<moveit_studio::behaviors::SharedResourcesNodeLoaderBase>
class_loader("moveit_studio_behavior_interface",
"moveit_studio::behaviors::SharedResourcesNodeLoaderBase");
auto node = std::make_shared<rclcpp::Node>("test_node");
auto shared_resources = std::make_shared<moveit_studio::behaviors::BehaviorContext>(node);
BT::BehaviorTreeFactory factory;
{
auto plugin_instance = class_loader.createUniqueInstance(
"delayed_message::DelayedMessageBehaviorsLoader");
ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources));
}
// Create a blackboard for the test Behavior to use
BT::NodeConfiguration config;
config.blackboard = BT::Blackboard::create();
// Set the duration input port value to a blackboard key (which has no value)
config.input_ports.insert(std::make_pair("delay_duration", "{delay_duration}"));
// Set the message input port to a string
config.input_ports.insert(std::make_pair("message", "Hello, World!"));
// Instantiate the Behavior
auto delayed_message_no_port = factory.instantiateTreeNode("delayed_message_no_port",
"DelayedMessage", config);
//Tick the Behavior to start the asynchronous thread
ASSERT_EQ(delayed_message_no_port->executeTick(), BT::NodeStatus::RUNNING);
// Simulate a 100Hz tick() rate
std::this_thread::sleep_for(std::chrono::milliseconds(10));
// The DelayedMessage Behavior should fail if the port is not set.
ASSERT_EQ(delayed_message_no_port->executeTick(), BT::NodeStatus::FAILURE);
// Instantiate a new Behavior
auto delayed_message_string = factory.instantiateTreeNode("delayed_message_string",
"DelayedMessage", config);
// Set the blackboard value for {delay_duration} to a string
config.blackboard->set("delay_duration", "five seconds");
//Tick the Behavior to start the asynchronous thread
ASSERT_EQ(delayed_message_string->executeTick(), BT::NodeStatus::RUNNING);
// Simulate a 100Hz tick() rate
std::this_thread::sleep_for(std::chrono::milliseconds(10));
// The DelayedMessage Behavior should fail if the port is a string.
ASSERT_EQ(delayed_message_string->executeTick(), BT::NodeStatus::FAILURE);
// Instantiate a new Behavior
auto delayed_message_negative = factory.instantiateTreeNode("delayed_message_negative",
"DelayedMessage", config);
// Set the input port with a negative number
config.blackboard->set("delay_duration", "-5");
// Tick the Behavior to start the asynchronous thread
ASSERT_EQ(delayed_message_negative->executeTick(), BT::NodeStatus::RUNNING);
// Simulate a 100Hz tick() rate
std::this_thread::sleep_for(std::chrono::milliseconds(10));
// The DelayedMessage Behavior should return FAILURE if the delay_duration is negative.
ASSERT_EQ(delayed_message_negative->executeTick(), BT::NodeStatus::FAILURE);
// Instantiate a new Behavior
auto delayed_message_zero = factory.instantiateTreeNode("delayed_message_zero",
"DelayedMessage", config);
// Set the input port with a value of zero
config.blackboard->set("delay_duration", "0");
// Tick the Behavior to start the asynchronous thread
ASSERT_EQ(delayed_message_zero->executeTick(), BT::NodeStatus::RUNNING);
// Simulate a 100Hz tick() rate
std::this_thread::sleep_for(std::chrono::milliseconds(10));
// The DelayedMessage Behavior should succeed.
ASSERT_EQ(delayed_message_zero->executeTick(), BT::NodeStatus::SUCCESS);
}
Build your Behavior
You can build just the Behavior package using:
moveit_pro build user_workspace --colcon-args="--packages-select delayed_message"
Run the Updated Tests
Once your tests appropriately cover your Behavior's expectations, you can build your package and run the updated tests using:
moveit_pro build user_workspace --colcon-args="--packages-select delayed_message" && \
moveit_pro test --colcon-args="--packages-select delayed_message"
You will see something similar to the following output:
1: [==========] Running 3 tests from 1 test suite.
1: [----------] Global test environment set-up.
1: [----------] 3 tests from BehaviorTests
1: [ RUN ] BehaviorTests.test_load_behavior_plugins
1: [ OK ] BehaviorTests.test_load_behavior_plugins (61 ms)
1: [ RUN ] BehaviorTests.test_bad_ports
1: [ ] [error] Behavior: delayed_message_no_port. Error: Failed to execute action: Failed to get required values from input data ports:
1: The input port 'delay_duration' was set to '{delay_duration}', which was not found. Please verify that this is the correct blackboard variable name.
1: [ ] [error] Behavior: delayed_message_string. Error: Failed to execute action: Failed to get required values from input data ports:
1: stod
1: [ ] [error] Behavior: delayed_message_negative. Error: Failed to execute action: Only positive delays are supported.
1: [ ] [info] Behavior: delayed_message_zero. Info: message_string
1: [ OK ] BehaviorTests.test_bad_ports (57 ms)
1: [ RUN ] BehaviorTests.test_behavior_success
1: [ ] [info] Behavior: test_behavior_name. Info: message_string
1: [ OK ] BehaviorTests.test_behavior_success (1626 ms)
1: [----------] 3 tests from BehaviorTests (1744 ms total)
1:
1: [----------] Global test environment tear-down
1: [==========] 3 tests from 1 test suite ran. (1744 ms total)
1: [ PASSED ] 3 tests.
Use The Behavior
With your Behavior built, you can now use it in the objective of your choosing, it will be available next time you start MoveIt Pro.
Conclusion
Comprehension of this guide should allow you to confidently build and test new Behaviors using MoveIt Pro. Adding ports allows your Behavior to both read, and write data to the Behavior Tree blackboard. Through the use of tests, we validated some basic requirements of the Behavior, such as:
- It can be found and loaded by the plugin loader
- It can be instantiated by the Behavior Tree factory
- It wait the correct amount of time before succeeding when the ports are set
- For the delay duration, we set the value on the blackboard and the port is set to a blackboard key
- For the message string, we set the port value directly
- It will fail when the ports are not set, or set incorrectly
Now that you feel comfortable writing new Behaviors, take a look at examples we have in the next guide: Behavior Specialization