Create 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 Studio Example Behaviors repository to your workspace’s src directory. An example site configuration with Objectives that make use of these Behaviors can be found in the moveit_studio_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 MoveIt Studio 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

Note

The files created by the moveit_studio container are owned by root. You can change ownership of the delayed_message package using sudo chown -R $USER:$USER STUDIO_HOST_USER_WORKSPACE/src/delayed_message.

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. 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);
}

5. Build the Behavior and Run the Tests

MoveIt Studio will build and test your workspace using the workspace_test service defined in docker-compose.yaml. It will use the moveit_studio_workspace_builder service to build your workspace (which you can run independently of workspace_test using docker compose up workspace_test) and then run colcon test within it. Run the following command in the directory that contains your docker-compose.yaml file to build and test the code we just added:

./moveit_studio test_workspace

Note

The container has a /root/.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 saying hello
  return BT::PortsList({
    BT::InputPort<int>("delay_duration")
  });
}

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); error)
  {
    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;
  }
}

2. Give The Input Port a Default Value and Description

In order for the MoveIt Studio agent to get the information it needs about the DelayedMessage Behavior when making it available for creating Objectives, it reads from the Behavior’s tree_nodes_model.xml file. Open config/tree_nodes_model.xml and add an input_port tag to the DelayedMessage Behavior so that it looks like this:

<root>
    <TreeNodesModel>
        <Action ID="DelayedMessage">
            <description>
                <p>
                    After some time, log a message that says "Hello, world!".
                </p>
            </description>
            <input_port name="delay_duration" default="5">
                The duration, in seconds, to wait before logging a message
            </input_port>
        </Action>
    </TreeNodesModel>
</root>

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_studio test_workspace

5. Use The New Behavior

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

./moveit_studio down

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

./moveit_studio 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.