Creating a Stateful Behavior

Making the Stateful Behavior

Tutorial Level: Intermediate

In the previous tutorial Create a Custom Behavior, we made a BT::SyncActionNode Behavior that returns a BT::NodeStatus when it gets ticked and logs an error message. In this tutorial, we will build a Behavior that does not finish immediately using a BT::StatefulActionNode.

While the BT::SyncActionNode can only return BT::NodeStatus::SUCCESS or BT::NodeStatus::FAILURE, the BT::StatefulActionNode can return BT::NodeStatus::RUNNING when it has not yet reached a completed state.

Upon completion of this tutorial, we will have a Delayed Message Behavior that will log a message like the Hello World Behavior, but will wait for a certain duration before doing so. Additionally, we will make use of Behavior ports to specify the amount of time the Behavior should wait for.

Note

This tutorial assumes you have completed the Making a Hello World Behavior tutorial or are familiar with the steps to create a custom Behavior. Alternatively, you can clone the MoveIt Pro Example Behaviors repository to your workspace’s src directory. An example configuration package with Objectives that make use of these Behaviors can be found in the moveit_studio_ur_ws repository.

1. Create the Behavior Package

In the same way we created the Hello World Behavior, we will create the Delayed Message Behavior through the web app:

In the New Custom Behavior popup window, enter a Behavior name and description for your custom Behavior:

• Enter the Behavior name delayed_message.
• Enter a description like After some time, log a message that says "Hello, world!"..

In the Behavior Type dropdown, select StatefulActionNodeWithSharedResources and complete the process to create a new Behavior.

2. Inspect the Newly Created Behavior

When using the UI to create the Behavior package, it will be placed in the location set by your STUDIO_HOST_USER_WORKSPACE environment variable. If left unchanged from the previous tutorial, you will find it along side the Hello World Behavior package.

Note

Throughout the remainder of this tutorial, we will refer to Delayed Message Behavior package files via relative paths such as include/..., src/..., test/..., and config/.... These paths are relative to the STUDIO_HOST_USER_WORKSPACE/src/delayed_message directory.

Open src/delayed_message.cpp in the text editor of your choice. You will notice there is no longer a tick() method, and in its place find the onStart(), onRunning(), and onHalted() methods.

While the tick() method is expected to execute in a short period of time, onRunning() is its stateful replacement. Neither function should block the execution of the Behavior tree! Both are expected to return a BT::NodeStatus, but tick() should return a SUCCESS or FAILURE immediately where onRunning() may return RUNNING while it completes its long running task.

When the Behavior is ticked for the first time, the onStart() method is called. The Behavior tree will continue ticking the Behavior by calling the onRunning() method as long as it returns RUNNING, unless the Objective is halted (in which case the onHalted() method is called).

For a detailed discussion on asynchronous Behaviors, refer to the BehaviorTree.CPP Documentation.

3. Edit the Behavior Source Code

Stateful nodes may require the use of member variables, so in this tutorial we will add a std::chrono::time_point<std::chrono::steady_clock> to keep track of the time since the Behavior started.

First, open include/delayed_message/delayed_message.hpp and add the header for the MoveItErrorCodes message type. Also add the chrono header so that we can use std::chrono::time_point:

#include <chrono>

#include <behaviortree_cpp/action_node.h>

// This header includes the SharedResourcesNode type
#include <moveit_studio_behavior_interface/shared_resources_node.hpp>

#include <moveit_msgs/msg/move_it_error_codes.hpp>

...

Then, add the private member variable start_time_:

class DelayedMessage : public moveit_studio::behaviors::SharedResourcesNode<BT::StatefulActionNode>
{
private:
  std::chrono::time_point<std::chrono::steady_clock> start_time_;

public:
...

The modifications to delayed_message.hpp are complete, so save your changes and open src/delayed_message.cpp.

We can initialize start_time_ when the node is first ticked by adding the following to the onStart() method:

BT::NodeStatus DelayedMessage::onStart()
{
  // Store the time at which this node was first ticked
  start_time_ = std::chrono::steady_clock::now();
  return BT::NodeStatus::RUNNING;
}

For now, we will set the delay duration to 5 seconds, so we know after the first tick the node will be RUNNING. Every subsequent tick to this node will call the onRunning() method, so here we will check to see if 5 seconds has passed:

BT::NodeStatus DelayedMessage::onRunning()
{
  // If 5 seconds have not elapsed since this node was started, return RUNNING
  if (std::chrono::duration_cast<std::chrono::seconds>(std::chrono::steady_clock::now()
      - start_time_).count() < 5)
  {
    return BT::NodeStatus::RUNNING;
  }
  else
  {
    // Log the "Hello, world!" message.
    // Setting the third argument to false ensures the message will be shown immediately
    shared_resources_->logger->publishInfoMessage(name(), "Hello, world!", false);
    return BT::NodeStatus::SUCCESS;
  }
}

In this node we don’t need to do anything special when the Behavior is halted, so we can leave the onHalted() method unchanged.

4. Add Behavior Metadata

You can also add a Behavior level description and subcategory in your implementation of metadata():

BT::KeyValueVector DelayedMessage::metadata()
{
  return { { "subcategory", "Example" },
           { "description", "After some time, log a message that says \"Hello, world!\"." } };

5. Write Unit Tests

As with all code, writing tests ensures your code functions, and continues to function, as expected. To test this Behavior, we add the following test 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_plugins)
{
  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));
  }

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

  // 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);

  std::this_thread::sleep_for(std::chrono::milliseconds(5000)); // sleep for 5 seconds

  // After a total of 6 seconds, the node should return SUCCESS.
  ASSERT_EQ(delayed_message_behavior->executeTick(), BT::NodeStatus::SUCCESS);
}

