Creating a Behavior to Publish a Message to a Topic

This example demonstrates how to publish ROS messages to a topic by implementing a PublishString Behavior.

Technical Background

Behaviors that inherit from the SharedResourcesNode class can access an instance of rclcpp::Node which is managed by the Objective Server through the shared_resources_ class member.

This node can be used to interact with the ROS 2 middleware, for example by creating a Publisher that publishes message to a topic.

Behaviors which use ROS 2 Nodes should use this shared node instead of creating and independently spinning their own nodes. This is to avoid runtime deadlocks which can be caused by having multiple nodes and executors owned by the same process.

Implementing the PublishString Behavior

Custom Behaviors in MoveIt Studio are built within ROS packages, so to add a new behavior we will need to create a new package.

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

This package needs to 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.

To create a new PublishString Behavior, first create a header file in the example_behaviors package at example_behaviors/include/example_behaviors/publish_string.hpp and add this code to that file:

#pragma once

#include <behaviortree_cpp/action_node.h>
#include <behaviortree_cpp/bt_factory.h>
#include <moveit_studio_behavior_interface/behavior_context.hpp>
#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
#include <rclcpp/publisher.hpp>
#include <std_msgs/msg/string.hpp>

#include <memory>
#include <string>

namespace example_behaviors
{
class PublishString final : public SharedResourcesNode<BT::SyncActionNode>
{
public:
  PublishString(const std::string& name, const BT::NodeConfiguration& config,
              const std::shared_ptr<BehaviorContext>& shared_resources);

  static BT::PortsList providedPorts();

  BT::NodeStatus tick() override;

private:
  std::shared_ptr<rclcpp::Publisher<std_msgs::msg::String>> publisher_;
};
}  // namespace example_behaviors

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

#include <example_behaviors/publish_string.hpp>

namespace example_behaviors
{
PublishString::PublishString(const std::string& name, const BT::NodeConfiguration& config,
                          const std::shared_ptr<BehaviorContext>& shared_resources)
  : SharedResourcesNode<BT::SyncActionNode>(name, config, shared_resources)
  , publisher_{ shared_resources_->node->create_publisher<std_msgs::msg::String>("/my_topic", rclcpp::QoS(1)) }
{
}

BT::PortsList PublishString::providedPorts()
{
  return {};
}

BT::NodeStatus PublishString::tick()
{
  publisher_->publish(std_msgs::build<std_msgs::msg::String>().data(
      "This is the message that will be published when the Behavior is ticked."));
  return BT::NodeStatus::SUCCESS;
}
}  // namespace example_behaviors

The publisher is created in the constructor of the PublishString Behavior using the Node provided through shared_resources_.

When the PublishString Behavior is run within a behavior tree, each time it is ticked it will publish the message to the "/my_topic" topic.

After creating the source code, 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 PublishString behavior would be this:

<Action ID="GetStringFromTopic">
    <description>
        <p>
            Publishes a fixed std_msgs::msg::String message to a topic named "/my_topic".
        </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>