当前位置：首页 > news >正文

ROS2 Action通信：C++实现长时任务管理与反馈机制

news 2026/8/13 23:22:38

1. 从服务到动作：为什么ROS2需要Action？

如果你用过ROS2的服务（Service），可能会觉得它挺方便的：客户端发个请求，服务端给个响应，一来一回，同步完成。但当你需要让机器人去执行一个耗时较长的任务，比如“移动到(x, y)点”，事情就变得棘手了。服务调用会一直阻塞，直到任务完成或超时。在这漫长的几秒甚至几十秒里，你的主程序啥也干不了，更无法得知机器人是卡住了，还是正在顺利移动。你可能会想，那我开个线程去等不就行了？但线程管理、状态同步、取消任务、获取中间进度……这些自己实现起来，又是一堆麻烦。

ROS2的动作（Action）就是为了解决这类“长时、可反馈、可取消”的任务而生的。你可以把它理解为“加强版的服务”。它底层基于话题（Topic）实现，但封装了一套更完善的通信协议。一个Action交互包含三个角色：Action Client（动作客户端）、Action Server（动作服务器）和Action Goal（目标）、Feedback（反馈）、Result（结果）三种消息。客户端发送一个目标（Goal）给服务器，服务器开始执行。在执行过程中，服务器可以持续地向客户端发送反馈（Feedback），比如“已移动30%”。客户端可以随时发送取消请求。最终，服务器完成任务后，会发送一个最终结果（Result）。

这次，我们就用C++，手把手实现一个最简单的Action通信示例：一个模拟的“计数”服务器。客户端设定一个目标数字，服务器从0开始累加，每秒反馈当前数值，累加到目标值后返回结果。通过这个例子，你会彻底搞懂Action的代码骨架、编译运行方法，以及那些官方文档里可能不会细说的调试技巧和常见坑位。

2. 项目结构与依赖准备：CMakeLists.txt与package.xml的配置要点

在开始写代码前，得先把房子（工作空间）和蓝图（编译配置）搭好。假设你的ROS2版本是Humble或Iron，并且已经配置好了基础环境。

首先，创建一个功能包。我习惯将所有Action相关的定义、服务器和客户端代码放在一个包里，结构清晰。

# 在你的工作空间src目录下，比如 ~/ros2_ws/src ros2 pkg create cpp_action_demo --build-type ament_cmake --dependencies rclcpp rclcpp_action rclcpp_components example_interfaces

这里的关键依赖是rclcpp_action，它提供了Action的C++客户端和服务器接口。example_interfaces包里有我们即将用到的Fibonacci.action接口，但为了彻底理解，我们先从自定义Action接口开始。不过，为了让第一个例子足够简单，我们这次先使用ROS2内置的一个简单Action接口：example_interfaces/action/Fibonacci。它是一个计算斐波那契数列的Action，我们稍加改造，用来模拟计数。

现在，看一下自动生成的package.xml，确保依赖项都已包含：

<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>cpp_action_demo</name> <version>0.0.0</version> <description>TODO: Package description</description> <maintainer email="you@example.com">Your Name</maintainer> <license>TODO: License declaration</license> <buildtool_depend>ament_cmake</buildtool_depend> <depend>rclcpp</depend> <depend>rclcpp_action</depend> <depend>rclcpp_components</depend> <depend>example_interfaces</depend> <test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_common</test_depend> <export> <build_type>ament_cmake</build_type> </export> </package>

重点是CMakeLists.txt。自动生成的版本需要添加可执行文件的编译目标。一个常见的错误是忘记链接rclcpp_action库。我们来修改它：

cmake_minimum_required(VERSION 3.8) project(cpp_action_demo) # 默认使用C++17标准，ROS2 Humble之后推荐使用 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() # 查找依赖包 find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(example_interfaces REQUIRED) # 声明可执行文件：Action服务器 add_executable(action_server src/action_server.cpp) ament_target_dependencies(action_server rclcpp rclcpp_action example_interfaces ) # 安装到lib/<pkg_name>目录 install(TARGETS action_server DESTINATION lib/${PROJECT_NAME} ) # 声明可执行文件：Action客户端 add_executable(action_client src/action_client.cpp) ament_target_dependencies(action_client rclcpp rclcpp_action example_interfaces ) install(TARGETS action_client DESTINATION lib/${PROJECT_NAME} ) # 安装启动文件（如果有的话，可以跳过） install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) # 导出包含路径，确保其他包能找到本包的头文件（本例中无自定义头文件，但好习惯是加上） ament_export_include_directories(include) ament_export_dependencies( rclcpp rclcpp_action example_interfaces ) # 最后调用ament_package()，必须 ament_package()

