ROS2动作机制详解：C++实现异步任务与机器人控制

1. 从“服务”到“动作”：为什么ROS2的Action是异步任务的首选

如果你已经用ROS2写过几个服务（Service）的客户端和服务端，可能会觉得服务调用已经能解决大部分“请求-响应”式的通信需求了。但当你真正开始构建一个需要长时间运行、可能被取消、并且需要实时反馈进度的机器人任务时，比如让机械臂移动到某个位置，或者让移动机器人导航到目标点，你就会发现单纯的服务调用显得力不从心。这时，ROS2的“动作”（Action）就该登场了。

简单来说，动作是ROS2中用于处理长时间运行、可抢占、带反馈的异步任务的通信机制。它本质上是一个“客户端-服务器”模型，但比服务复杂得多，也更强大。一个动作由三部分组成：目标（Goal）、反馈（Feedback）和结果（Result）。客户端向服务器发送一个目标，服务器开始执行一个可能耗时的任务。在执行过程中，服务器会周期性地向客户端发送反馈，告知当前进度或状态。最终，任务完成后，服务器会返回一个结果，告知成功或失败。

为什么不用服务模拟呢？想象一下用服务让机器人导航：客户端发送目标点，服务端开始导航。导航可能需要几十秒，期间客户端只能干等，无法知道机器人是卡住了还是在正常行进。如果想中途取消任务，服务机制没有原生的取消接口。而动作机制完美解决了这些问题：反馈让你能实时看到进度条，取消机制让你能在紧急情况下安全地中断任务。

在C++中实现动作，意味着我们将深入ROS2的中间件层，理解其基于“唯一标识符（UUID）”的任务管理、非阻塞的通信以及状态机管理。这对于构建健壮、可交互的机器人应用至关重要。接下来，我将以一个具体的例子——模拟一个执行时间可变的“计数”任务——来手把手带你用C++实现一个完整的动作服务器和客户端，并深入每一个细节和可能踩到的坑。

2. 动作通信的底层逻辑与接口定义

在动手写代码之前，我们必须彻底理解动作在ROS2中是如何工作的，以及如何定义我们自己的动作接口。这能避免后续很多“为什么代码不工作”的困惑。

2.1 动作：一个加强版的“服务+话题”复合体

你可以把动作服务器（Action Server）和动作客户端（Action Client）之间的交互，想象成一次有状态的、可监控的远程过程调用。

1. 目标发送与接受：客户端发送一个目标（Goal）到服务器。这类似于服务调用，但关键区别在于，服务器在收到目标后，会立即返回一个确认（ACK），并开始异步执行，而不是同步计算并返回结果。
2. 反馈流：在任务执行期间，服务器会通过一个独立的反馈话题（Feedback Topic）持续向客户端发送消息。这是一个单向的、持续的数据流，类似于发布者-订阅者模型。
3. 结果返回：任务最终完成后（无论是成功、取消还是失败），服务器会通过另一个通道将结果（Result）发送给客户端。这又类似于服务的响应。

ROS2使用三个底层的话题来实现这一机制：

• /_action/feedback：用于传输反馈流。
• /_action/status：用于传输动作服务器的状态（如空闲、执行中、取消中）。
• /_action/result：用于传输最终结果。

对于开发者而言，我们不需要直接操作这些底层话题。ROS2的rclcpp_action库为我们提供了高级的、易于使用的客户端和服务器类，封装了所有这些复杂性。

2.2 定义专属的动作接口（.action文件）

和消息（.msg）与服务（.srv）一样，动作也有自己的接口定义文件，后缀为.action。这个文件定义了Goal、Feedback和Result的数据结构。

让我们创建一个简单的CountUntil.action动作。假设我们想让服务器从一个数字开始计数到另一个数字，每秒计一个数，并反馈当前进度。

在功能包的action目录下创建CountUntil.action文件：

# 目标：告诉服务器要数到哪里 int64 target_number # 注意：这里可以添加更多字段，例如计数步长 --- # 结果：任务完成后的最终结果 int64 reached_number --- # 反馈：执行过程中周期性发送的进度 int64 current_number float32 progress_percentage

文件结构解析：