6. Build the Behavior and Run the Tests

Run the following commands in the directory that contains your docker-compose.yaml file to build and test the code we just added:

./moveit_pro build
./moveit_pro test

Note

The container has a ~/.colcon/defaults.yaml file which defines default settings when running colcon commands. The colcon build command is set up to use the following mixin verbs: ccache, lld, compile-commands, rel-with-deb-info, build-testing-on. The colcon test command is set up to use event-handlers values of console_direct+ and desktop_notification+.

Adding Behavior Ports

Now that we have a basic implementation working for our Behavior, let’s use Behavior ports to make the delay duration configurable.

1. Add an Input Port

When inspecting your newly created Behavior, you may have noticed the following section in src/delayed_message.cpp:

BT::PortsList DelayedMessage::providedPorts()
{
  // TODO(...)
  return BT::PortsList({});
}

In this section, you can specify the ports your Behavior uses to communicate with the blackboard. Detailed discussion of blackboard ports can be found here, but the concept of blackboards and ports can be summarized as:

• A “Blackboard” is a simple key/value storage shared by all the nodes of the Tree.
• An “entry” on the Blackboard is a key/value pair.
• Input ports can read an entry in the Blackboard, whilst an Output port can write into an entry.

Let’s add an input port to specify how long to wait before logging the message. Open src/delayed_message.cpp and add the following code:

BT::PortsList DelayedMessage::providedPorts()
{
  // delay_duration: Number of seconds to wait before logging a message
  return BT::PortsList(
      { BT::InputPort<double>("delay_duration", "5", "The duration, in seconds, to wait before logging a message.") });
}

The code above describes an input port named "delay_duration", with a default value of 5 and a description of the port. The default value and description arguments are optional, but highly recommended, as this information will appear in the UI for all users of the Behavior.

Since we will make use of the information on this port in both the onStart() and onRunning() methods, let’s add a member variable to store the value of delay_duration we read from the port. Add this additional variable below start_time_ in include/delayed_message/delayed_message.hpp:

class DelayedMessage : public moveit_studio::behaviors::SharedResourcesNode<BT::StatefulActionNode>
{
private:
  std::chrono::time_point<std::chrono::steady_clock> start_time_;
  int delay_duration_;

public:
...

We will make use of a helpful function called maybe_error() that will allow us to check if the input port was set incorrectly. Include the header that provides this function in include/delayed_message/delayed_message.hpp:

#include <chrono>

#include <behaviortree_cpp/action_node.h>

// This header includes the SharedResourcesNode type
#include <moveit_studio_behavior_interface/shared_resources_node.hpp>

// This header includes the MoveItErrorCodes
#include <moveit_msgs/msg/move_it_error_codes.hpp>

// This header includes the maybe_error function
#include <moveit_studio_behavior_interface/check_for_error.hpp>

...

In the onStart() method in src/delayed_message.cpp, we can make sure the delay_duration exists and store it in delay_duration_. If the delay is less than or equal to zero, we can return SUCCESS immediately. Update it like so:

BT::NodeStatus DelayedMessage::onStart()
{
  // Store the time at which this node was first ticked
  start_time_ = std::chrono::steady_clock::now();

  // getInput returns a BT::Optional, so we'll store the result temporarily
  // while we check if it was set correctly
  const auto maybe_duration = getInput<int>("delay_duration");

  // The maybe_error function returns a std::optional with an error message if the port
  // was set incorrectly
  if (const auto error = moveit_studio::behaviors::maybe_error(maybe_duration))
  {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("DelayedMessage"),
                        "Failed to read input data port:\n" << error.value());
    return BT::NodeStatus::FAILURE;
  }

  // Store the value of the port
  delay_duration_ = maybe_duration.value();

