xiaoshuai/ROS2_learn
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
b52cee9e394d1b968a2f3562ffbf154276d21404
ROS2_learn/doc/40-actions.md
T
xiaoshuai b52cee9e39 fix
2026-08-05 18:17:25 +08:00

21 KiB
Raw Blame History

40 · Action 深度:Goal / Feedback / Result(完全指南)

目标:吃透 ROS2 Action,涵盖 .action 定义、Goal/Feedback/Result 三件套、cancel、MultiThreadedExecutor、VLA/机器人应用,学完能写工业级 Action server。

目录

1. 为什么需要 Action

1.1 Service 的局限

Service 是"短查询",适合几毫秒到几秒。但机器人任务经常需要几分钟:

  • 抓取一个物体:5-30 秒
  • 移动底盘导航:几秒到几分钟
  • SLAM 建图:几小时

Service 调起来就"卡住",不知道进度,不能取消。

1.2 Action 解决的问题

Service 没有的 Action 提供
进度反馈 Feedback(周期性推送)
取消能力 cancel(client 中途叫停)
长任务友好 异步,不阻塞

1.3 vs Topic / Service

维度 Topic Service Action
同步 异步 同步 异步(long)
方向 单向 双向 双向
1对多 ✅ ❌ ❌
进度反馈 ❌ ❌ ✅
可取消 N/A ❌ ✅
持续 持续流 短查询 几秒~几小时
适合 传感器 拍照 抓取 / 导航 / SLAM

2. .action 文件定义

2.1 三段式格式

Goal 字段
---
Result 字段
---
Feedback 字段

2.2 标准 Fibonacci 例子

# example_interfaces/action/Fibonacci.action
int32 order
---
int32[] sequence
---
int32[] sequence
段 含义 字段
第一段 Goal(client 发) int32 order
--- 分隔
第二段 Result(server 一次性返回) int32[] sequence
--- 分隔
第三段 Feedback(server 周期性推) int32[] sequence

注意: Humble 的 example_interfaces/action/Fibonacci 里 Feedback 和 Result 字段名都是 sequence(不是 partial_sequence)。Python 生成 Fibonacci.Feedback.sequence 和 Fibonacci.Result.sequence。

2.3 实际抓取 Action(自定义)

# my_robot/action/ExecuteGripperPick.action
geometry_msgs/PoseStamped target_pose
string object_id
---
bool success
string error_message
---
float32 progress        # 0..1
string current_state    # "approach" / "grasp" / "lift" / "done"

2.4 复杂 .action 示例

# my_robot/action/NavigateToPose.action
geometry_msgs/PoseStamped target_pose
---
bool success
geometry_msgs/PoseStamped final_pose
builtin_interfaces/Duration total_time
---
float32 distance_remaining
float32 time_remaining
string current_behavior    # "computing_path" / "moving" / "recovering"

3. Action Server(Python / C++)

3.1 Python 标准模板

import rclpy
from rclpy.action import ActionServer
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from example_interfaces.action import Fibonacci

class FibonacciActionServer(Node):
    def __init__(self):
        # 节点名(实际本仓库无 _py 后缀)
        super().__init__('fibonacci_action_server')
        self._action_server = ActionServer(
            self,
            Fibonacci,                   # ActionType
            'fibonacci',                 # action 名
            self.execute_callback,        # 签名:cb(goal_handle) -> result
        )
        self.get_logger().info('ready')

    def execute_callback(self, goal_handle):
        order = goal_handle.request.order
        feedback = Fibonacci.Feedback()
        result = Fibonacci.Result()
        sequence = [0, 1]

        for i in range(1, order):
            # 1) 检查 client 是否请求取消
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                self.get_logger().info('Goal canceled')
                return Fibonacci.Result()       # 返回空 Result

            # 2) 做一部分工作
            sequence.append(sequence[i] + sequence[i-1])

            # 3) 推一次 Feedback
            feedback.sequence = sequence
            goal_handle.publish_feedback(feedback)

            # 4) 模拟耗时(实际场景中这里跑电机控制 / 规划 / 等)
            import time; time.sleep(0.5)

        # 5) 任务完成
        goal_handle.succeed()
        result.sequence = sequence
        return result

def main():
    rclpy.init()
    node = FibonacciActionServer()
    # 用 MultiThreadedExecutor!否则 Feedback 推送会卡死主线程
    executor = MultiThreadedExecutor()
    executor.add_node(node)
    executor.spin()

3.2 C++

#include "rclcpp_action/rclcpp_action.hpp"
#include "example_interfaces/action/fibonacci.hpp"

class Server : public rclcpp::Node {
public:
  using Fibonacci = example_interfaces::action::Fibonacci;
  using GoalHandleFib = rclcpp_action::ServerGoalHandle<Fibonacci>;

