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.
getWaitForMessageTimeout
is an optional function used to set the timeout used when waiting for a message to be received on the topic. This is left as default in this example (no timeout).getFuture
must be implemented for all classes derived fromAsyncBehaviorBase
. It returns ashared_future
class member.
The API documentation for the AsyncBehaviorBase
class is here.
Implementing the GetStringFromTopic
Behavior
Custom Behaviors in MoveIt Pro 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 Pro Docker container on startup, which by default is located at ~/moveit_pro/moveit_studio_ur_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>
using moveit_studio::behaviors::GetMessageFromTopicBehaviorBase;
using moveit_studio::behaviors::BehaviorContext;
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);
/** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */
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 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 example_behaviors
Next, create a source file at example_behaviors/src/get_string_from_topic.cpp
and add this code to that file:
#include <example_behaviors/get_string_from_topic.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)
{
}
BT::PortsList GetStringFromTopic::providedPorts()
{
// This node has one input port and one output port
return BT::PortsList({
BT::InputPort<std::string>("topic_name", "/chatter", "The name of the topic the Behavior subscribes to."),
BT::OutputPort<std::vector<int>>("message_out", "{my_string}", "The output String message."),
});
}
BT::KeyValueVector GetStringFromTopic::metadata()
{
return { { "subcategory", "Example" },
{ "description", "Captures a string message and makes it available on an output port." } };
}
} // 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 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/get_string_from_topic.cpp
)
Add a line to include your new Behavior 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/get_string_from_topic.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<GetStringFromTopic>(factory, "GetStringFromTopic", shared_resources);
}