Creating a Behavior to Publish a Message to a Topic

This example demonstrates how to write a Behavior that publishes ROS messages to a topic. This example will publish a std_msgs::msg::ColorRGBA to a topic. If you need a Behavior that publishes a std_msgs::msg::String to a topic, check out the PublishString Behavior that's included in MoveIt Pro!

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 PublishColorRGBA Behavior

Custom Behaviors in MoveIt Pro 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 Pro Docker container on startup, which by default is located at ~/moveit_pro/moveit_pro_example_ws.

To create a new PublishColorRGBA Behavior, first create a header file in the example_behaviors package at example_behaviors/include/example_behaviors/publish_color_rgba.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/color_rgba.hpp>

#include <memory>
#include <string>

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

  static BT::PortsList providedPorts();

  static BT::KeyValueVector metadata();

  BT::NodeStatus tick() override;

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

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

#include <example_behaviors/publish_color_rgba.hpp>

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

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

BT::KeyValueVector PublishColorRGBA::metadata()
{
  return { { "subcategory", "Example" },
           { "description", "Publishes a fixed std_msgs::msg::ColorRGBA message to a topic named \"/my_topic\"" } };
}

BT::NodeStatus PublishColorRGBA::tick()
{
  std_msgs::msg::ColorRGBA color_msg;
  color_msg.r = 0.5;
  color_msg.g = 0.5;
  color_msg.b = 0.5;
  color_msg.a = 0.5;
  publisher_->publish(color_msg);

  return BT::NodeStatus::SUCCESS;
}
}  // namespace example_behaviors

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

When the PublishColorRGBA Behavior is run within an Objective, each time it is ticked it will publish the message to the "/my_topic" topic.

After that, add the new source file to the example_behaviors CMake library target in CMakeLists.txt following the same pattern used by the other Behaviors. It should look something like this:

add_library(
  example_behaviors SHARED
  src/hello_world.cpp
  src/delayed_message.cpp
  src/setup_mtc_wave_hand.cpp
  src/register_behaviors.cpp
  src/publish_color_rgba.cpp
)

Add a line to include your new Behavior for registration using the same pattern as the lines for the other example Behaviors.

#include <example_behaviors/hello_world.hpp>
#include <example_behaviors/delayed_message.hpp>
#include <example_behaviors/setup_mtc_wave_hand.hpp>
#include <example_behaviors/publish_color_rgba.hpp>

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.

void registerBehaviors(BT::BehaviorTreeFactory& factory,
                   const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources) override
{
  // Other Behaviors registered above
  moveit_studio::behaviors::registerBehavior<PublishColorRGBA>(factory, "PublishColorRGBA", shared_resources);

}