注意ament_target_dependencies这一行，它确保了编译时和运行时都能找到正确的库。很多编译错误（比如找不到rclcpp_action::Client之类的类型）就是因为这里漏了依赖。

3. Action服务器实现详解：从接收到执行的全过程

接下来是重头戏：Action服务器的C++实现。我们创建一个src/action_server.cpp文件。

Action服务器的核心是继承自rclcpp::Node，并创建一个rclcpp_action::Server对象。这个Server需要处理三种回调：

目标处理回调（Handle Goal）：当客户端发送一个新目标时触发。在这里，你可以决定是否接受这个目标。比如，如果服务器正忙，可以拒绝。
取消处理回调（Handle Cancel）：当客户端请求取消当前执行的目标时触发。你需要在这里处理取消逻辑，并返回一个状态，告诉系统取消是否被接受。
执行回调（Execute）：这是任务执行的主函数。它在一个独立的线程中运行（默认由rclcpp_action::Server管理），不会阻塞服务器的其他回调（比如定时器或其他订阅者）。在这里，你要执行实际的任务，并定期发布反馈，最终设置结果。

下面我们结合代码，一步步拆解：

#include <rclcpp/rclcpp.hpp> #include <rclcpp_action/rclcpp_action.hpp> #include <example_interfaces/action/fibonacci.hpp> #include <memory> #include <thread> #include <chrono> using namespace std::chrono_literals; using Fibonacci = example_interfaces::action::Fibonacci; using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>; class FibonacciActionServer : public rclcpp::Node { public: // 构造函数中初始化Action Server FibonacciActionServer() : Node("fibonacci_action_server") { // 使用 create_server 模板函数创建服务器 // 第一个参数是服务器对象指针 // 第二个参数是Action名称，客户端将通过这个名称来连接 // 第三个是目标处理回调（绑定到本类的handle_goal方法） // 第四个是取消处理回调（绑定到本类的handle_cancel方法） // 第五个是执行回调（绑定到本类的handle_accepted方法） this->action_server_ = rclcpp_action::create_server<Fibonacci>( this, "fibonacci", // Action名 std::bind(&FibonacciActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&FibonacciActionServer::handle_cancel, this, std::placeholders::_1), std::bind(&FibonacciActionServer::handle_accepted, this, std::placeholders::_1)); RCLCPP_INFO(this->get_logger(), "Fibonacci Action Server 已启动，等待目标..."); } private: rclcpp_action::Server<Fibonacci>::SharedPtr action_server_; // 1. 处理新目标：决定是否接受 rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const Fibonacci::Goal> goal) { // 这个uuid是系统为每个目标生成的唯一标识符，可用于日志追踪 (void)uuid; // 显式忽略未使用参数警告 RCLCPP_INFO(this->get_logger(), "收到新目标：计算阶数为 %d 的斐波那契数列", goal->order); // 简单的验证逻辑：防止阶数过大导致计算时间过长或溢出 if (goal->order > 20) { RCLCPP_WARN(this->get_logger(), "目标阶数 %d 过大，拒绝执行。", goal->order); return rclcpp_action::GoalResponse::REJECT; } // 还可以检查服务器是否正在执行其他任务，这里我们假设一次只处理一个目标 RCLCPP_INFO(this->get_logger(), "目标被接受。"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; // 接受并立即执行 } // 2. 处理取消请求 rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr<GoalHandleFibonacci> goal_handle) { RCLCPP_INFO(this->get_logger(), "收到取消请求。"); // 在实际应用中，这里应该设置一个标志位，让执行线程安全地停止。 // 为了简单，我们直接返回接受取消。 (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; } // 3. 一旦目标被接受，就调用此函数。它负责启动执行线程。 void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle) { // 这里使用std::thread启动一个新线程来执行任务。 // 注意：必须将goal_handle的值捕获到lambda中，不能直接使用this->goal_handle之类的成员变量， // 因为多个目标可能同时被接受（虽然我们逻辑上串行处理）。 std::thread{std::bind(&FibonacciActionServer::execute, this, std::placeholders::_1), goal_handle}.detach(); } // 4. 真正的执行函数 void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle) { RCLCPP_INFO(this->get_logger(), "开始执行计算..."); rclcpp::Rate loop_rate(1); // 设置反馈频率，1Hz const auto goal = goal_handle->get_goal(); // 获取目标消息 auto feedback = std::make_shared<Fibonacci::Feedback>(); // 创建反馈消息 auto result = std::make_shared<Fibonacci::Result>(); // 创建结果消息 // 初始化斐波那契数列的前两个数 int a = 0, b = 1; feedback->sequence.clear(); feedback->sequence.push_back(a); feedback->sequence.push_back(b); // 循环计算，直到达到目标阶数或被取消 for (int i = 1; i <= goal->order && rclcpp::ok(); ++i) { // 检查是否被取消 if (goal_handle->is_canceling()) { result->sequence = feedback->sequence; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "任务被取消。"); return; } // 计算下一个数并加入反馈序列 int next = a + b; feedback->sequence.push_back(next); a = b; b = next; // 发布反馈 goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), "发布反馈: 已计算 %zu 个数", feedback->sequence.size()); // 模拟耗时计算 loop_rate.sleep(); } // 任务完成，设置结果 result->sequence = feedback->sequence; goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "任务成功完成，最终序列长度: %zu", result->sequence.size()); } }; int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = std::make_shared<FibonacciActionServer>(); rclcpp::spin(node); rclcpp::shutdown(); return 0; }

