目标:用 C++实现一个动作服务器和客户端。
教程级别:中级
时间:15 分钟
目录
背景
先决条件
任务
1. 创建 custom_action_cpp 包
2. 编写动作服务器
3. 编写动作客户端
摘要
相关内容
背景
动作是 ROS 中异步通信的一种形式。动作客户端向动作服务器发送目标请求。动作服务器向动作客户端发送目标反馈和结果。
先决条件
创建一个动作,您将需要 custom_action_interfaces
包和在上一教程中定义的 Fibonacci.action
接口。
任务
1. 创建 custom_action_cpp 包
在我们在创建包教程中看到的,我们需要创建一个新的包来容纳我们的 C++和支持代码。
1.1 创建 custom_action_cpp 包
进入您在上一个教程中创建的动作工作区(记得要引用工作区),并为 C++ 动作服务器创建一个新包:
cd ~/ros2_ws/src
cxy@ubuntu2404-cxy:~/ros2_ws/src$ ros2 pkg create --dependencies custom_action_interfaces rclcpp rclcpp_action rclcpp_components --license Apache-2.0 -- custom_action_cpp
going to create a new package
package name: custom_action_cpp
destination directory: /home/cxy/ros2_ws/src
package format: 3
version: 0.0.0
description: TODO: Package description
maintainer: ['cxy <[email protected]>']
licenses: ['Apache-2.0']
build type: ament_cmake
dependencies: ['custom_action_interfaces', 'rclcpp', 'rclcpp_action', 'rclcpp_components']
creating folder ./custom_action_cpp
creating ./custom_action_cpp/package.xml
creating source and include folder
creating folder ./custom_action_cpp/src
creating folder ./custom_action_cpp/include/custom_action_cpp
creating ./custom_action_cpp/CMakeLists.txt
1.2 添加可见性控制
为了使软件包在 Windows 上编译和运行,我们需要添加一些“可见性控制”。有关更多详情,请参阅 Windows 提示和技巧文档中的 Windows 符号可见性 https://docs.ros.org/en/jazzy/The-ROS2-Project/Contributing/Windows-Tips-and-Tricks.html#windows-symbol-visibility 。
打开 custom_action_cpp/include/custom_action_cpp/visibility_control.h
,然后输入以下代码:
cxy@ubuntu2404-cxy:~/ros2_ws/src/custom_action_cpp$ gedit include/custom_action_cpp/visibility_control.h
#ifndef CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_ // 如果没有定义 CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_
#define CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_ // 定义 CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_
#ifdef __cplusplus // 如果使用 C++ 编译器
extern "C" // 使用 C 语言的链接方式
{
#endif
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// 这段逻辑借鉴并命名空间化自 gcc wiki 上的示例:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__ // 如果定义了 _WIN32 或 __CYGWIN__
#ifdef __GNUC__ // 如果使用 GNU 编译器
#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 // 如果正在构建 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 // 如果 GNU 编译器版本大于等于 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 // 如果使用 C++ 编译器
}
#endif
#endif // CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_ // 结束预处理指令
2. 编写动作服务器
让我们专注于编写一个动作服务器,使用我们在创建动作教程中创建的动作https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Creating-an-Action.html 来计算斐波那契序列。
2.1 编写动作服务器代码
打开 custom_action_cpp/src/fibonacci_action_server.cpp
,然后输入以下代码:
#include <functional> // 包含功能库
#include <memory> // 包含内存管理库
#include <thread> // 包含线程库
#include "custom_action_interfaces/action/fibonacci.hpp" // 包含自定义动作接口的斐波那契头文件
#include "rclcpp/rclcpp.hpp" // 包含ROS 2的C++客户端库
#include "rclcpp_action/rclcpp_action.hpp" // 包含ROS 2的动作库
#include "rclcpp_components/register_node_macro.hpp" // 包含ROS 2的节点注册宏
#include "custom_action_cpp/visibility_control.h" // 包含自定义动作的可见性控制头文件
namespace custom_action_cpp // 定义命名空间 custom_action_cpp
{
class FibonacciActionServer : public rclcpp::Node // 定义 FibonacciActionServer 类,继承自 rclcpp::Node
{
public:
using Fibonacci = custom_action_interfaces::action::Fibonacci; // 定义 Fibonacci 类型
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>; // 定义 GoalHandleFibonacci 类型
CUSTOM_ACTION_CPP_PUBLIC // 定义公共可见性
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) // 显式构造函数
: Node("fibonacci_action_server", options) // 调用基类构造函数,初始化节点名称为 "fibonacci_action_server"
{
using namespace std::placeholders; // 使用占位符命名空间
auto handle_goal = [this](
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; // 忽略 uuid
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; // 接受并执行目标
};
auto handle_cancel = [this](
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); // 打印接收到的取消请求
(void)goal_handle; // 忽略 goal_handle
return rclcpp_action::CancelResponse::ACCEPT; // 接受取消请求
};
auto handle_accepted = [this](
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
// this needs to return quickly to avoid blocking the executor,
// 需要快速返回以避免阻塞执行器,
// so we declare a lambda function to be called inside a new thread
// 因此我们声明一个 lambda 函数在新线程中调用
auto execute_in_thread = this, goal_handle{return this->execute(goal_handle);}; // 定义在新线程中执行的 lambda 函数
std::thread{execute_in_thread}.detach(); // 创建并分离线程
};
this->action_server_ = rclcpp_action::create_server<Fibonacci>( // 创建动作服务器
this, // 当前节点
"fibonacci", // 动作名称
handle_goal, // 处理目标请求的函数
handle_cancel, // 处理取消请求的函数
handle_accepted); // 处理接受请求的函数
}
private:
rclcpp_action::Server<Fibonacci>::SharedPtr action_server_; // 动作服务器的共享指针
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle) { // 执行目标的函数
RCLCPP_INFO(this->get_logger(), "Executing goal"); // 打印正在执行目标
rclcpp::Rate loop_rate(1); // 设置循环频率为1Hz
const auto goal = goal_handle->get_goal(); // 获取目标
auto feedback = std::make_shared<Fibonacci::Feedback>(); // 创建反馈的共享指针
auto & sequence = feedback->partial_sequence; // 获取部分序列的引用
sequence.push_back(0); // 添加初始值0
sequence.push_back(1); // 添加初始值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()) { // 如果 ROS 2 正常运行
result->sequence = sequence; // 设置结果序列
goal_handle->succeed(result); // 目标成功
RCLCPP_INFO(this->get_logger(), "Goal succeeded"); // 打印目标成功
}
};
}; // class FibonacciActionServer
} // namespace custom_action_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(custom_action_cpp::FibonacciActionServer) // 注册 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<Fibonacci>( // 创建动作服务器
this, // 当前节点
"fibonacci", // 动作名称
handle_goal, // 处理目标请求的函数
handle_cancel, // 处理取消请求的函数
handle_accepted); // 处理接受请求的函数
动作服务器需要 6 件事:
模板化的动作类型名称:
Fibonacci
。一个 ROS 2 节点用于添加动作:
this
。动作名称:
'fibonacci'
。处理目标的回调函数:
handle_goal
处理取消的回调函数:
handle_cancel
。处理目标接受的回调函数:
handle_accept
。
各种回调的实现是在构造函数中用 lambda 表达式完成的。请注意,所有回调都需要快速返回,否则我们冒着使执行器饥饿的风险。
我们从处理新目标的回调开始:
auto handle_goal = [this](
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;
};
这个实现只接受所有目标。
接下来是处理取消的回调:
auto handle_cancel = [this](
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;
};
这个实现只是告诉客户它接受了取消。
最后一个回调接受一个新目标并开始处理它:
auto handle_accepted = [this](
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
// this needs to return quickly to avoid blocking the executor,
// so we declare a lambda function to be called inside a new thread
auto execute_in_thread = [this, goal_handle](){return this->execute(goal_handle);};
std::thread{execute_in_thread}.detach();
};
由于执行是一个长时间运行的操作,我们会分出一个线程来进行实际工作,并且从 handle_accepted
迅速返回。
所有进一步的处理和更新都在新线程中的 execute
方法中完成:
// 定义执行函数,参数为Fibonacci目标句柄的共享指针
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle) {
// 输出信息日志,打印"Executing goal"
RCLCPP_INFO(this->get_logger(), "执行目标");
// 创建循环速率对象,频率为1Hz
rclcpp::Rate loop_rate(1);
// 获取目标
const auto goal = goal_handle->get_goal();
// 创建Fibonacci反馈的共享指针
auto feedback = std::make_shared<Fibonacci::Feedback>();
// 获取反馈的部分序列
auto & sequence = feedback->partial_sequence;
// 将0和1添加到序列中
sequence.push_back(0);
sequence.push_back(1);
// 创建Fibonacci结果的共享指针
auto result = std::make_shared<Fibonacci::Result>();
// 循环,直到达到目标顺序或rclcpp不再ok
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
// 检查是否有取消请求
if (goal_handle->is_canceling()) {
// 如果有,将序列设置为结果,取消目标,并输出信息日志,打印"Goal canceled"
result->sequence = sequence;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "目标被取消");
return;
}
// 更新序列
sequence.push_back(sequence[i] + sequence[i - 1]);
// 发布反馈
goal_handle->publish_feedback(feedback);
// 输出信息日志,打印"Publish feedback"
RCLCPP_INFO(this->get_logger(), "发布反馈");
// 按照循环速率休眠
loop_rate.sleep();
}
// 检查目标是否完成
if (rclcpp::ok()) {
// 如果完成,将序列设置为结果,成功完成目标,并输出信息日志,打印"Goal succeeded"
result->sequence = sequence;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "目标成功");
}
};
获取目标、创建反馈指针、创建结果指针。循环遍历斐波那契数列{ (如果目标正在取消,设置结果序列,取消目标) 计算下一个斐波那契数,发布反馈。}如果ROS2正常运行,设置结果序列,发送成功结果
这个工作线程每秒处理一个斐波那契序列的序号,并为每一步发布一个反馈更新。当它处理完成后,它将 goal_handle
标记为成功,并退出。
我们现在有了一个功能齐全的动作服务器。让我们构建并运行它。
2.2 编译动作服务器
在上一节中,我们已经将动作服务器代码放到了适当的位置。为了使其编译并运行,我们还需要做一些额外的工作。
首先我们需要设置 CMakeLists.txt,以便编译动作服务器。打开 custom_action_cpp/CMakeLists.txt
,并在 find_package
调用之后立即添加以下内容:
// 添加一个名为action_server的共享库,源文件为src/fibonacci_action_server.cpp
add_library(action_server SHARED
src/fibonacci_action_server.cpp)
// 为action_server目标添加私有包含目录,这些目录在构建和安装阶段有所不同
target_include_directories(action_server PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> // 构建阶段的包含目录
$<INSTALL_INTERFACE:include>) // 安装阶段的包含目录
// 为action_server目标添加私有编译定义
target_compile_definitions(action_server
PRIVATE "CUSTOM_ACTION_CPP_BUILDING_DLL")
// 为action_server目标添加ament依赖项
ament_target_dependencies(action_server
"custom_action_interfaces" // 自定义动作接口
"rclcpp" // ROS客户端库
"rclcpp_action" // ROS动作客户端库
"rclcpp_components") // ROS组件库
// 注册一个名为action_server的节点插件,插件名为custom_action_cpp::FibonacciActionServer,可执行文件名为fibonacci_action_server
rclcpp_components_register_node(action_server PLUGIN "custom_action_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server)
// 安装目标文件
install(TARGETS
action_server // 要安装的目标
ARCHIVE DESTINATION lib // 静态库的目标目录
LIBRARY DESTINATION lib // 动态库的目标目录
RUNTIME DESTINATION bin) // 可执行文件的目标目录
现在我们可以编译包了。转到 ros2_ws
的顶层,然后运行:
colcon build
这应该编译整个工作空间,包括 custom_action_cpp
包中的 fibonacci_action_server
。
cxy@ubuntu2404-cxy:~/ros2_ws$ colcon build --packages-select custom_action_cpp
Starting >>> custom_action_cpp
Finished <<< custom_action_cpp [14.9s]
Summary: 1 package finished [15.1s]
2.3 运行动作服务器
现在我们已经构建了动作服务器,我们可以运行它。源自我们刚刚构建的工作空间( ros2_ws
),并尝试运行动作服务器:
ros2 run custom_action_cpp fibonacci_action_server
3. 编写动作客户端
3.1 编写动作客户端代码
打开 custom_action_cpp/src/fibonacci_action_client.cpp
,然后输入以下代码:ROS2动作客户端的实现,它创建了一个名为FibonacciActionClient的类,用于向动作服务器发送Fibonacci序列计算的目标,并处理服务器的响应和反馈。
// 包含C++标准库的功能头文件
#include <functional>
// 包含C++标准库的未来头文件,用于异步操作
#include <future>
// 包含C++标准库的内存头文件
#include <memory>
// 包含C++标准库的字符串头文件
#include <string>
// 包含C++标准库的字符串流头文件
#include <sstream>
// 包含自定义的Fibonacci动作接口
#include "custom_action_interfaces/action/fibonacci.hpp"
// 包含ROS2的节点类头文件
#include <rclcpp/rclcpp.hpp>
// 包含ROS2的动作客户端类头文件
#include <rclcpp_action/rclcpp_action.hpp>
// 包含ROS2的节点注册宏头文件
#include <rclcpp_components/register_node_macro.hpp>
// 定义自定义动作的命名空间
namespace custom_action_cpp
{
// 创建一个Fibonacci动作客户端类,继承自rclcpp::Node
class FibonacciActionClient : public rclcpp::Node
{
public:
// 使用Fibonacci动作和目标句柄的别名
using Fibonacci = custom_action_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
// 构造函数,初始化节点
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)
{
// 创建Fibonacci动作客户端
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
this,
"fibonacci");
// 定义定时器回调函数,用于发送目标
auto timer_callback_lambda = [this](){ return this->send_goal(); };
// 创建定时器,每500毫秒调用一次回调函数
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
timer_callback_lambda);
}
// 发送目标的函数
void send_goal()
{
// 使用std::placeholders命名空间
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();
}
// 创建一个Fibonacci目标消息
auto goal_msg = Fibonacci::Goal();
// 设置目标阶数为10
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 = [this](const GoalHandleFibonacci::SharedPtr & goal_handle)
{
// 如果目标被服务器拒绝,则记录错误
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");
}
};
// 设置反馈回调函数
send_goal_options.feedback_callback = [this](
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());
};
// 设置结果回调函数
send_goal_options.result_callback = [this](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();
};
// 异步发送目标
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
private:
// 定义Fibonacci动作客户端的共享指针
rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
// 定义定时器的共享指针
rclcpp::TimerBase::SharedPtr timer_;
}; // class FibonacciActionClient
} // namespace custom_action_cpp
// 注册FibonacciActionClient节点
RCLCPP_COMPONENTS_REGISTER_NODE(custom_action_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");
一个动作客户端需要三样东西:
模板化的动作类型名称:
Fibonacci
。一个 ROS 2 节点用于添加动作客户端到:
this
。动作名称:
'fibonacci'
。
我们还实例化了一个 ROS 定时器,它将启动对 send_goal
的唯一调用:
auto timer_callback_lambda = [this](){ return this->send_goal(); };
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
timer_callback_lambda);
当计时器到期时,它将调用 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 = [this](const GoalHandleFibonacci::SharedPtr & goal_handle)
{
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");
}
};
send_goal_options.feedback_callback = [this](
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());
};
send_goal_options.result_callback = [this](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();
};
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
此函数执行以下操作:
取消计时器(因此它只被调用一次)。
等待动作服务器启动。
实例化一个新的
Fibonacci::Goal
。设置响应、反馈和结果回调。
将目标发送到服务器。
当服务器接收并接受目标时,它会向客户端发送响应。该响应由 goal_response_callback
处理:
// 设置目标响应回调函数
send_goal_options.goal_response_callback = [this](const GoalHandleFibonacci::SharedPtr & goal_handle)
{
// 如果目标句柄不存在,说明目标被服务器拒绝
if (!goal_handle) {
// 输出错误日志,打印"Goal was rejected by server"
RCLCPP_ERROR(this->get_logger(), "目标被服务器拒绝");
} else {
// 否则,输出信息日志,打印"Goal accepted by server, waiting for result"
RCLCPP_INFO(this->get_logger(), "目标被服务器接受,等待结果");
}
};
假设服务器接受了目标,它将开始处理。任何对客户端的反馈都将由 feedback_callback
处理:
// 设置反馈回调函数
send_goal_options.feedback_callback = [this](
GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
// 创建一个字符串流
std::stringstream ss;
// 向字符串流中添加字符串"Next number in sequence received: "
ss << "接收到序列中的下一个数字:";
// 遍历反馈序列中的每一个数字
for (auto number : feedback->partial_sequence) {
// 将数字添加到字符串流中
ss << number << " ";
}
// 输出信息日志,打印反馈序列
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
};
当服务器处理完毕后,它会向客户端返回一个结果。结果由 result_callback
处理:
// 设置目标回调函数
send_goal_options.result_callback = [this](const GoalHandleFibonacci::WrappedResult & result)
{
// 根据结果代码进行判断
switch (result.code) {
// 如果结果代码为SUCCEEDED(成功)
case rclcpp_action::ResultCode::SUCCEEDED:
// 不执行任何操作
break;
// 如果结果代码为ABORTED(中止)
case rclcpp_action::ResultCode::ABORTED:
// 输出错误日志,目标被中止
RCLCPP_ERROR(this->get_logger(), "目标被中止");
// 返回,不再执行后续代码
return;
// 如果结果代码为CANCELED(取消)
case rclcpp_action::ResultCode::CANCELED:
// 输出错误日志,目标被取消
RCLCPP_ERROR(this->get_logger(), "目标被取消");
// 返回,不再执行后续代码
return;
// 如果结果代码为其他值
default:
// 输出错误日志,未知的结果代码
RCLCPP_ERROR(this->get_logger(), "未知的结果代码");
// 返回,不再执行后续代码
return;
}
// 创建一个字符串流
std::stringstream ss;
// 向字符串流中添加字符串"Result received: "
ss << "接收到结果:";
// 遍历结果序列中的每一个数字
for (auto number : result.result->sequence) {
// 将数字添加到字符串流中
ss << number << " ";
}
// 输出信息日志,打印结果序列
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
// 关闭rclcpp
rclcpp::shutdown();
};
我们现在有了一个功能齐全的动作客户端。让我们开始构建并运行它。
3.2 编译动作客户端
在前一节中,我们已经将动作客户端代码放到了适当的位置。为了使其编译并运行,我们还需要做一些额外的工作。
首先我们需要设置 CMakeLists.txt,以便编译动作客户端。打开 custom_action_cpp/CMakeLists.txt
,并在 find_package
调用之后立即添加以下内容:
cmake_minimum_required(VERSION 3.8) // 设置 CMake 的最低版本要求为 3.8
project(custom_action_cpp) // 定义项目名称为 custom_action_cpp
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") // 如果编译器是 GNU C++ 或 Clang
add_compile_options(-Wall -Wextra -Wpedantic) // 添加编译选项:显示所有警告、额外警告和严格的编译检查
endif()
# find dependencies
# 查找依赖项
find_package(ament_cmake REQUIRED) // 查找 ament_cmake 包
find_package(custom_action_interfaces REQUIRED) // 查找 custom_action_interfaces 包
find_package(rclcpp REQUIRED) // 查找 rclcpp 包
find_package(rclcpp_action REQUIRED) // 查找 rclcpp_action 包
find_package(rclcpp_components REQUIRED) // 查找 rclcpp_components 包
add_library(action_server SHARED // 添加共享库 action_server
src/fibonacci_action_server.cpp) // 源文件为 src/fibonacci_action_server.cpp
target_include_directories(action_server PRIVATE // 设置 action_server 的包含目录
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> // 构建时的包含目录
$<INSTALL_INTERFACE:include>) // 安装时的包含目录
target_compile_definitions(action_server // 设置 action_server 的编译定义
PRIVATE "CUSTOM_ACTION_CPP_BUILDING_DLL") // 定义 CUSTOM_ACTION_CPP_BUILDING_DLL
ament_target_dependencies(action_server // 设置 action_server 的依赖项
"custom_action_interfaces" // 依赖 custom_action_interfaces
"rclcpp" // 依赖 rclcpp
"rclcpp_action" // 依赖 rclcpp_action
"rclcpp_components") // 依赖 rclcpp_components
rclcpp_components_register_node(action_server PLUGIN "custom_action_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server) // 注册节点 FibonacciActionServer 并生成可执行文件 fibonacci_action_server
install(TARGETS // 安装目标
action_server // 安装 action_server
ARCHIVE DESTINATION lib // 安装静态库到 lib 目录
LIBRARY DESTINATION lib // 安装共享库到 lib 目录
RUNTIME DESTINATION bin) // 安装可执行文件到 bin 目录
add_library(action_client SHARED // 添加共享库 action_client
src/fibonacci_action_client.cpp) // 源文件为 src/fibonacci_action_client.cpp
target_include_directories(action_client PRIVATE // 设置 action_client 的包含目录
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> // 构建时的包含目录
$<INSTALL_INTERFACE:include>) // 安装时的包含目录
target_compile_definitions(action_client // 设置 action_client 的编译定义
PRIVATE "CUSTOM_ACTION_CPP_BUILDING_DLL") // 定义 CUSTOM_ACTION_CPP_BUILDING_DLL
ament_target_dependencies(action_client // 设置 action_client 的依赖项
"custom_action_interfaces" // 依赖 custom_action_interfaces
"rclcpp" // 依赖 rclcpp
"rclcpp_action" // 依赖 rclcpp_action
"rclcpp_components") // 依赖 rclcpp_components
rclcpp_components_register_node(action_client PLUGIN "custom_action_cpp::FibonacciActionClient" EXECUTABLE fibonacci_action_client) // 注册节点 FibonacciActionClient 并生成可执行文件 fibonacci_action_client
install(TARGETS // 安装目标
action_client // 安装 action_client
ARCHIVE DESTINATION lib // 安装静态库到 lib 目录
LIBRARY DESTINATION lib // 安装共享库到 lib 目录
RUNTIME DESTINATION bin) // 安装可执行文件到 bin 目录
if(BUILD_TESTING) // 如果启用了测试
find_package(ament_lint_auto REQUIRED) // 查找 ament_lint_auto 包
# the following line skips the linter which checks for copyrights
# 以下行跳过检查版权的 linter
# comment the line when a copyright and license is added to all source files
# 当所有源文件添加了版权和许可证时,注释掉此行
set(ament_cmake_copyright_FOUND TRUE) // 设置 ament_cmake_copyright_FOUND 为 TRUE
# the following line skips cpplint (only works in a git repo)
# 以下行跳过 cpplint(仅在 git 仓库中有效)
# comment the line when this package is in a git repo and when
# 当此包在 git 仓库中并且所有源文件添加了版权和许可证时,注释掉此行
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE) // 设置 ament_cmake_cpplint_FOUND 为 TRUE
ament_lint_auto_find_test_dependencies() // 自动查找测试依赖项
endif()
ament_package() // 声明这是一个 ament 包
现在我们可以编译包了。转到 ros2_ws
的顶层,然后运行:
colcon build
cxy@ubuntu2404-cxy:~/ros2_ws$ colcon build --packages-select custom_action_cpp
Starting >>> custom_action_cpp
Finished <<< custom_action_cpp [6.69s]
Summary: 1 package finished [7.46s]
这应该编译整个工作空间,包括 custom_action_cpp
包中的 fibonacci_action_client
。
3.3 运行动作客户端
现在我们已经构建了动作客户端,我们可以运行它。首先确保一个动作服务器在一个单独的终端中运行。现在源代码工作区我们刚刚构建的 ( ros2_ws
),并尝试运行动作客户端:
ros2 run custom_action_cpp fibonacci_action_client
您应该会看到日志消息,记录了目标被接受、反馈被打印以及最终结果。
摘要
在本教程中,您将逐行组建一个 C++动作服务器和动作客户端,并配置它们以交换目标、反馈和结果。
相关内容
您可以通过几种方式用 C++编写动作服务器和客户端;请查看 ros2/examples 仓库 https://github.com/ros2/examples/tree/jazzy/rclcpp 中的
minimal_action_server
和minimal_action_client
包。有关 ROS 操作的更详细信息,请参阅设计文章https://design.ros2.org/articles/actions.html 。
笔 记
rclcpp::Rate loop_rate(1); // 设置循环频率为1Hz
loop_rate.sleep(); // 休眠一段时间