Version: 6

Creating an Asynchronous Behavior

Writing the Behavior

Tutorial Level: Advanced

In the create-a-stateful-behavior tutorial, we wrote a Behavior that will return RUNNING for a certain amount of time before its state changes to returning SUCCESS when ticked. In this tutorial, we will write a Behavior that is tasked with doing some work while it is RUNNING before it can return SUCCESS. MTC (MoveIt Task Constructor) can solve challenging motion plans, some of which may require some time to create. For this reason, we will build our Behavior on top of the AsyncBehaviorBase class. AsyncBehaviorBase is a specialized class inheriting from StatefulActionNode that uses a std::async thread to perform long running operations.

This tutorial assumes you have completed the Create a Custom Behavior tutorial, which created the hello_world Behavior package. To use a different Behavior package, replace the usage of hello_world with your package name.

note

If you need the hello_world Behavior package code, but don't want to go through the Create a Custom Behavior tutorial, you can clone the MoveIt Pro Example Behaviors repository into your STUDIO_HOST_USER_WORKSPACE/src/ directory. The moveit_studio_example_behaviors repository also contains the completed setup_mtc_wave_hand files that are covered in this tutorial. If you are using the moveit_studio_example_behaviors repository but still want to go through the steps outlined in this tutorial, you can remove the setup_mtc_wave_hand files and re-add them as you complete the tutorial steps below. If you'd like to skip this tutorial and make use of the finished Behaviors in moveit_studio_example_behaviors right away, proceed to the Create an Objective Using a Custom Behavior tutorial.

1. Adding a New Behavior to an Existing Package

The new Behavior will be named SetupMTCWaveHand, and as its name suggests, it will contain code to set up an MTC task that makes the robot wave its hand.

Add the `setup_mtc_wave_hand.hpp` header file

Go to the directory that contains the hello_world package and create a new file at hello_world/include/hello_world/setup_mtc_wave_hand.hpp:

cd $STUDIO_HOST_USER_WORKSPACE
touch src/hello_world/include/hello_world/setup_mtc_wave_hand.hpp

Open this new file in your favorite text editor or IDE and copy in the code shown below. The comments are intentionally very detailed to explain the purpose of each part of this Behavior.

#pragma once

#include <behaviortree_cpp/action_node.h>
#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
#include <moveit_studio_behavior_interface/behavior_context.hpp>
#include <moveit_studio_behavior_interface/check_for_error.hpp>
#include <moveit_studio_behavior_interface/async_behavior_base.hpp>
#include <tl_expected/expected.hpp>

namespace hello_world
{
/**
* @brief The SetupMTCWaveHand Behavior makes any robot move the end of its arm back and forth.
* @details This is an example of a Behavior that uses MoveIt Task Constructor to configure an MTC task,
* which can be planned and executed by MoveIt Pro's core PlanMTCTask and ExecuteMTCTask Behaviors.
* It is derived from AsyncBehaviorBase, an extension of the templated SharedResourcesNode type,
* which augments the core BehaviorTree.Cpp types with an additional constructor parameter
* to allow the Behavior to access a rclcpp Node owned by the Agent's ObjectiveServerNode.
* The AsyncBehaviorBase requires a user-implemented function doWork()
* which is called within an async process in a separate thread.
* It returns a tl::expected which contains a bool indicating task success
* if the process completed successfully or was canceled,
* or an error message if the process failed unexpectedly.
*/
class SetupMTCWaveHand final : public moveit_studio::behaviors::AsyncBehaviorBase
{
public:
  /**
  * @brief Constructor for the SetupMTCWaveHand Behavior.
  * @param name The name of a particular instance of this Behavior. This will be set by the Behavior
  * tree factory when this Behavior is created within a new Behavior tree.
  * @param config This contains runtime configuration info for this Behavior, such as the mapping
  * between the Behavior's data ports on the Behavior tree's blackboard. This will be set by the
  * Behavior tree factory when this Behavior is created within a new Behavior tree.
  * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all
  * SharedResourcesNode Behaviors in the Behavior tree. This BehaviorContext is owned by the MoveIt Pro
  * Agent's ObjectiveServerNode.
  */
  SetupMTCWaveHand(const std::string& name, const BT::NodeConfiguration& config,
                  const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);

  /**
  * @brief Implementation of the required providedPorts() function for the SetupMTCWaveHand Behavior.
  * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function
  * named providedPorts() which defines their input and output ports. If the Behavior does not use
  * any ports, this function must return an empty BT::PortsList.
  * This function returns a list of ports with their names and port info, which is used internally
  * by the Behavior tree.
  * @return SetupMTCWaveHand has one data port: a bidirectional port named "task", which is a shared_ptr
  * to an MTC task object. This function returns a BT::PortsList that declares this single port.
  */
  static BT::PortsList providedPorts();