几个关键点与避坑经验：

GoalResponse的返回值：ACCEPT_AND_EXECUTE表示接受并立即执行。还有一个ACCEPT_AND_DEFER，表示接受但延迟执行，需要你之后手动调用goal_handle->execute()。对于大多数简单场景，直接用ACCEPT_AND_EXECUTE就行。
执行线程的分离：在handle_accepted中，我们创建了一个新线程并调用detach()。这意味着主线程（rclcpp::spin）不会等待这个线程结束。这是标准做法，确保服务器能继续响应其他请求（比如新的目标或取消请求）。切记：不要在这个线程里调用rclcpp::spin，否则会导致多个spin冲突。
资源管理与线程安全：这个简单示例一次只处理一个目标。如果设计成并发处理多个目标，你需要更复杂的逻辑来管理goal_handle和共享数据，避免竞争条件。通常可以为每个目标创建一个独立的std::thread或使用线程池。
反馈与结果的消息类型：feedback和result都是std::shared_ptr类型，指向Action定义中对应的消息结构（Fibonacci::Feedback和Fibonacci::Result）。你需要仔细查看Action的.msg定义，知道里面有哪些字段可以填充。比如Fibonacci.action中，Feedback有一个int32[] sequence字段，Result也有一个int32[] sequence字段。
循环中的rclcpp::ok()：这是一个好习惯，在长时间循环中检查ROS2系统是否还在正常运行（比如没有被Ctrl+C中断）。如果系统关闭了，循环应该退出。

4. Action客户端实现详解：发送、监控与取消

服务器准备好了，现在需要一个客户端来驱动它。创建src/action_client.cpp。

Action客户端的流程比服务器稍简单，但异步操作是其核心，也是容易迷惑的地方。我们将实现一个客户端，它：

连接到指定的Action服务器。
发送一个目标（例如，计算阶数为10的斐波那契数列）。
监听反馈，并打印出来。
等待结果，并处理完成（成功、取消、中止）的情况。
提供一个简单的超时机制。

我们将使用异步方式，这是ROS2 Action客户端推荐的做法，因为它不会阻塞你的主线程。

