编写动作服务器和客户端(C++)
目标 用 C++ 实现动作服务器和客户端。
辅导水平: 中级
时间 15 分钟
背景介绍
操作是 ROS 中的一种异步通信形式。 行动客户 将目标申请发送至 行动服务器. 行动服务器 将目标反馈和结果发送至 行动客户.
先决条件
您需要 自定义动作接口
软件包和 Fibonacci.action
接口、 创建行动.
任务
1 创建 custom_action_cpp 软件包
正如我们在 创建软件包 教程,我们需要创建一个新的软件包来保存我们的 C++ 和辅助代码。
1.1 创建 custom_action_cpp 软件包
进入在 上一个教程 (工作区),并为 C++ 操作服务器创建一个新软件包:
CD ~/ros2_ws/src ros2 包 创建 --依赖 自定义动作接口 rclcpp rclcpp_action rclcpp_components --许可证 Apache-2.0 -- 自定义动作
CD ~/ros2_ws/src ros2 包 创建 --依赖 自定义动作接口 rclcpp rclcpp_action rclcpp_components --许可证 Apache-2.0 -- 自定义动作
CD \ros2_ws\src ros2 包 创建 --依赖 自定义动作接口 rclcpp rclcpp_action rclcpp_components --许可证 Apache-2.0 -- 自定义动作
1.2 加入可见度控制
为了使软件包能在 Windows 上编译和运行,我们需要添加一些 "可见性控制"。更多详情,请参阅 Windows 提示和技巧文档中的 Windows 符号可见性.
打开 custom_action_cpp/include/custom_action_cpp/visibility_control.h
并输入以下代码:
#ifndef CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_
#define CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_
#ifdef __cplusplus
外部 "C";
{
#endif
// 这个逻辑是从 gcc wiki 上的示例中借用(然后命名)的:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define CUSTOM_ACTION_CPP_EXPORT __attribute__ ((dllexport))
#define CUSTOM_ACTION_CPP_IMPORT __attribute__ ((dllimport))
#else
#define CUSTOM_ACTION_CPP_EXPORT __declspec(dllexport)
#define CUSTOM_ACTION_CPP_IMPORT __declspec(dllimport)
#endif
#ifdef CUSTOM_ACTION_CPP_BUILDING_DLL
#define CUSTOM_ACTION_CPP_PUBLIC CUSTOM_ACTION_CPP_EXPORT
#else
#define CUSTOM_ACTION_CPP_PUBLIC CUSTOM_ACTION_CPP_IMPORT
#endif
#define CUSTOM_ACTION_CPP_PUBLIC_TYPE CUSTOM_ACTION_CPP_PUBLIC
#define CUSTOM_ACTION_CPP_LOCAL
#else
#define CUSTOM_ACTION_CPP_EXPORT __attribute__ ((visibility("default")))
#define CUSTOM_ACTION_CPP_IMPORT
#if __GNUC__ >= 4
#define CUSTOM_ACTION_CPP_PUBLIC __attribute__ ((visibility("default")))
#define CUSTOM_ACTION_CPP_LOCAL __attribute__ ((visibility("hidden")))
#else
#define CUSTOM_ACTION_CPP_PUBLIC
#define CUSTOM_ACTION_CPP_LOCAL
#endif
#define CUSTOM_ACTION_CPP_PUBLIC_TYPE
#endif
#ifdef __cplusplus
}
#endif
#endif // custom_action_cpp__visibility_control_h_
2 编写行动服务器
让我们集中精力编写一个动作服务器,使用我们在 创建行动 教程。
2.1 编写行动服务器代码
打开 custom_action_cpp/src/fibonacci_action_server.cpp
并输入以下代码:
#include 功能强大;
#include <内存>;
#include <thread>;
#include "custom_action_interfaces/action/fibonacci.hpp";
#include "rclcpp/rclcpp.hpp";
#include "rclcpp_action/rclcpp_action.hpp";
#include "rclcpp_components/register_node_macro.hpp";
#include "custom_action_cpp/visibility_control.h";
命名空间 自定义动作
{
类 FibonacciActionServer : 公 rclcpp::节点
{
公:
使用 斐波那契 = 自定义动作接口::行动::斐波那契;
使用 目标手柄斐波纳契 = rclcpp_action::服务器目标句柄<;斐波那契>;;
自定义动作_cpp_public
不含糊 FibonacciActionServer(缢 rclcpp::节点选项 及样品; 选项 = rclcpp::节点选项())
: 节点("fibonacci_action_server";, 选项)
{
使用 命名空间 标准::占位符;
汽车 处理目标 = [此](
缢 rclcpp_action::目标 UUID 及样品; uuid,
标准::共享_ptr<;缢 斐波那契::目标>; 目标)
{
RCLCPP_INFO(此->;get_logger(), "收到目标请求,订单 %d";, 目标->;订单);
(空白)uuid;
返回 rclcpp_action::目标响应::接受并执行;
};
汽车 处理取消 = [此](
缢 标准::共享_ptr<;目标手柄斐波纳契>; 目标句柄)
{
RCLCPP_INFO(此->;get_logger(), 收到取消目标的请求";);
(空白)目标句柄;
返回 rclcpp_action::取消响应::接受;
};
汽车 已接受 = [此](
缢 标准::共享_ptr<;目标手柄斐波纳契>; 目标句柄)
{
// 需要快速返回,以避免阻塞执行器、
// 因此,我们声明一个 lambda 函数,在新线程中调用
汽车 在线程中执行 = [此, 目标句柄](){返回 此->;执行(目标句柄);};
标准::线程{在线程中执行}.脱离();
};
此->;行动服务器 = rclcpp_action::创建服务器<;斐波那契>;(
此,
"fibonacci";,
处理目标,
处理取消,
已接受);
}
私人:
rclcpp_action::服务器<;斐波那契>::SharedPtr 行动服务器;
空白 执行(缢 标准::共享_ptr<;目标手柄斐波纳契>; 目标句柄) {
RCLCPP_INFO(此->;get_logger(), "执行目标";);
rclcpp::费率 循环速率(1);
缢 汽车 目标 = 目标句柄->;get_goal();
汽车 反馈 = 标准::共享<;斐波那契::反馈意见>;();
汽车 及样品; 顺序 = 反馈->;部分序列;
顺序.推回(0);
顺序.推回(1);
汽车 结果 = 标准::共享<;斐波那契::结果>;();
对于 (int i = 1; (i <; 目标->;订单) &&; rclcpp::好的(); ++i) {
// 检查是否有取消请求
如果 (目标句柄->;is_canceling()) {
结果->;顺序 = 顺序;
目标句柄->;已取消(结果);
RCLCPP_INFO(此->;get_logger(), 目标取消";);
返回;
}
// 更新序列
顺序.推回(顺序[i] + 顺序[i - 1]);
// 发布反馈意见
目标句柄->;发布反馈(反馈);
RCLCPP_INFO(此->;get_logger(), "发布反馈";);
循环速率.睡眠();
}
// 检查目标是否完成
如果 (rclcpp::好的()) {
结果->;顺序 = 顺序;
目标句柄->;继承(结果);
RCLCPP_INFO(此->;get_logger(), "目标成功";);
}
};
}; // 类 FibonacciActionServer
} // 命名空间 custom_action_cpp
rclcpp_components_register_node(自定义动作::FibonacciActionServer)
前几行包含了我们需要编译的所有头文件。
接下来,我们创建一个从 rclcpp::Node
:
类 FibonacciActionServer : 公 rclcpp::节点
的构造函数 FibonacciActionServer
类将节点名称初始化为 fibonacci_action_server
:
不含糊 FibonacciActionServer(缢 rclcpp::节点选项 及样品; 选项 = rclcpp::节点选项())
: 节点("fibonacci_action_server";, 选项)
构造函数还会实例化一个新的动作服务器:
循环速率.睡眠();
}
// 检查目标是否完成
如果 (rclcpp::好的()) {
行动服务器需要具备 6 个条件:
模板化操作类型名称:
斐波那契
.要添加操作的 ROS 2 节点:
此
.行动名称:
斐波那契
.用于处理目标的回调函数:
处理目标
用于处理取消的回调函数:
处理取消
.用于处理目标接受的回调函数:
接受
.
各种回调的实现都是通过 [lambda 表达式] (https://en.cppreference.com/w/cpp/language/lambda) 在构造函数中。需要注意的是,所有的回调都需要快速返回,否则就有可能使执行器处于饥饿状态。
我们从处理新目标的回调开始:
汽车 处理目标 = [此](
缢 rclcpp_action::目标 UUID 及样品; uuid,
标准::共享_ptr<;缢 斐波那契::目标>; 目标)
{
RCLCPP_INFO(此->;get_logger(), "收到目标请求,订单 %d";, 目标->;订单);
(空白)uuid;
返回 rclcpp_action::目标响应::接受并执行;
};
这种实现方式只接受所有目标。
接下来是处理取消订单的回调:
汽车 处理取消 = [此](
缢 标准::共享_ptr<;目标手柄斐波纳契>; 目标句柄)
{
RCLCPP_INFO(此->;get_logger(), 收到取消目标的请求";);
(空白)目标句柄;
返回 rclcpp_action::取消响应::接受;
};
这种实现方式只是告诉客户端它接受了取消。
最后一个回调接受一个新目标并开始处理:
汽车 已接受 = [此](
缢 标准::共享_ptr<;目标手柄斐波纳契>; 目标句柄)
{
// 需要快速返回,以避免阻塞执行器、
// 因此,我们声明一个 lambda 函数,在新线程中调用
汽车 在线程中执行 = [此, 目标句柄](){返回 此->;执行(目标句柄);};
标准::线程{在线程中执行}.脱离();
};
由于执行的是一个长期运行的操作,我们会产生一个线程来执行实际工作,并从 已接受
很快。
所有进一步的处理和更新都在 执行
方法:
空白 执行(缢 标准::共享_ptr<;目标手柄斐波纳契>; 目标句柄) {
RCLCPP_INFO(此->;get_logger(), "执行目标";);
rclcpp::费率 循环速率(1);
缢 汽车 目标 = 目标句柄->;get_goal();
汽车 反馈 = 标准::共享<;斐波那契::反馈意见>;();
汽车 及样品; 顺序 = 反馈->;部分序列;
顺序.推回(0);
顺序.推回(1);
汽车 结果 = 标准::共享<;斐波那契::结果>;();
对于 (int i = 1; (i <; 目标->;订单) &&; rclcpp::好的(); ++i) {
// 检查是否有取消请求
如果 (目标句柄->;is_canceling()) {
结果->;顺序 = 顺序;
目标句柄->;已取消(结果);
RCLCPP_INFO(此->;get_logger(), 目标取消";);
返回;
}
// 更新序列
顺序.推回(顺序[i] + 顺序[i - 1]);
// 发布反馈意见
目标句柄->;发布反馈(反馈);
RCLCPP_INFO(此->;get_logger(), "发布反馈";);
循环速率.睡眠();
}
// 检查目标是否完成
如果 (rclcpp::好的()) {
结果->;顺序 = 顺序;
目标句柄->;继承(结果);
RCLCPP_INFO(此->;get_logger(), "目标成功";);
}
};
该工作线程每秒处理一个斐波那契数列的序列号,每一步都会发布反馈更新。完成处理后,它将 目标句柄
并退出。
我们现在有了一个功能完备的行动服务器。让我们构建并运行它。
2.2 编译行动服务器
在上一节中,我们将动作服务器代码安装到位。为了让它编译和运行,我们还需要做几件事。
首先,我们需要设置 CMakeLists.txt,以便编译动作服务器。打开 custom_action_cpp/CMakeLists.txt
之后添加以下内容 查找软件包
电话
add_library(行动服务器 共享
src/fibonacci_action_server.cpp)
目标包含目录(行动服务器 私人
$<;BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>;
$<;INSTALL_INTERFACE:include>;)
目标编译定义(行动服务器
私人 "CUSTOM_ACTION_CPP_BUILDING_DLL";)
ament_target_dependencies(行动服务器
"custom_action_interfaces";
"rclcpp";
"rclcpp_action";
"rclcpp_components";)
rclcpp_components_register_node(行动服务器 插件 "custom_action_cpp::FibonacciActionServer"; 可执行 fibonacci_action_server)
安装(目标
行动服务器
存档 目的地 lib
图书馆 目的地 lib
运行时间 目的地 箱柜)
现在我们可以编译软件包了。转到 ros2_ws
然后运行:
胶管 构建
这将编译整个工作区,包括 fibonacci_action_server
在 自定义动作
包装
2.3 运行行动服务器
现在我们已经构建了行动服务器,可以运行它了。将我们刚刚构建的工作区(ros2_ws
),并尝试运行动作服务器:
玫瑰2 运行 自定义动作 fibonacci_action_server
3 撰写行动客户
3.1 编写动作客户端代码
打开 custom_action_cpp/src/fibonacci_action_client.cpp
并输入以下代码:
#include 功能强大;
#include 未来;
#include <内存>;
#include <字符串>;
#include 流>;
#include "custom_action_interfaces/action/fibonacci.hpp";
#include "rclcpp/rclcpp.hpp";
#include "rclcpp_action/rclcpp_action.hpp";
#include "rclcpp_components/register_node_macro.hpp";
命名空间 自定义动作
{
类 FibonacciActionClient : 公 rclcpp::节点
{
公:
使用 斐波那契 = 自定义动作接口::行动::斐波那契;
使用 目标手柄斐波纳契 = rclcpp_action::客户目标句柄<;斐波那契>;;
不含糊 FibonacciActionClient(缢 rclcpp::节点选项 及样品; 选项)
: 节点("fibonacci_action_client";, 选项)
{
此->;客户端ptr_ = rclcpp_action::创建客户端<;斐波那契>;(
此,
"fibonacci";);
汽车 timer_callback_lambda = [此](){ 返回 此->;发送目标(); };
此->;定时器 = 此->;创建隔离墙计时器(
标准::计时器::毫秒数(500),
timer_callback_lambda);
}
空白 发送目标()
{
使用 命名空间 标准::占位符;
此->;定时器->;取消();
如果 (!此->;客户端ptr_->;等待行动服务器()) {
RCLCPP_ERROR(此->;get_logger(), "等待后行动服务器不可用";);
rclcpp::关闭();
}
汽车 goal_msg = 斐波那契::目标();
goal_msg.订单 = 10;
RCLCPP_INFO(此->;get_logger(), 发送目标";);
汽车 发送目标选项 = rclcpp_action::客户<;斐波那契>::发送目标选项();
发送目标选项.目标响应回调 = [此](缢 目标手柄斐波纳契::SharedPtr 及样品; 目标句柄)
{
如果 (!目标句柄) {
RCLCPP_ERROR(此->;get_logger(), 目标被服务器拒绝";);
} 不然 {
RCLCPP_INFO(此->;get_logger(), 服务器接受目标,等待结果";);
}
};
发送目标选项.反馈回调 = [此](
目标手柄斐波纳契::SharedPtr,
缢 标准::共享_ptr<;缢 斐波那契::反馈意见>; 反馈)
{
标准::字符串流 ss;
ss <<; "收到序列中的下一个号码:";;
对于 (汽车 编号 : 反馈->;部分序列) {
ss <<; 编号 <<; " ";;
}
RCLCPP_INFO(此->;get_logger(), ss.字符串().c_str());
};
发送目标选项.result_callback = [此](缢 目标手柄斐波纳契::包裹结果 及样品; 结果)
{
开关 (结果.代码) {
个案 rclcpp_action::结果代码::成功:
断裂;
个案 rclcpp_action::结果代码::中止:
RCLCPP_ERROR(此->;get_logger(), 目标失败";);
返回;
个案 rclcpp_action::结果代码::取消:
RCLCPP_ERROR(此->;get_logger(), 目标被取消";);
返回;
默认:
RCLCPP_ERROR(此->;get_logger(), 未知结果代码";);
返回;
}
标准::字符串流 ss;
ss <<; 已收到结果: ";;
对于 (汽车 编号 : 结果.结果->;顺序) {
ss <<; 编号 <<; " ";;
}
RCLCPP_INFO(此->;get_logger(), ss.字符串().c_str());
rclcpp::关闭();
};
此->;客户端ptr_->;异步发送目标(goal_msg, 发送目标选项);
}
私人:
rclcpp_action::客户<;斐波那契>::SharedPtr 客户端ptr_;
rclcpp::定时器基数::SharedPtr 定时器;
}; // 类 FibonacciActionClient
} // 命名空间 custom_action_cpp
rclcpp_components_register_node(自定义动作::FibonacciActionClient)
前几行包含了我们需要编译的所有头文件。
接下来,我们创建一个从 rclcpp::Node
:
类 FibonacciActionClient : 公 rclcpp::节点
的构造函数 FibonacciActionClient
类将节点名称初始化为 斐波那契行动客户端
:
不含糊 FibonacciActionClient(缢 rclcpp::节点选项 及样品; 选项)
: 节点("fibonacci_action_client";, 选项)
构造函数还会实例化一个新的动作客户端:
此->;客户端ptr_ = rclcpp_action::创建客户端<;斐波那契>;(
此,
"fibonacci";);
行动客户需要具备 3 个条件:
模板化操作类型名称:
斐波那契
.要添加动作客户端的 ROS 2 节点:
此
.行动名称:
斐波那契
.
我们还实例化了一个 ROS 定时器,它将启动对 发送目标
:
汽车 timer_callback_lambda = [此](){ 返回 此->;发送目标(); };
此->;定时器 = 此->;创建隔离墙计时器(
标准::计时器::毫秒数(500),
timer_callback_lambda);
计时器到期时,它会调用 发送目标
:
空白 发送目标()
{
使用 命名空间 标准::占位符;
此->;定时器->;取消();
如果 (!此->;客户端ptr_->;等待行动服务器()) {
RCLCPP_ERROR(此->;get_logger(), "等待后行动服务器不可用";);
rclcpp::关闭();
}
汽车 goal_msg = 斐波那契::目标();
goal_msg.订单 = 10;
RCLCPP_INFO(此->;get_logger(), 发送目标";);
汽车 发送目标选项 = rclcpp_action::客户<;斐波那契>::发送目标选项();
发送目标选项.目标响应回调 = [此](缢 目标手柄斐波纳契::SharedPtr 及样品; 目标句柄)
{
如果 (!目标句柄) {
RCLCPP_ERROR(此->;get_logger(), 目标被服务器拒绝";);
} 不然 {
RCLCPP_INFO(此->;get_logger(), 服务器接受目标,等待结果";);
}
};
发送目标选项.反馈回调 = [此](
目标手柄斐波纳契::SharedPtr,
缢 标准::共享_ptr<;缢 斐波那契::反馈意见>; 反馈)
{
标准::字符串流 ss;
ss <<; "收到序列中的下一个号码:";;
对于 (汽车 编号 : 反馈->;部分序列) {
ss <<; 编号 <<; " ";;
}
RCLCPP_INFO(此->;get_logger(), ss.字符串().c_str());
};
发送目标选项.result_callback = [此](缢 目标手柄斐波纳契::包裹结果 及样品; 结果)
{
开关 (结果.代码) {
个案 rclcpp_action::结果代码::成功:
断裂;
个案 rclcpp_action::结果代码::中止:
RCLCPP_ERROR(此->;get_logger(), 目标失败";);
返回;
个案 rclcpp_action::结果代码::取消:
RCLCPP_ERROR(此->;get_logger(), 目标被取消";);
返回;
默认:
RCLCPP_ERROR(此->;get_logger(), 未知结果代码";);
返回;
}
标准::字符串流 ss;
ss <<; 已收到结果: ";;
对于 (汽车 编号 : 结果.结果->;顺序) {
ss <<; 编号 <<; " ";;
}
RCLCPP_INFO(此->;get_logger(), ss.字符串().c_str());
rclcpp::关闭();
};
此->;客户端ptr_->;异步发送目标(goal_msg, 发送目标选项);
}
该功能执行以下操作
取消计时器(因此只调用一次)。
等待行动服务器出现。
实例化一个新的
Fibonacci::Goal
.设置响应、反馈和结果回调。
将目标发送到服务器。
服务器接收并接受目标后,会向客户端发送一个响应。该响应由 目标响应回调
:
发送目标选项.目标响应回调 = [此](缢 目标手柄斐波纳契::SharedPtr 及样品; 目标句柄)
{
如果 (!目标句柄) {
RCLCPP_ERROR(此->;get_logger(), 目标被服务器拒绝";);
} 不然 {
RCLCPP_INFO(此->;get_logger(), 服务器接受目标,等待结果";);
}
};
假设服务器接受了目标,就会开始处理。对客户端的任何反馈都将由 反馈回调
:
发送目标选项.反馈回调 = [此](
目标手柄斐波纳契::SharedPtr,
缢 标准::共享_ptr<;缢 斐波那契::反馈意见>; 反馈)
{
标准::字符串流 ss;
ss <<; "收到序列中的下一个号码:";;
对于 (汽车 编号 : 反馈->;部分序列) {
ss <<; 编号 <<; " ";;
}
RCLCPP_INFO(此->;get_logger(), ss.字符串().c_str());
};
服务器处理完毕后,会向客户端返回一个结果。结果由 result_callback
:
发送目标选项.result_callback = [此](缢 目标手柄斐波纳契::包裹结果 及样品; 结果)
{
开关 (结果.代码) {
个案 rclcpp_action::结果代码::成功:
断裂;
个案 rclcpp_action::结果代码::中止:
RCLCPP_ERROR(此->;get_logger(), 目标失败";);
返回;
个案 rclcpp_action::结果代码::取消:
RCLCPP_ERROR(此->;get_logger(), 目标被取消";);
返回;
默认:
RCLCPP_ERROR(此->;get_logger(), 未知结果代码";);
返回;
}
标准::字符串流 ss;
ss <<; 已收到结果: ";;
对于 (汽车 编号 : 结果.结果->;顺序) {
ss <<; 编号 <<; " ";;
}
RCLCPP_INFO(此->;get_logger(), ss.字符串().c_str());
rclcpp::关闭();
};
我们现在有了一个功能完备的行动客户端。让我们构建并运行它。
3.2 编译行动客户端
在上一节中,我们将动作客户端代码安装到位。为了让它编译和运行,我们还需要做几件事。
首先,我们需要设置 CMakeLists.txt,以便编译动作客户端。打开 custom_action_cpp/CMakeLists.txt
之后添加以下内容 查找软件包
电话
add_library(动作客户端 共享
src/fibonacci_action_client.cpp)
目标包含目录(动作客户端 私人
$<;BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>;
$<;INSTALL_INTERFACE:include>;)
目标编译定义(动作客户端
私人 "CUSTOM_ACTION_CPP_BUILDING_DLL";)
ament_target_dependencies(动作客户端
"custom_action_interfaces";
"rclcpp";
"rclcpp_action";
"rclcpp_components";)
rclcpp_components_register_node(动作客户端 插件 "custom_action_cpp::FibonacciActionClient"; 可执行 斐波那契行动客户端)
安装(目标
动作客户端
存档 目的地 lib
图书馆 目的地 lib
运行时间 目的地 箱柜)
现在我们可以编译软件包了。转到 ros2_ws
然后运行:
胶管 构建
这将编译整个工作区,包括 斐波那契行动客户端
在 自定义动作
包装
3.3 运行行动客户端
现在我们已经创建了动作客户端,可以运行它了。首先,确保动作服务器在单独的终端中运行。现在,将我们刚刚构建的工作区 (ros2_ws
),并尝试运行动作客户端:
玫瑰2 运行 自定义动作 斐波那契行动客户端
你应该能看到目标被接受、反馈被打印以及最终结果的记录信息。
摘要
在本教程中,您将逐行组装 C++ 动作服务器和动作客户端,并配置它们以交换目标、反馈和结果。