  /**
  * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
  * subcategory, in the MoveIt Studio Developer Tool.
  * @return A BT::KeyValueVector containing the Behavior metadata.
  */
  static BT::KeyValueVector metadata();

private:
  /**
  * @brief Async thread for SetupMTCWaveHand. Adds MTC stages to an MTC task provided on a data port.
  * @details This function is where the Behavior performs its work asynchronously while the Behavior tree ticks.
  * It is very important that Behaviors return from being ticked very quickly because if it blocks before returning
  * it will block execution of the entire Behavior tree, which may have undesirable consequences for other Behaviors
  * that require a fast update rate to work correctly.
  * @return A tl::expected which contains a true if the MTC stages were configured and added to the MTC task,
  * or an error if it failed to retrieve the MTC task from the "task" data port.
  */
  tl::expected<bool, std::string> doWork() override;

  /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
  std::shared_future<tl::expected<bool, std::string>>& getFuture() override
  {
    return future_;
  }

  /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
  std::shared_future<tl::expected<bool, std::string>> future_;
};
}  // namespace hello_world

Add the `setup_mtc_wave_hand.cpp` source file

From the directory that contains the hello_world package, create a new file at hello_world/src/setup_mtc_wave_hand.cpp:

cd $STUDIO_HOST_USER_WORKSPACE
touch src/hello_world/src/setup_mtc_wave_hand.cpp

Paste the code shown below into this file:

#include <hello_world/setup_mtc_wave_hand.hpp>

#include <moveit/task_constructor/solvers.h>
#include <moveit/task_constructor/stages.h>
#include <moveit/task_constructor/task.h>
#include <moveit_studio_behavior/utils/mtc_cartesian_planner.hpp>
#include <spdlog/spdlog.h>

namespace
{
// Define a constant for the ID of the Behavior's data port.
constexpr auto kPortIDTask = "task";

// Define constants for the names of the Behavior parameters used by the SetupMTCWaveHand Behavior.
// These defaults are set to match the UR5e robot defined in the example packages.
constexpr auto kPrimaryGroupName = "manipulator";
constexpr auto kHandFrameName = "grasp_link";

// Create a rclcpp Logger to use when printing messages to the console.
const rclcpp::Logger kLogger = rclcpp::get_logger("WaveHello");
}

namespace hello_world
{
SetupMTCWaveHand::SetupMTCWaveHand(
    const std::string& name, const BT::NodeConfiguration& config,
    const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources)
  : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources)
{
}

BT::PortsList SetupMTCWaveHand::providedPorts()
{
  return {
    BT::BidirectionalPort<std::shared_ptr<moveit::task_constructor::Task>>(kPortIDTask, "{mtc_task}",
                                                                          "MoveIt Task Constructor task."),
  };
}

BT::KeyValueVector SetupMTCWaveHand::metadata()
{
  return { { "subcategory", "Example" }, { "description", "Wave hello with the end effector." } };
}