#include <rclcpp/rclcpp.hpp> #include <rclcpp_action/rclcpp_action.hpp> #include <example_interfaces/action/fibonacci.hpp> #include <chrono> #include <functional> #include <future> #include <memory> using namespace std::chrono_literals; using Fibonacci = example_interfaces::action::Fibonacci; using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>; class FibonacciActionClient : public rclcpp::Node { public: FibonacciActionClient() : Node("fibonacci_action_client") { // 创建Action客户端，指定Action类型和名称（必须与服务器一致） this->client_ptr_ = rclcpp_action::create_client<Fibonacci>( this, "fibonacci"); // Action名称 RCLCPP_INFO(this->get_logger(), "Fibonacci Action Client 已创建。"); } // 发送目标的公有方法 void send_goal(int order) { // 等待Action服务器上线 if (!client_ptr_->wait_for_action_server(5s)) { RCLCPP_ERROR(this->get_logger(), "Action服务器未在5秒内响应。"); return; } // 构造目标消息 auto goal_msg = Fibonacci::Goal(); goal_msg.order = order; RCLCPP_INFO(this->get_logger(), "发送目标：计算阶数为 %d 的斐波那契数列", order); // 设置发送目标的选项 auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions(); // 设置反馈回调：当服务器发布反馈时触发 send_goal_options.feedback_callback = [this](GoalHandleFibonacci::SharedPtr, const std::shared_ptr<const Fibonacci::Feedback> feedback) { RCLCPP_INFO(this->get_logger(), "收到反馈 -> 当前序列: "); for (auto number : feedback->sequence) { std::cout << number << " "; } std::cout << std::endl; }; // 设置结果回调：当目标最终完成（成功、取消、中止）时触发 send_goal_options.result_callback = [this](const GoalHandleFibonacci::WrappedResult & result) { switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: RCLCPP_INFO(this->get_logger(), "目标成功完成！"); RCLCPP_INFO(this->get_logger(), "最终结果序列: "); for (auto number : result.result->sequence) { std::cout << number << " "; } std::cout << std::endl; break; case rclcpp_action::ResultCode::CANCELED: RCLCPP_WARN(this->get_logger(), "目标被取消。"); break; case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(this->get_logger(), "目标被中止。"); break; default: RCLCPP_ERROR(this->get_logger(), "未知结果码。"); break; } // 这里可以设置一个标志，通知主循环任务结束 // 例如：this->goal_done_ = true; }; // 异步发送目标！这是非阻塞调用。 // 它会返回一个 std::shared_future，可以用来等待目标被接受（或拒绝）。 auto goal_handle_future = client_ptr_->async_send_goal(goal_msg, send_goal_options); // 我们可以选择等待一段时间，看目标是否被服务器接受 if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(this->get_logger(), "发送目标失败或超时。"); return; } // 获取Goal Handle goal_handle_ = goal_handle_future.get(); if (!goal_handle_) { RCLCPP_ERROR(this->get_logger(), "目标被服务器拒绝。"); return; } RCLCPP_INFO(this->get_logger(), "目标已被服务器接受，正在执行..."); // 此时，反馈回调和结果回调会在后台被自动调用。 // 主线程可以继续做其他事情，比如监听用户输入来取消任务。 } // 一个取消当前目标的方法 void cancel_goal() { if (!goal_handle_) { RCLCPP_WARN(this->get_logger(), "没有活跃的目标可供取消。"); return; } RCLCPP_INFO(this->get_logger(), "发送取消请求..."); // 异步取消 auto future_cancel = client_ptr_->async_cancel_goal(goal_handle_); // 可以等待取消操作完成，这里简单忽略 } private: rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_; std::shared_ptr<GoalHandleFibonacci> goal_handle_; // 保存当前目标的句柄 }; int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto client_node = std::make_shared<FibonacciActionClient>(); // 发送一个目标，例如计算阶数为10的数列 client_node->send_goal(10); // 为了让程序保持运行以接收反馈和结果，我们需要spin。 // 但注意，send_goal是异步的，主线程会立刻继续执行到这里。 // 这里我们简单地spin节点，直到用户按下Ctrl+C。 // 在实际应用中，你可能会有一个主循环，在循环里检查任务状态或处理其他逻辑。 rclcpp::spin(client_node); rclcpp::shutdown(); return 0; }

客户端的关键细节与调试技巧：

wait_for_action_server：在发送目标前调用这个函数至关重要。如果服务器还没启动，客户端直接发送目标会失败。超时参数可以根据实际情况调整。
SendGoalOptions：这是配置客户端行为的核心。你必须设置feedback_callback和result_callback。这两个回调函数会在客户端节点的执行器（executor）线程中被调用，因此它们内部可以安全地调用ROS2的日志、发布消息等API。
异步发送与Future：async_send_goal返回一个std::shared_future。调用goal_handle_future.get()会阻塞，直到收到服务器的接受或拒绝响应。如果你想完全非阻塞，可以忽略这个future，完全依靠result_callback来得知最终结果。但通常，我们至少需要知道目标是否被接受。
结果码（ResultCode）：在result_callback中，result.code表明了任务的最终状态：
- SUCCEEDED：服务器调用goal_handle->succeed(result)。
- CANCELED：服务器调用goal_handle->canceled(result)（客户端或服务器取消）。
- ABORTED：服务器调用goal_handle->abort(result)（任务执行失败）。
goal_handle_的生命周期：我们将goal_handle_保存为成员变量，以便在cancel_goal方法中使用。确保在任务完成后（result_callback被调用后），不再使用这个句柄，或者将其重置。
主循环设计：这个示例的main函数很简单，发送目标后直接spin。在真实的机器人应用中，你的客户端节点可能是一个状态机的一部分，在spin的同时，定期检查任务状态或响应其他事件。一种常见模式是：在send_goal后，进入一个while(rclcpp::ok())循环，在循环内spin_some处理回调，并检查一个由result_callback设置的标志位（如goal_done_）来判断任务是否结束。

