概要:这篇内容主要介绍如何使用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");
一个动作客户端要求有3
部分内容:
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
的编写;如何才能用到上一节课创建的动作接口(开始创建包时候,作为依赖添加上去).
这课程是在等毕业证那十几天搞的,室友问,现在在线翻译这么强大,为啥还在这里瞎折腾呢?我说,我的目地是好好认真看一下,了解一下,自己折腾,目前是我想到最好的办法来获得最佳效果,即使这翻译有点别扭,哈哈哈.
#####################
不积硅步,无以至千里
好记性不如烂笔头
感觉有点收获的话,麻烦大大们点赞收藏哈