  Server() : rclcpp::Node("server") {
    server_ = rclcpp_action::create_server<Fibonacci>(
      this, "fibonacci",
      [this](auto, auto) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; },
      [this](auto) { return rclcpp_action::CancelResponse::ACCEPT; },
      [this](std::shared_ptr<GoalHandleFib> gh) {
        // 主逻辑
        auto feedback = std::make_shared<Fibonacci::Feedback>();
        auto result = std::make_shared<Fibonacci::Result>();
        std::vector<int32_t> seq = {0, 1};

        for (int i = 1; i < gh->get_request()->order; i++) {
          if (gh->is_canceling()) { gh->canceled(result); return; }
          seq.push_back(seq[i] + seq[i-1]);
          feedback->sequence = seq;
          gh->publish_feedback(feedback);
          std::this_thread::sleep_for(500ms);
        }

        gh->succeed(result);
        result->sequence = seq;
      });
  }
private:
  rclcpp_action::Server<Fibonacci>::SharedPtr server_;
};

4. Action Client(Python / C++)

4.1 Python 同步风格(简单但阻塞)

class Client(Node):
    def __init__(self):
        super().__init__('client')
        self._client = ActionClient(self, Fibonacci, 'fibonacci')

        # 必须等 server ready
        while not self._client.wait_for_server(timeout_sec=1.0):
            self.get_logger().info('waiting...')

    def call_sync(self, order):
        goal = Fibonacci.Goal()
        goal.order = order

        # call_async 返回 Future
        send_future = self._client.send_goal_async(
            goal,
            feedback_callback=self.feedback_cb,
        )

        # spin 等 server 接收
        rclpy.spin_until_future_complete(self, send_future)
        goal_handle = send_future.result()
        if not goal_handle.accepted:
            self.get_logger().warn('Goal rejected')
            return None

        # spin 等 server 完成
        result_future = goal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, result_future)
        return result_future.result().result

    def feedback_cb(self, feedback_msg):
        self.get_logger().info(f'fb: {list(feedback_msg.feedback.sequence)}')

4.2 Python 异步回调风格(生产推荐)

class AsyncClient(Node):
    def __init__(self):
        super().__init__('async_client')
        self._client = ActionClient(self, Fibonacci, 'fibonacci')
        self._client.wait_for_server()

    def send(self, order):
        goal = Fibonacci.Goal()
        goal.order = order
        send_future = self._client.send_goal_async(
            goal,
            feedback_callback=self.feedback_cb,
        )

        # 异步注册回调:Goal 被接受后
        send_future.add_done_callback(self.goal_response_cb)

    def goal_response_cb(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().warn('rejected')
            return
        # 注册 result 回调
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self.result_cb)

    def feedback_cb(self, feedback_msg):
        self.get_logger().info(f'fb: {list(feedback_msg.feedback.sequence)}')

    def result_cb(self, future):
        result = future.result().result
        self.get_logger().info(f'result: {list(result.sequence)}')
        rclpy.shutdown()

4.3 C++

class Client : public rclcpp::Node {
public:
  using Fibonacci = example_interfaces::action::Fibonacci>;
  Client() : rclcpp::Node("client") {
    client_ = rclcpp_action::create_client<Fibonacci>(
      this, "fibonacci");
    client_->wait_for_action_server();
  }

  void call(int order) {
    auto goal = Fibonacci::Goal();
    goal.order = order;
    auto send_future = client_->async_send_goal(goal,
      [this](auto) { /* feedback */ });

    auto goal_handle = send_future.get();
    if (!goal_handle) return;

    auto result_future = client_->async_get_result(goal_handle);
    if (rclcpp::spin_until_future_complete(this->shared_from_this(),
                                          result_future, 5s) ==
        rclcpp::FutureReturnCode::SUCCESS) {
      RCLCPP_INFO(this->get_logger(), "result.sequence.size = %zu",
                  result_future.get()->result.sequence.size());
    }
  }
};

5. Goal Handle 状态机

   ┌─────────────────┐
   │   PENDING       │  client 发 Goal,server 未处理
   └────────┬────────┘
            ▼ server accept / reject
   ┌─────────────────┐
   │   ACCEPTED      │  server 在执行
   └────────┬────────┘
            ▼ server 周期 publish_feedback
   ┌─────────────────┐
   │   EXECUTING     │  (隐式,在 callback 里)
   └────────┬────────┘
            ▼
       ┌────┴────┐
       ▼         ▼
   ┌─────┐   ┌─────┐
   │SUCC.│   │ABRT.│  server.succeed() / abort()
   └─────┘   └─────┘
       │
       ▼ client.cancel_request → CANCELED
   ┌─────┐
   │CNCL.│
   └─────┘

5.1 关键 API