• 第一部分（第一个---之前）是Goal的定义。我们定义了一个target_number，客户端将用它来告诉服务器“数到多少”。
• 第二部分（第一个---和第二个---之间）是Result的定义。我们定义了一个reached_number，服务器用它来告诉客户端“最终数到了多少”。在简单场景下，这可能就等于target_number，但在被取消或出错时，可能不同。
• 第三部分（第二个---之后）是Feedback的定义。我们定义了两个字段：current_number表示当前数到的数字，progress_percentage表示进度百分比，这是一个非常实用的设计。

注意：在修改了.action文件后，必须在CMakeLists.txt和package.xml中添加相应的依赖和编译指令，然后重新编译功能包，C++头文件才会被生成。这是新手最常忘记的一步，会导致#include “功能包/action/count_until.hpp”失败。

在CMakeLists.txt中需要添加：

find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "action/CountUntil.action" )

在package.xml中需要添加：

<buildtool_depend>rosidl_default_generators</buildtool_depend> <depend>action_msgs</depend> <member_of_group>rosidl_interface_packages</member_of_group>

3. 构建动作服务器：处理目标、反馈与取消

动作服务器是执行任务的核心。我们需要处理来自客户端的三个主要请求：接受/拒绝目标、执行任务并发送反馈、处理取消请求。

3.1 服务器类的基本框架

首先，我们创建一个继承自rclcpp::Node的类CountUntilServerNode。

#include “rclcpp/rclcpp.hpp” #include “rclcpp_action/rclcpp_action.hpp” #include “your_package_name/action/count_until.hpp” // 替换为你的功能包名 using CountUntil = your_package_name::action::CountUntil; using GoalHandleCountUntil = rclcpp_action::ServerGoalHandle<CountUntil>; class CountUntilServerNode : public rclcpp::Node { public: CountUntilServerNode() : Node(“count_until_server”) { // 创建动作服务器 this->action_server_ = rclcpp_action::create_server<CountUntil>( this, // 所属节点 “count_until”, // 动作名称 std::bind(&CountUntilServerNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&CountUntilServerNode::handle_cancel, this, std::placeholders::_1), std::bind(&CountUntilServerNode::handle_accepted, this, std::placeholders::_1) ); RCLCPP_INFO(this->get_logger(), “动作服务器已启动，等待目标...”); } private: rclcpp_action::Server<CountUntil>::SharedPtr action_server_; // 后续将添加三个核心回调函数：handle_goal, handle_cancel, handle_accepted };

关键点在于rclcpp_action::create_server函数，它需要三个回调函数：

1. handle_goal: 当新目标到达时被调用，决定是接受还是拒绝这个目标。
2. handle_cancel: 当客户端请求取消当前正在执行的目标时被调用。
3. handle_accepted: 当目标被接受后，启动实际的任务执行线程。

3.2 实现目标处理回调（handle_goal）

这个回调函数用于验证目标的合法性。例如，我们可以拒绝负数目标。

rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const CountUntil::Goal> goal) { (void)uuid; // 暂时未使用UUID，但保留以保持接口一致 RCLCPP_INFO(this->get_logger(), “收到新目标：数到 %ld”, goal->target_number); // 目标验证逻辑 if (goal->target_number <= 0) { RCLCPP_WARN(this->get_logger(), “目标值 %ld 无效（必须为正数），拒绝目标。”, goal->target_number); return rclcpp_action::GoalResponse::REJECT; } // 防止服务器过载：如果已经在执行任务，拒绝新目标 // 这里需要一个标志位来跟踪执行状态，为了简化，我们先假设每次只处理一个目标 // 更健壮的实现需要维护一个目标队列或状态机 RCLCPP_INFO(this->get_logger(), “目标有效，已接受。”); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; // 接受并立即执行 }

ACCEPT_AND_EXECUTE表示接受目标并准备执行。还有一个选项是ACCEPT_AND_DEFER，表示接受但延迟执行，适用于更复杂的任务调度。

3.3 实现取消处理回调（handle_cancel）

取消请求可能在任何时候发生，服务器必须优雅地处理它。

rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr<GoalHandleCountUntil> goal_handle) { RCLCPP_INFO(this->get_logger(), “收到取消请求。”); // 在实际应用中，这里应该设置一个取消标志，让执行循环检查并退出。 // 例如：goal_handle->get_goal_id() 对应的任务线程应该被中断。 // 为了演示，我们直接返回同意取消。 (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; }

ACCEPT表示服务器同意取消。如果任务无法取消（例如，一个不可中断的硬件操作），可以返回REJECT。重要：仅仅返回ACCEPT并不会自动停止任务线程，你必须在执行线程中检查取消状态。

3.4 实现任务执行与反馈（handle_accepted）

这是最核心的部分。一旦目标被接受，这个函数被调用，它负责启动一个执行任务的线程（或协程），并管理反馈和结果的发送。

void handle_accepted(const std::shared_ptr<GoalHandleCountUntil> goal_handle) { RCLCPP_INFO(this->get_logger(), “目标已被接受，开始执行...”); // 使用std::thread在新线程中执行任务，避免阻塞ROS2的executor。 std::thread{std::bind(&CountUntilServerNode::execute_counting, this, std::placeholders::_1), goal_handle}.detach(); } void execute_counting(const std::shared_ptr<GoalHandleCountUntil> goal_handle) { const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared<CountUntil::Feedback>(); auto result = std::make_shared<CountUntil::Result>(); int64_t current_number = 0; int64_t target = goal->target_number; rclcpp::Rate loop_rate(1); // 1 Hz，每秒一次 for (int64_t i = 1; i <= target; ++i) { // 检查是否收到取消请求 if (goal_handle->is_canceling()) { result->reached_number = current_number; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), “任务在数到 %ld 时被取消。”, current_number); return; } // 执行“计数”工作 current_number = i; RCLCPP_INFO(this->get_logger(), “计数: %ld”, current_number); // 发布反馈 feedback->current_number = current_number; feedback->progress_percentage = (static_cast<float>(current_number) / target) * 100.0; goal_handle->publish_feedback(feedback); loop_rate.sleep(); } // 任务成功完成 result->reached_number = current_number; goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), “任务完成！最终计数: %ld”, result->reached_number); }

关键细节与避坑指南：

1. 线程分离：handle_accepted中必须使用std::thread并将线程分离（detach）或妥善管理其生命周期。如果直接在主线程（回调组线程）中执行耗时的execute_counting，会阻塞整个节点的其他回调（如定时器、订阅者），导致节点“卡死”。
2. 取消检查：在执行循环中，必须定期调用goal_handle->is_canceling()来检查取消状态。这是实现可取消任务的唯一方式。如果检查到取消，必须调用goal_handle->canceled(result)来通知客户端任务已取消，并返回一个结果（通常是当前进度）。
3. 反馈发布：使用goal_handle->publish_feedback(feedback)发布反馈。频率不宜过高，通常1-10Hz足够，避免给网络带来不必要的负担。
4. 结果终态：任务结束时，必须调用goal_handle->succeed(result)、goal_handle->canceled(result)或goal_handle->abort(result)来设置最终状态。如果不调用，客户端将永远等待结果，导致资源泄漏。abort通常用于任务因错误而失败的情况。

4. 构建动作客户端：发送目标与处理异步响应

客户端负责发起任务、监控进度并处理最终结果。它的工作流程也是异步的。

4.1 客户端类的基本框架与目标发送

#include “rclcpp/rclcpp.hpp” #include “rclcpp_action/rclcpp_action.hpp” #include “your_package_name/action/count_until.hpp” #include <chrono> #include <functional> using CountUntil = your_package_name::action::CountUntil; using GoalHandleCountUntil = rclcpp_action::ClientGoalHandle<CountUntil>; class CountUntilClientNode : public rclcpp::Node { public: CountUntilClientNode() : Node(“count_until_client”) { this->client_ = rclcpp_action::create_client<CountUntil>(this, “count_until”); RCLCPP_INFO(this->get_logger(), “动作客户端已创建。”); // 示例：启动一个定时器，在3秒后发送目标 timer_ = this->create_wall_timer( std::chrono::seconds(3), std::bind(&CountUntilClientNode::send_goal, this)); } void send_goal() { timer_->cancel(); // 只发送一次 if (!client_->wait_for_action_server(std::chrono::seconds(5))) { RCLCPP_ERROR(this->get_logger(), “动作服务器未在5秒内响应。”); rclcpp::shutdown(); return; } auto goal = CountUntil::Goal(); goal.target_number = 10; // 设置目标：数到10 // 设置发送选项 auto send_goal_options = rclcpp_action::Client<CountUntil>::SendGoalOptions(); send_goal_options.goal_response_callback = std::bind(&CountUntilClientNode::goal_response_callback, this, std::placeholders::_1); send_goal_options.feedback_callback = std::bind(&CountUntilClientNode::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); send_goal_options.result_callback = std::bind(&CountUntilClientNode::result_callback, this, std::placeholders::_1); RCLCPP_INFO(this->get_logger(), “正在发送目标：数到 %ld”, goal.target_number); client_->async_send_goal(goal, send_goal_options); } private: rclcpp_action::Client<CountUntil>::SharedPtr client_; rclcpp::TimerBase::SharedPtr timer_; // 后续将添加三个回调函数：goal_response_callback, feedback_callback, result_callback };

async_send_goal是异步发送函数，它立即返回，不会阻塞。我们需要通过SendGoalOptions来设置三个回调函数，以处理服务器的响应。

4.2 处理目标响应、反馈和结果

这三个回调函数分别对应了动作生命周期的不同阶段。

void goal_response_callback(std::shared_future<GoalHandleCountUntil::SharedPtr> future) { auto goal_handle = future.get(); if (!goal_handle) { RCLCPP_ERROR(this->get_logger(), “目标被服务器拒绝。”); } else { RCLCPP_INFO(this->get_logger(), “目标已被服务器接受，任务ID: %s”, rclcpp_action::to_string(goal_handle->get_goal_id()).c_str()); // 可以在这里保存goal_handle，用于后续可能的取消操作 } } void feedback_callback( GoalHandleCountUntil::SharedPtr, const std::shared_ptr<const CountUntil::Feedback> feedback) { // 第一个参数是goal_handle，这里我们不需要使用它 RCLCPP_INFO(this->get_logger(), “收到反馈: 当前数 %ld, 进度 %.1f%%”, feedback->current_number, feedback->progress_percentage); } void result_callback(const GoalHandleCountUntil::WrappedResult & result) { switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: RCLCPP_INFO(this->get_logger(), “任务成功！最终数: %ld”, result.result->reached_number); break; case rclcpp_action::ResultCode::CANCELED: RCLCPP_WARN(this->get_logger(), “任务被取消。最终数: %ld”, result.result->reached_number); break; case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(this->get_logger(), “任务失败（中止）。最终数: %ld”, result.result->reached_number); break; default: RCLCPP_ERROR(this->get_logger(), “未知结果码。”); break; } // 结果收到后，可以安全地关闭节点或发起新任务 rclcpp::shutdown(); }

客户端避坑指南：

1. 等待服务器：在发送目标前，务必使用client_->wait_for_action_server()等待服务器上线。否则async_send_goal会失败。
2. 回调线程安全：这些回调函数在ROS2 Executor的线程中被调用。确保回调函数本身是线程安全的，并且不要执行耗时操作，以免阻塞其他回调。
3. 结果处理：result_callback是任务结束的唯一可靠通知。即使任务被取消或中止，也会调用此回调。务必根据result.code处理所有可能的结果状态。
4. 目标句柄存储：goal_response_callback中返回的goal_handle是后续操作（如取消）的凭据。如果你设计的功能需要支持中途取消，必须将这个goal_handle保存到类的成员变量中。

4.3 实现取消功能：一个完整的客户端示例

让我们扩展客户端，使其在运行5秒后主动取消任务。

class CountUntilClientNode : public rclcpp::Node { public: CountUntilClientNode() : Node(“count_until_client”), goal_handle_(nullptr) { this->client_ = rclcpp_action::create_client<CountUntil>(this, “count_until”); RCLCPP_INFO(this->get_logger(), “动作客户端已创建。”); // 定时器1：3秒后发送目标 send_goal_timer_ = this->create_wall_timer( std::chrono::seconds(3), std::bind(&CountUntilClientNode::send_goal, this)); // 定时器2：发送目标后8秒（即任务开始后约5秒）取消任务 cancel_goal_timer_ = this->create_wall_timer( std::chrono::seconds(8), std::bind(&CountUntilClientNode::cancel_goal, this)); cancel_goal_timer_->cancel(); // 先禁用，等目标发送成功后再激活 } void send_goal() { send_goal_timer_->cancel(); // ... 等待服务器和发送目标的代码与之前相同 ... // 在 goal_response_callback 中，如果目标被接受，则激活取消定时器 // 修改 goal_response_callback: // if (goal_handle) { ...; cancel_goal_timer_->reset(); } } void cancel_goal() { cancel_goal_timer_->cancel(); if (goal_handle_) { RCLCPP_INFO(this->get_logger(), “发送取消请求...”); client_->async_cancel_goal(goal_handle_); } else { RCLCPP_WARN(this->get_logger(), “无有效的目标句柄，无法取消。”); } } private: rclcpp_action::Client<CountUntil>::SharedPtr client_; rclcpp::TimerBase::SharedPtr send_goal_timer_; rclcpp::TimerBase::SharedPtr cancel_goal_timer_; GoalHandleCountUntil::SharedPtr goal_handle_; // 保存目标句柄 };

这样，我们就实现了一个完整的、支持取消的动作客户端。运行这个客户端和之前的服务器，你会看到客户端发送目标，接收反馈，然后在5秒后发送取消请求，服务器响应取消并终止计数。

5. 编译、运行与深度调试技巧

代码写完了，但让它们跑起来并理解其内部状态，还需要一些实操步骤和调试工具。

5.1 编译配置与常见编译错误

确保你的CMakeLists.txt和package.xml配置正确。

CMakeLists.txt关键部分：

find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(your_package_name REQUIRED) # 你的消息/动作接口包 add_executable(server_node src/count_until_server.cpp) ament_target_dependencies(server_node rclcpp rclcpp_action your_package_name) add_executable(client_node src/count_until_client.cpp) ament_target_dependencies(client_node rclcpp rclcpp_action your_package_name) install(TARGETS server_node client_node DESTINATION lib/${PROJECT_NAME})

常见编译错误：

• 找不到动作头文件：检查find_package是否包含了你的接口包，以及rosidl_generate_interfaces是否正确列出了.action文件。编译接口包后，需要source install/setup.bash。
• 链接错误：检查ament_target_dependencies是否包含了所有必要的依赖，特别是rclcpp_action。

5.2 使用命令行工具监控动作

ROS2提供了强大的命令行工具来直观地查看动作的状态，这是调试的利器。

1. 查看动作列表：启动你的服务器节点后，在新的终端运行：

   ros2 action list

   你应该能看到/count_until这个动作。

2. 查看动作信息：

   ros2 action info /count_until

   这会显示该动作的服务器和客户端数量。

3. 手动发送目标/取消（测试服务器）：

   # 发送目标 ros2 action send_goal /count_until your_package_name/action/CountUntil “{target_number: 5}” # 发送目标并请求反馈 ros2 action send_goal /count_until your_package_name/action/CountUntil “{target_number: 5}” --feedback # 在另一个终端，获取目标ID后，可以尝试取消（需要先通过 --feedback 看到goal ID） ros2 action cancel_goal <GOAL_ID>

5.3 实战中的高级模式与经验之谈

在真实机器人项目中，动作的使用会更加复杂。这里分享几个关键经验：

1. 服务器端的并发与队列：上面的示例是“一次只处理一个目标”。在实际中，你可能需要处理并发目标。rclcpp_action::Server本身支持并发，但你的execute_counting函数需要是线程安全的。更常见的模式是使用一个任务队列和工作线程池。当handle_accepted被调用时，将goal_handle放入队列，由一组工作线程取出执行。这需要对goal_handle进行生命周期管理，避免悬空指针。

2. 客户端的目标状态管理：如果你的客户端需要管理多个并发的动作目标，你需要维护一个从goal_id到goal_handle或自定义任务状态的映射。result_callback中需要通过result.goal_id来区分是哪个任务完成了。

3. 反馈与结果的序列化：确保你的反馈和结果消息类型是简单的、可序列化的。避免在反馈中传递大型数据（如图像），这会导致性能问题。对于大量数据，应该通过一个独立的话题（Topic）来传输，而在动作反馈中只传递一个引用（如话题名称或ID）。

4. 超时处理：动作本身没有内置的超时机制。客户端需要自己实现：在发送目标后启动一个定时器，如果超时后仍未收到结果，可以认为任务失败并尝试取消。服务器端也应对长时间运行的任务设置检查点，避免无限期运行。

5. 与有限状态机（FSM）集成：在复杂的机器人行为中，动作常常与有限状态机（如smach2或FlexBE）结合使用。动作可以作为状态机中的一个状态，其执行结果（成功、失败、取消）决定了状态机的转移。这种模式能极大地提升复杂任务编排的可管理性和可读性。

通过这个从原理到实现，再到调试和进阶思考的完整流程，你应该对ROS2中的C++动作有了扎实的理解。动作机制是构建响应式、可监控机器人系统的基石，熟练掌握它，你的机器人代码将从此告别“黑盒”式的漫长等待，变得透明、可控且健壮。