Creating a Behavior to Call an Action
This tutorial shows how to create a custom Behavior that creates an action goal request, process feedback, and process results. The feedback and results are logged and made available via output data ports.
We will achieve this by specializing the ActionClientBehaviorBase
class for the action type of our choice.
For the purposes of this tutorial, we will target the example example_interfaces::action::Fibonacci
service and create a FibonacciActionClient
Behavior.
An example action server using Fibonacci
is available as part of the ROS 2 examples.
Technical Background
From a C++ development perspective, we will create a new class that inherits from the ActionClientBehaviorBase
class and specializes it for a particular message type.
The API documentation for the ActionClientBehaviorBase
class is here.
The ActionClientBehaviorBase
class itself inherits from AsyncBehaviorBase
, which has some virtual functions which must be implemented in the new class as well.
getActionName
is used to get the name of the action when initializing the action client.createGoal
is used to function to create the action goal. In this example, we will make the goal from data specified from an input port.getResultTimeout
is an optional function used to set the timeout used when waiting for the action result. This is left as default in this example (no timeout).processResult
is used to process the result from the action server. In this example, we will log the result and make it available on an output port.processFeedback
is used to process the feedback from the action server. In this example, we will log the feedback and make it available on an output port.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 FibonacciActionClient
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/fibonacci_action_client.hpp
and add this code to that file:
#pragma once
#include <moveit_studio_behavior_interface/action_client_behavior_base.hpp>
#include <example_interfaces/action/fibonacci.hpp>
using moveit_studio::behaviors::BehaviorContext;
using moveit_studio::behaviors::ActionClientBehaviorBase;
using Fibonacci = example_interfaces::action::Fibonacci;
namespace example_behaviors
{
class FibonacciActionClient final : public ActionClientBehaviorBase<Fibonacci>
{
public:
FibonacciActionClient(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 User-provided function to get the name of the action when initializing the action client. */
tl::expected<std::string, std::string> getActionName() override;
/** @brief User-provided function to create the action goal before sending the action goal request. */
tl::expected<Fibonacci::Goal, std::string> createGoal() override;
/** @brief Optional user-provided function to process the action result after the action has finished. */
tl::expected<bool, std::string> processResult(const std::shared_ptr<Fibonacci::Result> result) override;
/** @brief Optional user-provided function to process feedback sent by the action server. */
void processFeedback(const std::shared_ptr<const Fibonacci::Feedback> feedback) 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 example_behaviors
Next, create a source file at example_behaviors/src/fibonacci_action_client.cpp
and add this code to that file:
#include <example_behaviors/fibonacci_action_client.hpp>
// Include the template implementation for ActionClientBehaviorBase<T>.
#include <moveit_studio_behavior_interface/impl/action_client_behavior_base_impl.hpp>
namespace example_behaviors
{
FibonacciActionClient::FibonacciActionClient(
const std::string& name, const BT::NodeConfiguration& config,
const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources)
: ActionClientBehaviorBase<Fibonacci>(name, config, shared_resources)
{
}
BT::PortsList FibonacciActionClient::providedPorts()
{
// This node has two input ports and two output port
return BT::PortsList({
BT::InputPort<std::string>("service_name", "/add_two_ints", "The name of the service to call."),
BT::InputPort<int>("addend1", "The first int to add to the other."),
BT::InputPort<int>("addend2", "The second int to add to the other."),
BT::OutputPort<int>("result", "{result}", "Result of the AddTwoInts service."),
});
}
BT::KeyValueVector FibonacciActionClient::metadata()
{
return { { "subcategory", "Example" },
{ "description",
"Calls an action to get a Fibonacci sequence and makes the result available on an output port." } };
}
tl::expected<std::string, std::string> FibonacciActionClient::getActionName()
{
const auto action_name = getInput<std::string>("action_name");
if (const auto error = moveit_studio::behaviors::maybe_error(action_name))
{
return tl::make_unexpected("Failed to get required value from input data port: " + error.value());
}
return action_name.value();
}
tl::expected<Fibonacci::Goal, std::string> FibonacciActionClient::createGoal()
{
const auto order = getInput<std::size_t>("order");
if (const auto error = moveit_studio::behaviors::maybe_error(order))
{
return tl::make_unexpected("Failed to get required value from input data port: " + error.value());
}
return example_interfaces::build<Fibonacci::Goal>().order(order.value());
}
tl::expected<bool, std::string> FibonacciActionClient::processResult(
const std::shared_ptr<Fibonacci::Result> result)
{
std::stringstream stream;
for (const auto& value : result->sequence)
{
stream << value << " ";
}
std::cout << stream.str() << std::endl;
// Publish the result to the UI
shared_resources_->logger->publishInfoMessage(name(), "Result: " + stream.str(), false);
setOutput<std::vector<int>>("result", result->sequence);
return { true };
}
void FibonacciActionClient::processFeedback(
const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
std::stringstream stream;
for (const auto& value : feedback->sequence)
{
stream << value << " ";
}
std::cout << stream.str() << std::endl;
// Publish the feedback to the UI
shared_resources_->logger->publishInfoMessage(name(), "Feedback: " + stream.str(), false);
setOutput<std::vector<int>>("feedback", feedback->sequence);
}
} // namespace example_behaviors
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/fibonacci_action_client.cpp
)
Next, add a dependency for the action definition by adding the requisite package.
The Fibonacci
action definition is provided by the example_interfaces
package.
Add this package to THIS_PACKAGE_INCLUDES_DEPENDS
along with the other dependencies.
set(
THIS_PACKAGE_INCLUDE_DEPENDS
moveit_studio_behavior_interface
pluginlib
example_interfaces
)
Also, add the example_interfaces
package as a dependency in the package.xml
alongside the other dependencies.
<depend>moveit_studio_behavior_interface</depend>
<depend>example_interfaces</depend>
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/fibonacci_action_client.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<FibonacciActionClient>(factory, "FibonacciActionClient", shared_resources);
}
This action client sends a request to the fibonacci
action server.
The action name, ports, request construction, and results handling can be modified for your custom action.
For the purposes of this example, if you’d like to test the service client you just created, you can use the fibonacci
action server implementation from the ROS 2 examples.
This can be installed and run like this if you have ROS installed on the host and configured to use the same DDS configuration as MoveIt Pro (see Configuring DDS for MoveIt Pro):
sudo apt install ros-humble-examples-rclcpp-minimal-action-server
ros2 run examples_rclcpp_minimal_action_server action_server_not_composable
This example Behavior will log the action feedback and result to the UI at the INFO
level.
INFO
level logs are not shown by default.
To see these logs, click the bell icon in the top-right corner of the UI and select the Info
checkbox.
Now, when this Behavior is run and feedback or results are received, the data will be shown in the UI.