方法 何时调
goal_handle.accept() / reject() server 开始处理时
goal_handle.is_cancel_requested 周期性检查(看 client 是否取消)
goal_handle.publish_feedback(msg) 周期性推送进度
goal_handle.succeed() 任务正常完成
goal_handle.abort() 任务异常
goal_handle.canceled() client 取消时

6. cancel(取消)

6.1 client 发起取消

ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci "{order: 100}" --feedback

# 另一终端取消(查 status)
ros2 action info /fibonacci

# cancel 命令
# (没有直接 cancel CLI,需要单独客户端)

6.2 server 检查 + 处理

def execute_callback(self, goal_handle):
    for i in range(1, order):
        # 关键:周期性检查
        if goal_handle.is_cancel_requested:
            goal_handle.canceled()           # 必须调,标状态
            self.get_logger().info('Goal canceled by client')
            return Fibonacci.Result()        # 返回空 Result
        ...

6.3 实际场景

  • client 觉得"等太久了"
  • 网络断(超时)
  • 任务本身状态变化(已无意义)

7. MultiThreadedExecutor(关键!)

7.1 为什么必须

Action server 的 callback 跑在主线程。如果用 SingleThreadedExecutor:

  • 主线程被 callback 阻塞
  • publish_feedback 不会真正发出
  • client 收不到进度

7.2 正确写法

from rclpy.executors import MultiThreadedExecutor
executor = MultiThreadedExecutor(num_threads=4)   # 4 线程池
executor.add_node(node)
executor.spin()

或者用 rclpy.callback_groups.MutuallyExclusiveCallbackGroup 精细控制。

8. 实战:写一个抓取 Action

8.1 .action 定义

# my_robot/action/ExecuteGripperPick.action
geometry_msgs/PoseStamped target_pose
string object_id
---
bool success
string error_message
---
float32 progress        # 0..1
string current_state    # "approach" / "grasp" / "lift" / "done"

8.2 Server

class PickServer(Node):
    def __init__(self):
        super().__init__('pick_server')
        self._action_server = ActionServer(
            self, ExecuteGripperPick, 'pick', self.execute_cb)

    def execute_cb(self, goal_handle):
        feedback = ExecuteGripperPick.Feedback()
        result = ExecuteGripperPick.Result()
        target = goal_handle.request.target_pose

        # 阶段 1: approach
        feedback.progress = 0.0
        feedback.current_state = 'approach'
        goal_handle.publish_feedback(feedback)
        if not self.move_to(target.pose):     # MoveIt2 算轨迹 + 执行
            goal_handle.abort()
            result.success = False
            result.error_message = 'approach failed'
            return result

        if goal_handle.is_cancel_requested:
            goal_handle.canceled()
            return result

        # 阶段 2: grasp
        feedback.progress = 0.5
        feedback.current_state = 'grasp'
        goal_handle.publish_feedback(feedback)
        self.close_gripper(force=50)

        # 阶段 3: lift
        feedback.progress = 0.8
        feedback.current_state = 'lift'
        goal_handle.publish_feedback(feedback)
        self.move_to(self.lift_pose)

        # 完成
        feedback.progress = 1.0
        feedback.current_state = 'done'
        goal_handle.publish_feedback(feedback)
        goal_handle.succeed()
        result.success = True
        return result

8.3 Client

class PickClient(Node):
    def pick(self, target_pose, object_id):
        goal = ExecuteGripperPick.Goal()
        goal.target_pose = target_pose
        goal.object_id = object_id

        future = self._client.send_goal_async(
            goal,
            feedback_callback=self.fb_cb,
        )
        future.add_done_callback(self.response_cb)

    def fb_cb(self, fb_msg):
        f = fb_msg.feedback
        print(f'[{f.progress*100:.0f}%] {f.current_state}')

    def response_cb(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            print('rejected')
            return
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self.result_cb)

    def result_cb(self, future):
        result = future.result().result
        print(f'success={result.success}')

9. 调试命令

# 列所有 action
ros2 action list

# 看 action 元数据
ros2 action info /fibonacci

# 用 CLI 发 Goal(无 client 节点时方便)
ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci "{order: 6}" --feedback

# 输出实时反馈
ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci "{order: 6}" --feedback
# 预期输出:
#   Feedback:
#       sequence: [0, 1, 1, 2, 3, 5]
#   Result:
#       sequence: [0, 1, 1, 2, 3, 5, 8]
#   Goal finished with status: SUCCEEDED

10. 常见坑

10.1 wait_for_server 死锁

# ❌ 错:__init__ 里阻塞 wait,主线程 spin 跑不动 → DDS discovery 没动 → 永远 wait
def __init__(self):
    super().__init__(...)
    self._client.wait_for_server()    # 死锁!

# ✅ 对:用 server_is_ready() 轮询
def __init__(self):
    super().__init__(...)
    self._client = ActionClient(...)

