Creating a Behavior to Get the Latest Message From a Topic

This tutorial shows how to create a custom Behavior that subscribes to a topic, waits until it receives a message on that topic, and then sets the message it received as an output data port.

We will achieve this by specializing the GetMessageFromTopicBehaviorBase class for the message type of our choice.

For the purposes of this tutorial, we will target the built-in std_msgs::msg::String message and create a GetStringFromTopic Behavior.

Technical Background

From a C++ development perspective, we will create a new class that inherits from the GetMessageFromTopicBehaviorBase class and specializes it for a particular message type.

The API documentation for the GetMessageFromTopicBehaviorBase class is here.

The GetMessageFromTopicBehaviorBase class itself inherits from AsyncBehaviorBase, which has some virtual functions which must be implemented in the new class as well.

The API documentation for the AsyncBehaviorBase class is here.

Implementing the GetStringFromTopic Behavior

Custom Behaviors in MoveIt Studio are built within ROS packages, so the new Behavior must be either created within a new package or added to an existing package.

This public repo contains some demo Behaviors to show an example of this.

The package containing the new Behavior must be within the workspace which is loaded by the MoveIt Studio Docker container on startup, which by default is located at ~/moveit_studio/moveit_studio_ws.

Within the example_behaviors package, first create a header file at example_behaviors/include/example_behaviors/get_string_from_topic.hpp and add this code to that file:

#pragma once

#include <moveit_studio_behavior_interface/get_message_from_topic.hpp>
#include <std_msgs/msg/string.hpp>

namespace example_behaviors
{
class GetStringFromTopic final : public GetMessageFromTopicBehaviorBase<std_msgs::msg::String>
{
public:
  GetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config,
            const std::shared_ptr<BehaviorContext>& shared_resources);

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

  /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
  std::shared_future<fp::Result<bool>> future_;
};
}  // namespace example_behaviors

Next, create a source file at example_behaviors/src/get_string_from_topic.cpp and add this code to that file:

#include <moveit_studio_behavior_interface/test/mockup/get_string.hpp>

// Include the template implementation for GetMessageFromTopicBehaviorBase<T>.
#include <moveit_studio_behavior_interface/impl/get_message_from_topic_impl.hpp>

namespace example_behaviors
{
GetStringFromTopic::GetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config,
                    const std::shared_ptr<BehaviorContext>& shared_resources)
  : GetMessageFromTopicBehaviorBase<std_msgs::msg::String>(name, config, shared_resources)
{
}
}  // namespace example_behaviors

// Template specialization of GetMessageFromTopicBehaviorBase<T> for the String message type
template class moveit_studio::behaviors::GetMessageFromTopicBehaviorBase<std_msgs::msg::String>;

After that, add the new source file to the example_behaviors CMake library target following the same pattern used by the other Behaviors.

Then, add a line to register the new Behavior with the package’s Behavior loader plugin, following the same pattern as the lines for the other example Behaviors.

Finally, add a new entry to the tree_nodes_model.xml file, which tells the MoveIt Studio Objective Editor UI about how to categorize the Behavior and which input and output ports it has.

The XML entry for the GetStringFromTopic behavior would be this:

<Action ID="GetStringFromTopic">
    <description>
        <p>
            Captures a String and makes it available on an output port.
        </p>
    </description>
    <input_port name="topic_name" default="/chatter">The name of the topic the behavior subscribes to.</input_port>
    <output_port name="message_out" default="{my_string}">Output String message.</output_port>
</Action>