5. 编译、运行与问题排查实战

代码写完了，接下来是编译和运行。进入你的工作空间根目录（~/ros2_ws）：

# 编译功能包 colcon build --packages-select cpp_action_demo # 激活环境（每次新开终端都需要） source install/setup.bash

运行与测试：

启动Action服务器：
```
ros2 run cpp_action_demo action_server
```
你应该看到输出：Fibonacci Action Server 已启动，等待目标...
启动Action客户端：另开一个终端，激活环境后运行：
```
ros2 run cpp_action_demo action_client
```
客户端会发送一个阶数为10的目标。观察两个终端的输出。服务器会每秒打印一次反馈，客户端也会收到并打印反馈。最终，服务器打印成功信息，客户端打印最终结果序列。
测试取消功能：我们需要修改一下客户端，让它能在发送目标后等待几秒，然后主动取消。为了演示，我们可以写一个简单的带取消的客户端，或者用ROS2命令行工具来模拟取消。
- 方法一：使用ros2 action命令行工具（推荐，用于调试）：
  - 先启动服务器。
  - 在新的终端，发送目标：
```
ros2 action send_goal fibonacci example_interfaces/action/Fibonacci "{order: 15}" --feedback
```
    --feedback参数会实时显示反馈信息。
  - 在目标执行过程中，另开一个终端，取消这个目标。首先需要知道目标的UUID，可以通过ros2 action list查看活跃的目标，或者直接使用ros2 action cancel_goal。最简单的方法是，在发送目标的命令后，快速按Ctrl+C中断发送命令的终端，然后立即运行：
```
ros2 action cancel_goal fibonacci
```
    这会取消该Action服务器上最新的目标。你会看到服务器和（如果原命令还在运行）反馈终端都显示任务被取消。
- 方法二：修改客户端代码，在send_goal后，等待几秒然后调用cancel_goal。这需要你管理好线程和时间，稍微复杂一些。

常见问题与排查：

编译错误：找不到rclcpp_action或example_interfaces：
- 检查CMakeLists.txt中的find_package和ament_target_dependencies是否已添加。
- 检查package.xml的<depend>标签。
- 确保工作空间已正确编译 (colcon build) 并激活 (source install/setup.bash)。
运行时错误：客户端报Action服务器未在5秒内响应：
- 确认服务器节点是否已经启动。
- 确认Action名称是否一致。服务器创建时用的名字是"fibonacci"，客户端连接时也必须是"fibonacci"。
- 使用ros2 node list和ros2 topic list检查节点和话题是否存在。Action底层是话题，你应该能看到名为/fibonacci/_action/feedback,/fibonacci/_action/status等话题。
服务器收不到目标或客户端收不到反馈：
- 最常见原因：没有调用rclcpp::spin。确保服务器和客户端的节点对象都被spin了。spin是ROS2接收和处理所有回调（包括定时器、订阅、Action请求）的必需步骤。
- 检查日志级别。有时默认的日志级别是WARN，INFO级别的日志看不到。可以在启动节点时设置日志级别：ros2 run cpp_action_demo action_server --ros-args --log-level info。
任务无法取消：
- 在服务器的execute函数中，必须定期检查goal_handle->is_canceling()。如果循环执行得很快（比如没有sleep），可能来不及检查取消标志。
- 确保客户端的取消请求确实发送了。可以在服务器的handle_cancel回调中添加日志来确认。
反馈或结果消息字段不对：
- 仔细核对Action定义文件。对于Fibonacci.action，其Feedback和Result中的字段名都是sequence（一个整数数组）。如果你自定义Action，务必确保服务器填充的字段和客户端期望读取的字段名称、类型完全一致。