def wait_for_server(self, timeout=5.0):
    import time
    end = time.time() + timeout
    while time.time() < end:
        if self._client.server_is_ready():
            return True
        time.sleep(0.05)
    return False

10.2 client callback 里 shutdown

# ❌ 错:在 result callback 里 shutdown,后续 fixture 也 shutdown 会报错
def result_cb(self, future):
    result = future.result().result
    rclpy.shutdown()        # 第一次 shutdown

# 测试代码:
def test_xxx():
    fixture.rclpy.shutdown()   # 第二次,报 "Context must be initialized"

修法:用 flag + 主循环检测:

def result_cb(self, future):
    self._done_flag = True

# 主循环:
while not node._done_flag and time.time() < end:
    exec_.spin_once(timeout_sec=0.1)
# 测试代码统一 rclpy.shutdown()

10.3 单线程 executor 没反馈

# ❌ 错:SingleThreadedExecutor,callback 阻塞主线程
exec_ = SingleThreadedExecutor()
exec_.add_node(node)
exec_.spin()                # publish_feedback 卡死,client 收不到

# ✅ 对:MultiThreadedExecutor
from rclpy.executors import MultiThreadedExecutor
exec_ = MultiThreadedExecutor(num_threads=4)
exec_.add_node(node)
exec_.spin()

10.4 字段名错

# ❌ 错:Feedback 字段名误用
feedback.partial_sequence = sequence    # AttributeError

# ✅ 对:用正确的字段名(看 .action 定义)
feedback.sequence = sequence

本仓库用的 example_interfaces/action/Fibonacci:

  • Goal 字段:order
  • Result 字段:sequence
  • Feedback 字段:sequence(不是 partial_sequence!)

10.5 server 未注册就 client 调

future = client.send_goal_async(goal)
# server 还没 register,future 直接进入 reject 分支
# 客户端看不到 GoalRejected 状态

修法: 先 wait_for_server()(轮询版)。

11. 在本仓库里跑

11.1 启 server

docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup action_launch.py"

11.2 发 Goal

docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci '{\"order\": 6}' --feedback"

预期输出:

Waiting for an action server to become available...
Sending goal:
     order: 6

Goal accepted with ID: 3774142495e7431cb1f45189da01955c

Feedback:
    sequence: [0, 1, 1, 2, 3, 5]
Feedback:
    sequence: [0, 1, 1, 2, 3, 5, 8]
Result:
    sequence: [0, 1, 1, 2, 3, 5, 8]
Goal finished with status: SUCCEEDED

11.3 源码

12. VLA / 机器人应用

12.1 VLA 推理天然适合 Action

VLA(Vision-Language-Action)模型:

  • 输入:图像 + 语言指令
  • 输出:7-DoF 关节轨迹
  • 推理时间:几秒到几十秒

完全符合 Action 模型:

# vla_action/SampleVLA.action
sensor_msgs/Image image
string instruction
---
trajectory_msgs/JointTrajectory trajectory
bool success
---
float32 progress
string current_state

12.2 完整 VLA-ROS2 集成

┌──────────────┐
│  RGB Camera  │
└──────┬───────┘
       │ /image_raw
       ▼
┌──────────────────────────────┐
│  VLA Inference Node (PC)      │
│  - 订阅 /image_raw            │
│  - 订阅 /instruction (String) │
│  - 调 OpenVLA 模型推理        │
│  - publish Feedback(进度)     │
└──────┬───────────────────────┘
       │ Action /vla_pick
       ▼
┌──────────────────────────────┐
│  Execution Node (PC/RDK X5)   │
│  - 接收 Action                │
│  - 算 MoveIt2 轨迹            │
│  - 调 ros2_control             │
└──────┬───────────────────────┘
       │ /joint_trajectory
       ▼
┌──────────────────────────────┐
│  ros2_control (RK3506)        │
│  - PID 闭环                   │
│  - 电机驱动                   │
└──────────────────────────────┘

12.3 实际项目建议

  1. 本仓库 跑通 7 包 + 跨机通信
  2. 升级到 py_action_demo 的 Action server 做"抓取"接口
  3. 接 OpenVLA / Pi0 输出轨迹
  4. 接 MoveIt2 + ros2_control 实际执行

详见 doc/99-embodied-ai.md。

接下来读

主题 文档
Topic 20-topics.md
Service 30-services.md
TF2 + 抓取 50-tf2.md
具身智能路径 99-embodied-ai.md
三机部署 100-embedded-deployment.md

📖 阅读路径导航

💡 这是仓库 doc/ 下所有文档的推荐阅读顺序。返回 README 总导航

⏱ 本文预计阅读时间: 60 分钟 📍 当前位置: 第 15 / 24 篇

Reference in New Issue View Git Blame Copy Permalink