Goal: Implement action server and client in C++.
Tutorial level: Intermediate
Time: 15 minutes
content
background
Actions are a form of asynchronous communication in ROS. The action client sends a target request to the action server . The action server sends target feedback and results to the action client .
prerequisites
You will need the package and interface defined in the previous tutorial's create action .action_tutorials_interfaces
Fibonacci.action
Task
1 Create action_tutorials_cpp package
As we saw in the Create your first ROS 2 package tutorial, we need to create a new package to hold our C++ and supporting code.
1.1 Create action_tutorials_cpp package
Go into the operations workspace you created in the previous tutorial (remember to provide a source for the workspace) and create a new package for the C++ operations server:
cd ~/action_ws/src ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
1.2 Add visibility control
In order for the package to compile and work on Windows, we need to add some "visibility controls". Details on why this is required can be found here .
Open it action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h
and put the following code:
#ifndef ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
#define ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
#ifdef __cplusplus
extern "C"
{
#endif
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((dllexport))
#define ACTION_TUTORIALS_CPP_IMPORT __attribute__ ((dllimport))
#else
#define ACTION_TUTORIALS_CPP_EXPORT __declspec(dllexport)
#define ACTION_TUTORIALS_CPP_IMPORT __declspec(dllimport)
#endif
#ifdef ACTION_TUTORIALS_CPP_BUILDING_DLL
#define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_EXPORT
#else
#define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_IMPORT
#endif
#define ACTION_TUTORIALS_CPP_PUBLIC_TYPE ACTION_TUTORIALS_CPP_PUBLIC
#define ACTION_TUTORIALS_CPP_LOCAL
#else
#define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((visibility("default")))
#define ACTION_TUTORIALS_CPP_IMPORT
#if __GNUC__ >= 4
#define ACTION_TUTORIALS_CPP_PUBLIC __attribute__ ((visibility("default")))
#define ACTION_TUTORIALS_CPP_LOCAL __attribute__ ((visibility("hidden")))
#else
#define ACTION_TUTORIALS_CPP_PUBLIC
#define ACTION_TUTORIALS_CPP_LOCAL
#endif
#define ACTION_TUTORIALS_CPP_PUBLIC_TYPE
#endif
#ifdef __cplusplus
}
#endif
#endif // ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
2. Writing an action server
Let's focus on writing an action server that calculates the Fibonacci sequence using the action we created in the Create Actions tutorial.
2.1 Write action server code
Open it action_tutorials_cpp/src/fibonacci_action_server.cpp
and put the following code:
#include <functional>
#include <memory>
#include <thread>
#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "action_tutorials_cpp/visibility_control.h"
namespace action_tutorials_cpp
{
class FibonacciActionServer : public rclcpp::Node
{
public:
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
ACTION_TUTORIALS_CPP_PUBLIC
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("fibonacci_action_server", options)
{
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
this->get_node_base_interface(),
this->get_node_clock_interface(),
this->get_node_logging_interface(),
this->get_node_waitables_interface(),
"fibonacci",
std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
std::bind(&FibonacciActionServer::handle_cancel, this, _1),
std::bind(&FibonacciActionServer::handle_accepted, this, _1));
}
private:
rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const Fibonacci::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
using namespace std::placeholders;
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
}
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
rclcpp::Rate loop_rate(1);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<Fibonacci::Feedback>();
auto & sequence = feedback->partial_sequence;
sequence.push_back(0);
sequence.push_back(1);
auto result = std::make_shared<Fibonacci::Result>();
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
// Check if there is a cancel request
if (goal_handle->is_canceling()) {
result->sequence = sequence;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
return;
}
// Update sequence
sequence.push_back(sequence[i] + sequence[i - 1]);
// Publish feedback
goal_handle->publish_feedback(feedback);
RCLCPP_INFO(this->get_logger(), "Publish feedback");
loop_rate.sleep();
}
// Check if goal is done
if (rclcpp::ok()) {
result->sequence = sequence;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
}
}
}; // class FibonacciActionServer
} // namespace action_tutorials_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer)
The first few lines contain all the header files we need to compile.
Next we create a class that is a derived class of rclcpp::Node
:
class FibonacciActionServer : public rclcpp::Node
The constructor of this class FibonacciActionServer
initializes the node name to fibonacci_action_server
:
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : Node("fibonacci_action_server", options)
The constructor also instantiates a new action server:
this->action_server_ = rclcpp_action::create_server<Fibonacci>( this->get_node_base_interface(), this->get_node_clock_interface(), this->get_node_logging_interface(), this->get_node_waitables_interface(), "fibonacci", std::bind(&FibonacciActionServer::handle_goal, this, _1, _2), std::bind(&FibonacciActionServer::handle_cancel, this, _1), std::bind(&FibonacciActionServer::handle_accepted, this, _1));
An action server requires 6 things:
-
Templated operation type name:
Fibonacci
. -
ROS 2 node to add the operation to:
this
. -
Action name:
'fibonacci'
. -
Callback function for processing the target:
handle_goal
-
Callback function to handle cancellation:
handle_cancel
. -
The callback function used to handle the target accept
handle_accept
:.
Next in the file is the implementation of various callbacks. Note that all callbacks need to return quickly, otherwise we might block the executor.
We start by handling the callback for the new target:
rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const Fibonacci::Goal> goal) { RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); (void)uuid; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }
This implementation only accepts all targets.
Next is the callback that handles cancellation:
rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr<GoalHandleFibonacci> goal_handle) { RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; }
This implementation simply tells the client that it accepted the cancellation.
The last callback accepts a new target and starts processing it:
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle) { using namespace std::placeholders; // this needs to return quickly to avoid blocking the executor, so spin up a new thread std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach(); }
Since execution is a long-running operation, we spawn a thread to do the actual work and handle_accepted
return quickly.
execute
All further processing and updates are done in the new thread's methods:
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle) { RCLCPP_INFO(this->get_logger(), "Executing goal"); rclcpp::Rate loop_rate(1); const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared<Fibonacci::Feedback>(); auto & sequence = feedback->partial_sequence; sequence.push_back(0); sequence.push_back(1); auto result = std::make_shared<Fibonacci::Result>(); for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { // Check if there is a cancel request if (goal_handle->is_canceling()) { result->sequence = sequence; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Goal canceled"); return; } // Update sequence sequence.push_back(sequence[i] + sequence[i - 1]); // Publish feedback goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), "Publish feedback"); loop_rate.sleep(); } // Check if goal is done if (rclcpp::ok()) { result->sequence = sequence; goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "Goal succeeded"); } }
This worker thread processes one Fibonacci sequence number per second, publishing a feedback update for each step. When it finishes processing, it marks goal_handle
success and exits.
We now have a fully functional action server. Let's build and run it.
2.2 Compile action server
In the previous section, we put the action server code in place. In order for it to compile and run, we need to do a few extra things.
First we need to set up CMakeLists.txt in order to compile the action server. Open action_tutorials_cpp/CMakeLists.txt
and add the following after calls find_package
:
add_library(action_server SHARED src/fibonacci_action_server.cpp) target_include_directories(action_server PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<INSTALL_INTERFACE:include>) target_compile_definitions(action_server PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL") ament_target_dependencies(action_server "action_tutorials_interfaces" "rclcpp" "rclcpp_action" "rclcpp_components") rclcpp_components_register_node(action_server PLUGIN "action_tutorials_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server) install(TARGETS action_server ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin)
Now we can compile the package. Go to the top level of action_ws
, and run:
colcon build
This should compile the entire workspace, including fibonacci_action_server
packages action_tutorials_cpp
.
2.3 Running the action server
Now that we have built the action server, we can run it. Get the workspace we just built ( action_ws
) and try running the action server:
ros2 run action_tutorials_cpp fibonacci_action_server
3. Write action client
3.1 Write action client code
Open it action_tutorials_cpp/src/fibonacci_action_client.cpp
and put the following code:
#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>
#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
namespace action_tutorials_cpp
{
class FibonacciActionClient : public rclcpp::Node
{
public:
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)
{
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
this->get_node_base_interface(),
this->get_node_graph_interface(),
this->get_node_logging_interface(),
this->get_node_waitables_interface(),
"fibonacci");
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&FibonacciActionClient::send_goal, this));
}
void send_goal()
{
using namespace std::placeholders;
this->timer_->cancel();
if (!this->client_ptr_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
rclcpp::shutdown();
}
auto goal_msg = Fibonacci::Goal();
goal_msg.order = 10;
RCLCPP_INFO(this->get_logger(), "Sending goal");
auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&FibonacciActionClient::result_callback, this, _1);
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
private:
rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
{
auto goal_handle = future.get();
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
} else {
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
}
}
void feedback_callback(
GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
std::stringstream ss;
ss << "Next number in sequence received: ";
for (auto number : feedback->partial_sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
}
void result_callback(const GoalHandleFibonacci::WrappedResult & result)
{
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
return;
}
std::stringstream ss;
ss << "Result received: ";
for (auto number : result.result->sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
rclcpp::shutdown();
}
}; // class FibonacciActionClient
} // namespace action_tutorials_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient)
The first few lines contain all the header files we need to compile.
Next we create a class that is a derived class of rclcpp::Node
:
class FibonacciActionClient : public rclcpp::Node
The constructor of this class FibonacciActionClient
initializes the node name to fibonacci_action_client
:
explicit FibonacciActionClient(const rclcpp::NodeOptions & options) : Node("fibonacci_action_client", options)
The constructor also instantiates a new action client:
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>( this->get_node_base_interface(), this->get_node_graph_interface(), this->get_node_logging_interface(), this->get_node_waitables_interface(), "fibonacci");
An action client requires three things:
-
Templated operation type name:
Fibonacci
. -
Add the action client to the ROS 2 node:
this
. -
Action name:
'fibonacci'
.
We also instantiate a ROS timer that will initiate one and only call send_goal
:
this->timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&FibonacciActionClient::send_goal, this));
When the timer expires, it calls send_goal
:
void send_goal() { using namespace std::placeholders; this->timer_->cancel(); if (!this->client_ptr_->wait_for_action_server()) { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } auto goal_msg = Fibonacci::Goal(); goal_msg.order = 10; RCLCPP_INFO(this->get_logger(), "Sending goal"); auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions(); send_goal_options.goal_response_callback = std::bind(&FibonacciActionClient::goal_response_callback, this, _1); send_goal_options.feedback_callback = std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2); send_goal_options.result_callback = std::bind(&FibonacciActionClient::result_callback, this, _1); this->client_ptr_->async_send_goal(goal_msg, send_goal_options); }
This function does the following:
-
Cancels the timer (so it is only called once).
-
Wait for the action server to appear.
-
Instantiate a new one
Fibonacci::Goal
. -
Set response, feedback and result callbacks.
-
Send the target to the server.
When the server receives and accepts the target, it sends a response to the client. This response is handled by goal_response_callback
:
void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future) { auto goal_handle = future.get(); if (!goal_handle) { RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); } else { RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); } }
Assuming the target has been accepted by the server, it will begin processing. Any feedback to customers will be handled by feedback_callback
:
void feedback_callback( GoalHandleFibonacci::SharedPtr, const std::shared_ptr<const Fibonacci::Feedback> feedback) { std::stringstream ss; ss << "Next number in sequence received: "; for (auto number : feedback->partial_sequence) { ss << number << " "; } RCLCPP_INFO(this->get_logger(), ss.str().c_str()); }
After the server completes processing, it will return a result to the client. Results are processed by result_callback
:
void result_callback(const GoalHandleFibonacci::WrappedResult & result) { switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: break; case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); return; case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); return; default: RCLCPP_ERROR(this->get_logger(), "Unknown result code"); return; } std::stringstream ss; ss << "Result received: "; for (auto number : result.result->sequence) { ss << number << " "; } RCLCPP_INFO(this->get_logger(), ss.str().c_str()); rclcpp::shutdown(); }
We now have a fully functional action client. Let's build and run it.
3.2 Compile action client
In the previous section, we put the action client code in place. In order for it to compile and run, we need to do a few extra things.
First we need to set up CMakeLists.txt in order to compile the action client. Open action_tutorials_cpp/CMakeLists.txt
and add the following after calls find_package
:
add_library(action_client SHARED src/fibonacci_action_client.cpp) target_include_directories(action_client PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<INSTALL_INTERFACE:include>) target_compile_definitions(action_client PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL") ament_target_dependencies(action_client "action_tutorials_interfaces" "rclcpp" "rclcpp_action" "rclcpp_components") rclcpp_components_register_node(action_client PLUGIN "action_tutorials_cpp::FibonacciActionClient" EXECUTABLE fibonacci_action_client) install(TARGETS action_client ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin)
Now we can compile the package. Go to the top level of action_ws
, and run:
colcon build
This should compile the entire workspace, including fibonacci_action_client
packages action_tutorials_cpp
.
3.3 Run action client
Now that we have built the action client, we can run it. First make sure the action server is running in a separate terminal. Now get the workspace we just built ( action_ws
) and try running the action client:
ros2 run action_tutorials_cpp fibonacci_action_client
You should see a logging message that the goal was accepted, feedback being printed, and the final result.
generalize
In this tutorial, you put together a C++ action server and action client line-by-line and configured them to exchange goals, feedback, and results.