Writing an action server and client (C++)¶

Goal: Implement an action server and client in C++.

Tutorial level: Intermediate

Time: 15 minutes

Background¶

Actions are a form of asynchronous communication in ROS. Action clients send goal requests to action servers. Action servers send goal feedback and results to action clients.

Prerequisites¶

You will need the action_tutorials_interfaces package and the Fibonacci.action interface defined in the previous tutorial, Creating an action.

1 Creating the action_tutorials_cpp package¶

As we saw in the Creating your first ROS 2 package tutorial, we need to create a new package to hold our C++ and supporting code.

1.1 Creating the action_tutorials_cpp package¶

Go into the action workspace you created in the previous tutorial (remember to source the workspace), and create a new package for the C++ action server:

cd ~/action_ws/src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp

cd ~/action_ws/src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp

cd \dev\action_ws\src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp


In order to make the package compile and work on Windows, we need to add in some “visibility control”. For details on why this is needed, see here.

Open up action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h, and put the following code in:

#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 computes the Fibonacci sequence using the action we created in the Creating an action tutorial.

2.1 Writing the action server code¶

Open up action_tutorials_cpp/src/fibonacci_action_server.cpp, and put the following code in:

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 #include #include #include #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; 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( this, "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::SharedPtr action_server_; rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr 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 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 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 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(); auto & sequence = feedback->partial_sequence; sequence.push_back(0); sequence.push_back(1); auto result = std::make_shared(); 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 include all of the headers we need to compile.

Next we create a class that is a derived class of rclcpp::Node:

class FibonacciActionServer : public rclcpp::Node


The constructor for the FibonacciActionServer class initializes the node name as 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,
"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:

1. The templated action type name: Fibonacci.

2. A ROS 2 node to add the action to: this.

3. The action name: 'fibonacci'.

4. A callback function for handling goals: handle_goal

5. A callback function for handling cancellation: handle_cancel.

6. A callback function for handling goal accept: handle_accept.

The implementation of the various callbacks is next in the file. Note that all of the callbacks need to return quickly, otherwise we risk starving the executor.

  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 just accepts all goals.

Next up is the callback for dealing with 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 just tells the client that it accepted the cancellation.

The last of the callbacks accepts a new goal 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
}


Since the execution is a long-running operation, we spawn off a thread to do the actual work and return from handle_accepted quickly.

All further processing and updates are done in the execute method in the new thread:

  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 work thread processes one sequence number of the Fibonacci sequence every second, publishing a feedback update for each step. When it has finished processing, it marks the goal_handle as succeeded, and quits.

We now have a fully functioning action server. Let’s get it built and running.

2.2 Compiling the action server¶

In the previous section we put the action server code into place. To get it to compile and run, we need to do a couple of additional things.

First we need to setup the CMakeLists.txt so that the action server is compiled. Open up action_tutorials_cpp/CMakeLists.txt, and add the following right after the find_package calls:

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)  And now we can compile the package. Go to the top-level of the action_ws, and run: colcon build  This should compile the entire workspace, including the fibonacci_action_server in the action_tutorials_cpp package. 2.3 Running the action server¶ Now that we have the action server built, we can run it. Source the workspace we just built (action_ws), and try to run the action server: ros2 run action_tutorials_cpp fibonacci_action_server  3 Writing an action client¶ 3.1 Writing the action client code¶ Open up action_tutorials_cpp/src/fibonacci_action_client.cpp, and put the following code in:   1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 #include #include #include #include #include #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; explicit FibonacciActionClient(const rclcpp::NodeOptions & options) : Node("fibonacci_action_client", options) { this->client_ptr_ = rclcpp_action::create_client( this, "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::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::SharedPtr client_ptr_; rclcpp::TimerBase::SharedPtr timer_; void goal_response_callback(std::shared_future 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 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 include all of the headers we need to compile. Next we create a class that is a derived class of rclcpp::Node: class FibonacciActionClient : public rclcpp::Node  The constructor for the FibonacciActionClient class initializes the node name as 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, "fibonacci");  An action client requires 3 things: 1. The templated action type name: Fibonacci. 2. A ROS 2 node to add the action client to: this. 3. The action name: 'fibonacci'. We also instantiate a ROS timer that will kick off the one and only call to send_goal:  this->timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&FibonacciActionClient::send_goal, this));  When the timer expires, it will call 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: 1. Cancels the timer (so it is only called once). 2. Waits for the action server to come up. 3. Instantiates a new Fibonacci::Goal. 4. Sets the response, feedback, and result callbacks. 5. Sends the goal to the server. When the server receives and accepts the goal, it will send a response to the client. That 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 goal was accepted by the server, it will start processing. Any feedback to the client will be handled by the 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()); }  When the server is finished processing, it will return a result to the client. The result is handled by the 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 functioning action client. Let’s get it built and running. 3.2 Compiling the action client¶ In the previous section we put the action client code into place. To get it to compile and run, we need to do a couple of additional things. First we need to setup the CMakeLists.txt so that the action client is compiled. Open up action_tutorials_cpp/CMakeLists.txt, and add the following right after the find_package calls: 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)


And now we can compile the package. Go to the top-level of the action_ws, and run:

colcon build


This should compile the entire workspace, including the fibonacci_action_client in the action_tutorials_cpp package.

3.3 Running the action client¶

Now that we have the action client built, we can run it. First make sure that an action server is running in a separate terminal. Now source the workspace we just built (action_ws), and try to run the action client:

ros2 run action_tutorials_cpp fibonacci_action_client


You should see logged messages for the goal being accepted, feedback being printed, and the final result.

Summary¶

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.