  // If the duration is less than or equal to zero, we can log the message immediately
  if (delay_duration_ <= 0)
  {
    // Log the "Hello, world!" message.
    // Setting the third argument to false ensures the message will be shown immediately
    shared_resources_->logger->publishInfoMessage(name(), "Hello, world!", false);
    return BT::NodeStatus::SUCCESS;
  }

  return BT::NodeStatus::RUNNING;
}

Update the onRunning() method to use delay_duration_:

BT::NodeStatus DelayedMessage::onRunning()
{
  // If the delay duration has not elapsed since this node was started, return RUNNING
  if (std::chrono::duration_cast<std::chrono::seconds>(std::chrono::steady_clock::now()
    - start_time_).count() < delay_duration_)
  {
    return BT::NodeStatus::RUNNING;
  }
  else
  {
    // Log the "Hello, world!" message.
    // Setting the third argument to false ensures the message will be shown immediately
    shared_resources_->logger->publishInfoMessage(name(), "Hello, world!", false);
    return BT::NodeStatus::SUCCESS;
  }
}

3. Add Tests to Your Behavior

Now that we’ve expanded the functionality of the Delayed Message Behavior, we should increase our test coverage too. Add the following tests to your test/test_behavior_plugins.cpp:

/**
* @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 port remapping rules so the keys on the blackboard
  // are the same as the keys used by the Behavior
  config.input_ports.insert(std::make_pair("delay_duration", "="));
  // Instantiate the Behavior
  auto delayed_message_no_port = factory.instantiateTreeNode("delayed_message_no_port",
                                                             "DelayedMessage", config);

  // 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 input port with a string instead of a number
  config.blackboard->set("delay_duration", "five seconds");

  // 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");

  // The DelayedMessage Behavior should immediately return SUCCESS if the delay_duration
  // is negative.
  ASSERT_EQ(delayed_message_negative->executeTick(), BT::NodeStatus::SUCCESS);

  // 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");

  // The DelayedMessage Behavior should succeed immediately.
  ASSERT_EQ(delayed_message_zero->executeTick(), BT::NodeStatus::SUCCESS);

  // Instantiate a new Behavior
  auto delayed_message_double = factory.instantiateTreeNode("delayed_message_double",
                                                              "DelayedMessage", config);

  // Set the input port with a double
  config.blackboard->set("delay_duration", "5.6");

  // The DelayedMessage Behavior will convert the incorrect datatypes if possible.
  ASSERT_EQ(delayed_message_double->executeTick(), BT::NodeStatus::RUNNING);

  std::this_thread::sleep_for(std::chrono::milliseconds(5500)); // sleep for 5.5 seconds

  // After a total of 5.5 seconds, the node should return SUCCESS.
  ASSERT_EQ(delayed_message_double->executeTick(), BT::NodeStatus::SUCCESS);
}

Since the Behavior now requires a value on the input port, make sure it is set in your test_behavior_plugins test. Modify it to match the code below:

/**
* @brief This test makes sure that the Behavior in this package waits the appropriate amount
* of time before returning SUCCESS
*/
TEST(BehaviorTests, test_behavior_plugins)
{
  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 port remapping rules so the keys on the blackboard
  // are the same as the keys used by the Behavior
  config.input_ports.insert(std::make_pair("delay_duration", "="));
  // Instantiate the Behavior
  auto delayed_message_behavior = factory.instantiateTreeNode("test_behavior_name",
                                                              "DelayedMessage", config);

  // Set the input port with a value of five
  config.blackboard->set("delay_duration", "5");

  // 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);

  std::this_thread::sleep_for(std::chrono::milliseconds(5000)); // sleep for 5 seconds

  // After a total of 6 seconds, the node should return SUCCESS.
  ASSERT_EQ(delayed_message_behavior->executeTick(), BT::NodeStatus::SUCCESS);
}

4. Run the Updated Tests

It is left as an exercise to the reader to add additional tests that verify the expected Behavior. For example, changes can be made to the onStart() method to return FAILURE if the delay is set to a negative number. Such a change would require updating the tests to ensure the expected result of delayed_message_behavior->executeTick() given a negative delay duration is FAILURE.

Once your tests appropriately cover your Behavior’s expectations, you can build your workspace and run the updated tests using:

./moveit_pro test

5. Use The New Behavior

Now that you’ve ensured your Behavior works correctly, you’ll need to restart MoveIt Pro so that it can discover your new Behavior. You can stop MoveIt Pro by navigating to the location of your docker-compose.yaml and entering:

./moveit_pro down

Once MoveIt Pro stops and the command prompt returns, you can restart it with:

./moveit_pro run

You can now use your new Behavior when building Objectives. You will notice that your Behavior now has a text field where you can specify the delay_duration value when editing an Objective.