ROS2学习笔记21--编写action服务器和客户端（C++）
作者：互联网
概要：这篇内容主要介绍如何使用C++来编写动作服务器和客户端
环境：ubuntu20.04,ros2-foxy,vscode
最后如果没有陈述实操过程中碰到问题的话，则表示该章节都可被本人正常复现．
3.2编写动作服务器和客户端（C++）（原文：https://docs.ros.org/en/foxy/Tutorials/Actions/Writing-a-Cpp-Action-Server-Client.html）
>>教程>>编写动作服务器和客户端（C++）
你正阅读的是ros2较老版本(Foxy)，但仍然支持的说明文档．想查看最新版本的信息，请看galactic版本链接（ https://docs.ros.org/en/galactic/Tutorials.html）
编写动作服务器和客户端（C++）
目标：实现c++版的动作服务器和客户端．
时长：15min
目录
1.背景
2.预备知识
3.步骤
3.1创建action_tutorials_cpp包
3.2编写动作服务器
3.3编写动作客户端
4.总结
5.相关内容
1.背景
动作是ros异步通信的方式．动作客户端发送目标请求到动作服务器，动作服务器发送目标反馈以及结果到动作客户端．
2.预备知识
你需要前面创建一个动作教程里面创建的action_tutorials_interfaces包以及Fibonacci.action接口．
3.步骤
3.1创建action_tutorials_cpp包
参考创建第一个ros2包课程，我们需要创建一个包来放c++源码的，并提供支持的．
3.1.1创建包action_tutorials_cpp package
进入前面课程创建动作的工作空间（记得source一下环境变量），然后创建一个装c++动作服务器的包：
linux:
cd ~/action_ws/src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
(本人练习时，还是在之前dev_ws工作空间，所以进入工作空间会跟官方有点出入)
3.1.2添加明显需要内容
为了保证包的编译以及在窗口有效，我们需要添加明显需要的内容．为啥需要这些的细节缘由，可以看这里（https://docs.microsoft.com/en-us/cpp/cpp/dllexport-dllimport?view=msvc-160）．
打开并创建路径文件action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h，并复制一下代码进去：
#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_
3.2编写动作服务器
现在，让我们专注于计算Fibonacci数列的动作服务器编写，这会用到前面创建动作课程所创建的动作．
3.2.1编写动作服务器的代码
打开路径并创建action_tutorials_cpp/src/fibonacci_action_server.cpp，然后复制以下代码到里面：
1#include <functional>
2#include <memory>
3#include <thread>
4
5#include "action_tutorials_interfaces/action/fibonacci.hpp"
6#include "rclcpp/rclcpp.hpp"
7#include "rclcpp_action/rclcpp_action.hpp"
8#include "rclcpp_components/register_node_macro.hpp"
9
10#include "action_tutorials_cpp/visibility_control.h"
11
12namespace action_tutorials_cpp
13{
14class FibonacciActionServer : public rclcpp::Node
15{
16public:
17 using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
18 using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
19
20 ACTION_TUTORIALS_CPP_PUBLIC
21 explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
22 : Node("fibonacci_action_server", options)
23 {
24 using namespace std::placeholders;
25
26 this->action_server_ = rclcpp_action::create_server<Fibonacci>(
27 this,
28 "fibonacci",
29 std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
30 std::bind(&FibonacciActionServer::handle_cancel, this, _1),
31 std::bind(&FibonacciActionServer::handle_accepted, this, _1));
32 }
33
34private:
35 rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
36
37 rclcpp_action::GoalResponse handle_goal(
38 const rclcpp_action::GoalUUID & uuid,
39 std::shared_ptr<const Fibonacci::Goal> goal)
40 {
41 RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
42 (void)uuid;
43 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
44 }
45
46 rclcpp_action::CancelResponse handle_cancel(
47 const std::shared_ptr<GoalHandleFibonacci> goal_handle)
48 {
49 RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
50 (void)goal_handle;
51 return rclcpp_action::CancelResponse::ACCEPT;
52 }
53
54 void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
55 {
56 using namespace std::placeholders;
57 // this needs to return quickly to avoid blocking the executor, so spin up a new thread
58 std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
59 }
60
61 void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
62 {
63 RCLCPP_INFO(this->get_logger(), "Executing goal");
64 rclcpp::Rate loop_rate(1);
65 const auto goal = goal_handle->get_goal();
66 auto feedback = std::make_shared<Fibonacci::Feedback>();
67 auto & sequence = feedback->partial_sequence;
68 sequence.push_back(0);
69 sequence.push_back(1);
70 auto result = std::make_shared<Fibonacci::Result>();
71
72 for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
73 // Check if there is a cancel request
74 if (goal_handle->is_canceling()) {
75 result->sequence = sequence;
76 goal_handle->canceled(result);
77 RCLCPP_INFO(this->get_logger(), "Goal canceled");
78 return;
79 }
80 // Update sequence
81 sequence.push_back(sequence[i] + sequence[i - 1]);
82 // Publish feedback
83 goal_handle->publish_feedback(feedback);
84 RCLCPP_INFO(this->get_logger(), "Publish feedback");
85
86 loop_rate.sleep();
87 }
88
89 // Check if goal is done
90 if (rclcpp::ok()) {
91 result->sequence = sequence;
92 goal_handle->succeed(result);
93 RCLCPP_INFO(this->get_logger(), "Goal succeeded");
94 }
95 }
96}; // class FibonacciActionServer
97
98} // namespace action_tutorials_cpp
99
100 RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer)
首先，前面几行是包含我们编译所需的头文件．
接着，我们创建一个rclcpp::Node派生类：
class FibonacciActionServer : public rclcpp::Node
FibonacciActionServer类的构造函数初始化节点名字为fibonacci_action_server：
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("fibonacci_action_server", options)
构造函数也实例化一个新的动作服务器：
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));
一个动作服务器要求有6样东西：
1.模板动作类名：Fibonacci
2.用于添加动作的ros2节点：this
3.动作名称：'fibonacci'
4.负责目标的回调函数：handle_goal
5.负责取消的回调函数：handle_cancel
6.负责目标取消的回调函数：handle_accept
各个回调函数的实现（函数定义）在文件下面．注意，所有的反馈需要返回迅速，否则，有执行器中断风险．
我们看看负责新目标反馈部分：
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();
}
由于execute是一个需要长时间运行的操作,我们开一个线程来做实际的工作,并且可以很快地从handle_accepted返回。
execute方法将要做的处理和更新工作，都是在新线程里面进行的：
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");
}
}
工作线程每秒都会处理Fibonacci数列的连串数字，并发布信息反馈每一步的更新．当完成处理过程，它会标记goal_handle为成功，并且退出．
现在我们有了一个完整的功能性动作服务器，编译一下并且运行．
3.2.2编译动作服务器
在前面章节，我们写好了动作服务器代码．为了编译运行它，我们还得做些额外工作．
首先，我们需要配置一下CMakeLists.txt，保证动作服务器可以编译．打开action_tutorials_cpp/CMakeLists.txt，在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)
现在，我们可以编译这个包了．返回action_ws（根据自己实际使用的工作空间修改）工作空间的根目录，执行：
colcon build
3.2.3运行动作服务器
现在我们构建好动作服务器了，可以运行它了．source一下工作空间环境变量，然后尝试运行动作服务器：
ros2 run action_tutorials_cpp fibonacci_action_server
3.3编写动作客户端
3.3.1编写动作客户端代码
打开路径并新建action_tutorials_cpp/src/fibonacci_action_client.cpp，把下面代码放到里面：
1#include <functional>
2#include <future>
3#include <memory>
4#include <string>
5#include <sstream>
6
7#include "action_tutorials_interfaces/action/fibonacci.hpp"
8
9#include "rclcpp/rclcpp.hpp"
10#include "rclcpp_action/rclcpp_action.hpp"
11#include "rclcpp_components/register_node_macro.hpp"
12
13namespace action_tutorials_cpp
14{
15class FibonacciActionClient : public rclcpp::Node
16{
17public:
18 using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
19 using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
20
21 explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
22 : Node("fibonacci_action_client", options)
23 {
24 this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
25 this,
26 "fibonacci");
27
28 this->timer_ = this->create_wall_timer(
29 std::chrono::milliseconds(500),
30 std::bind(&FibonacciActionClient::send_goal, this));
31 }
32
33 void send_goal()
34 {
35 using namespace std::placeholders;
36
37 this->timer_->cancel();
38
39 if (!this->client_ptr_->wait_for_action_server()) {
40 RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
41 rclcpp::shutdown();
42 }
43
44 auto goal_msg = Fibonacci::Goal();
45 goal_msg.order = 10;
46
47 RCLCPP_INFO(this->get_logger(), "Sending goal");
48
49 auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
50 send_goal_options.goal_response_callback =
51 std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
52 send_goal_options.feedback_callback =
53 std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
54 send_goal_options.result_callback =
55 std::bind(&FibonacciActionClient::result_callback, this, _1);
56 this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
57 }
58
59private:
60 rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
61 rclcpp::TimerBase::SharedPtr timer_;
62
63 void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
64 {
65 auto goal_handle = future.get();
66 if (!goal_handle) {
67 RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
68 } else {
69 RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
70 }
71 }
72
73 void feedback_callback(
74 GoalHandleFibonacci::SharedPtr,
75 const std::shared_ptr<const Fibonacci::Feedback> feedback)
76 {
77 std::stringstream ss;
78 ss << "Next number in sequence received: ";
79 for (auto number : feedback->partial_sequence) {
80 ss << number << " ";
81 }
82 RCLCPP_INFO(this->get_logger(), ss.str().c_str());
83 }
84
85 void result_callback(const GoalHandleFibonacci::WrappedResult & result)
86 {
87 switch (result.code) {
88 case rclcpp_action::ResultCode::SUCCEEDED:
89 break;
90 case rclcpp_action::ResultCode::ABORTED:
91 RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
92 return;
93 case rclcpp_action::ResultCode::CANCELED:
94 RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
95 return;
96 default:
97 RCLCPP_ERROR(this->get_logger(), "Unknown result code");
98 return;
99 }
100 std::stringstream ss;
101 ss << "Result received: ";
102 for (auto number : result.result->sequence) {
103 ss << number << " ";
104 }
105 RCLCPP_INFO(this->get_logger(), ss.str().c_str());
106 rclcpp::shutdown();
107 }
108}; // class FibonacciActionClient
109
110} // namespace action_tutorials_cpp
111
112 RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient)
首先，前面几行是包含我们编译所需的头文件．
接着，我们创建一个rclcpp::Node派生类：
class FibonacciActionClient : public rclcpp::Node
FibonacciActionClient类的构造函数初始化了一个名为fibonacci_action_client的节点：
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)
这个构造函数也实例化一个新的动作客户端：
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
this,
"fibonacci");
一个动作客户端要求有３部分内容：
1.动作类模板名：Fibonacci
2.用于添加动作客户端的ros2节点：this
3.动作名：'fibonacci'
我们也要实例化ros定时器，仅当要呼叫send_goal才会开启一个（定时器）．
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&FibonacciActionClient::send_goal, this));
当定时器到点时，它将会呼叫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);
}
这个函数所做的内容如下：
1.取消定时器（所以它只是调用一次）
2.等待动作服务器启动
3.实例化一个新的Fibonacci::Goal
4.设置（目标）响应，反馈和结果的回调的回调函数
5.发送目标到服务器
当服务器接收并接受目标，它会给一个响应到客户端，这个响应由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");
}
}
当服务器处理完成，它会返回一个结果到客户端．这个结果（反馈）由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();
}
现在我们有了一个完整功能性的动作客户端，让我们开始编译运行吧．
3.3.2编译动作客户端
在上一小节，我们写好了动作客户端的代码．为了让它可以编译并运行，我们需要做一些额外工作．
首先，我们需要配置一下CMakeLists.txt，保证动作客户端可以编译．打开action_tutorials_cpp/CMakeLists.txt，在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)
现在，我们可以编译这个包了．回到所在工作空间的根目录，运行：
colcon build
（如果是有多个包情况，建议添加指令--packages-select来指定编译，提高编译速度）
上面的指令会编译整个工作空间，包括action_tutorials_cpp包里面的fibonacci_action_client．
3.3.3运行动作客户端
现在我们构建好了动作客户端，我们可以运行了．首先保证动作服务器正在别的终端运行，source一下工作空间环境变量，然后尝试运行动作客户端：
ros2 run action_tutorials_cpp fibonacci_action_client
你会看见日志信息，目标被接受，反馈被打印和最后结果．
4.总结
在本课程，你用c++逐行一起实现动作服务器和动作客户端，并且配置它们以实现传递目标，传递反馈和传递结果的功能．
5.相关内容
这里（https://github.com/ros2/examples/tree/foxy/rclcpp）有几种方式，你可以用来编写c++版本的动作服务器和动作客户端，并且可以校核minimal_action_server和 minimal_action_client包．
想知道更多关于ros动作细节，你可以参考设计文档（http://design.ros2.org/articles/actions.html）．
其他
个人认为重点：
动作服务器，动作客户端分别对应的功能函数的每一行句子的含义理解；配置文件，这里具体指的是CMakeLists.txt的编写；如何才能用到上一节课创建的动作接口（开始创建包时候，作为依赖添加上去）．
这课程是在等毕业证那十几天搞的，室友问，现在在线翻译这么强大，为啥还在这里瞎折腾呢？我说，我的目地是好好认真看一下，了解一下，自己折腾，目前是我想到最好的办法来获得最佳效果，即使这翻译有点别扭，哈哈哈．
#####################
不积硅步，无以至千里
好记性不如烂笔头
感觉有点收获的话，麻烦大大们点赞收藏哈
标签：std,handle,21,get,--,rclcpp,C++,action,goal 来源： https://blog.csdn.net/qq_45701501/article/details/119281373