tl::expected<bool, std::string> SetupMTCWaveHand::doWork()
{
  // Retrieve the "task" data port
  const auto task = getInput<moveit::task_constructor::TaskPtr>(kPortIDTask);

  // Check that all required input data ports were set
  if (const auto error = moveit_studio::behaviors::maybe_error(task))
  {
spdlog::error("Failed to get required values from input data ports:\n{}", error.value());
    // Task setup cannot succeed if we failed to retrieve the MTC task shared_ptr from the "task" port, so we return false.
    return tl::make_unexpected("Failed to get required values from input data ports: " + error.value());
  }

  // Create a Cartesian path planner used to perform linear moves relative to the end effector frame.
  auto cartesian_planner = std::make_shared<moveit_studio::behaviors::mtc_solvers::MTCCartesianPlanner>();

  // Create an MTC stage to define a motion that translates along the end effector X+ axis. Assumes that the robot is not already at its joint limits.

  // Set the direction to move along as a TwistStamped.
  geometry_msgs::msg::TwistStamped wave_first_direction;
  wave_first_direction.header.frame_id = kHandFrameName;
  wave_first_direction.twist.linear.x = 1.0;

  // Create a new MTC stage
  {
    // Create a new MoveRelative stage that uses the Cartesian motion planner
    auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>(
        std::string("Wave One Direction"), cartesian_planner);
    stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
    // Configure the stage to move the UR-5e's arm.
    stage->setGroup(kPrimaryGroupName);
    // Configure the stage to move relative to the UR-5e's gripper frame.
    stage->setIKFrame(kHandFrameName);
    // Configure the stage to move along the direction specified by the TwistStamped.
    stage->setDirection(wave_first_direction);
    // Set that we will move at most 0.2m along the vector.
    stage->setMaxDistance(0.2);
    // Add the stage to the MTC task which was retrieved from the "task" data port.
    task.value()->add(std::move(stage));
  }

  // Create a second MTC stage to define a motion that translates along the end effector X- axis, which is the opposite motion from what we did in the previous stage.

  // Set the direction to move along as a TwistStamped.
  geometry_msgs::msg::TwistStamped wave_second_direction;
  wave_second_direction.header.frame_id = kHandFrameName;
  wave_second_direction.twist.linear.x = -1.0;

  {
    // Create a new MoveRelative stage that uses the Cartesian motion planner
    auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>(
        std::string("Wave Opposite Direction"), cartesian_planner);
    stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
    // Configure the stage to move the UR-5e's arm.
    stage->setGroup(kPrimaryGroupName);
    // Configure the stage to move relative to the UR-5e's gripper frame.
    stage->setIKFrame(kHandFrameName);
    // Configure the stage to move along the direction specified by the TwistStamped.
    stage->setDirection(wave_second_direction);
    // Set that we will move at most 0.2m along the vector.
    stage->setMaxDistance(0.2);
    // Add the stage to the MTC task which was retrieved from the "task" data port.
    task.value()->add(std::move(stage));
  }

  //Once the work is done, we can return true so that the node returns SUCCESS next time it is ticked
  return true;
}

}  // namespace hello_world

Edit the package CMakeLists.txt

Open hello_world/CMakeLists.txt in your text editor or IDE.

The only change needed here is a modification to the add_library function to add the new setup_mtc_wave_hand.cpp source file:

add_library(hello_world SHARED
  src/hello_world.cpp
  src/setup_mtc_wave_hand.cpp
  src/register_behaviors.cpp)

Edit the Behavior loader plugin

Open hello_world/src/register_behaviors.cpp in your text editor or IDE.

Add a line to include the hello_world/setup_mtc_wave_hand.hpp header file.

Then, modify the implementation of the registerBehaviors function to add the line to register the new SetupMTCWaveHand Behavior:

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

#include <pluginlib/class_list_macros.hpp>

namespace hello_world
{
class HelloWorldBehaviorsLoader : 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", shared_resources);
    moveit_studio::behaviors::registerBehavior<SetupMTCWaveHand>(factory, "SetupMTCWaveHand", shared_resources);

  }
};
}  // namespace hello_world

PLUGINLIB_EXPORT_CLASS(hello_world::HelloWorldBehaviorsLoader,
                       moveit_studio::behaviors::SharedResourcesNodeLoaderBase);

Edit the package unit tests

Modify hello_world/test/test_behavior_plugins.cpp to add a line that attempts to instantiate the new SetupMTCWaveHand Behavior:

TEST(RobotTask, load_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("hello_world::MyBehaviorBehaviorsLoader");
    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.
  EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "HelloWorld", BT::NodeConfiguration()));

  EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "SetupMTCWaveHand", BT::NodeConfiguration()));

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

2. Build the Package

If you are not introducing new dependencies, you only need to build the new Behavior package:

moveit_pro build user_workspace

To install new dependencies in your MoveIt Pro Docker image, rebuild the Docker overlay and Colcon workspace using:

moveit_pro build

Your workspace will now be built. If successful, you will be able to use the new Behavior after restarting MoveIt Pro.

3. Run the Package Unit Tests

Run unit tests for your workspace:

moveit_pro test

The tests should succeed, showing that the plugin configuration is correct and our new SetupMTCWaveHand Behavior can be instantiated by the Behavior tree factory.

4. Shutdown and Relaunch the Agent

Run moveit_pro down from the directory containing docker-compose.yaml to shut down MoveIt Pro.

Once MoveIt Pro has been shut down, run moveit_pro run to bring MoveIt Pro back up.

Next Steps

Now that our SetupMTCWaveHand Behavior has been created and tested, we can use it in an Objective in the Create an Objective Using a Custom Behavior tutorial.

Creating an Asynchronous Behavior

Writing the Behavior​

1. Adding a New Behavior to an Existing Package​

Add the setup_mtc_wave_hand.hpp header file​

Add the setup_mtc_wave_hand.cpp source file​

Edit the package CMakeLists.txt​

Edit the Behavior loader plugin​

Edit the package unit tests​

2. Build the Package​

3. Run the Package Unit Tests​

4. Shutdown and Relaunch the Agent​

Next Steps​