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

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

---

## 目录

- [1. 为什么需要 Action](#1-为什么需要-action)
- [2. .action 文件定义](#2-action-文件定义)
- [3. Action Server(Python / C++)](#3-action-serverpython--c)
- [4. Action Client(Python / C++)](#4-action-clientpython--c)
- [5. Goal Handle 状态机](#5-goal-handle-状态机)
- [6. cancel(取消)](#6-cancel取消)
- [7. MultiThreadedExecutor(关键!)](#7-multithreadedexecutor关键)
- [8. 实战:写一个抓取 Action](#8-实战写一个抓取-action)
- [9. 调试命令](#9-调试命令)
- [10. 常见坑](#10-常见坑)
- [11. 在本仓库里跑](#11-在本仓库里跑)
- [12. VLA / 机器人应用](#12-vla--机器人应用)

---

## 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 例子

```action
# 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(自定义)

```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 示例

```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 标准模板

```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):
        super().__init__('fibonacci_action_server_py')
        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++

```cpp
#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 同步风格(简单但阻塞)

```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 异步回调风格(生产推荐)

```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++

```cpp
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 发起取消

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

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

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

### 6.2 server 检查 + 处理

```python
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 正确写法

```python
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 定义

```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

```python
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

```python
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. 调试命令

```bash
# 列所有 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 死锁

```python
# ❌ 错:__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

```python
# ❌ 错:在 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 + 主循环检测:
```python
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 没反馈

```python
# ❌ 错: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 字段名错

```python
# ❌ 错: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 调

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

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

---

## 11. 在本仓库里跑

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

### 11.2 发 Goal
```bash
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 源码
- Server: [`src/py_action_demo/py_action_demo/fibonacci_server.py`](../src/py_action_demo/py_action_demo/fibonacci_server.py)
- Client: [`src/py_action_demo/py_action_demo/fibonacci_client.py`](../src/py_action_demo/py_action_demo/fibonacci_client.py)
- 测试: [`src/py_action_demo/test/test_action.py`](../src/py_action_demo/test/test_action.py)
- launch: [`src/bringup/launch/action_launch.py`](../src/bringup/launch/action_launch.py)

---

## 12. VLA / 机器人应用

### 12.1 VLA 推理天然适合 Action

VLA(Vision-Language-Action)模型:
- 输入:图像 + 语言指令
- 输出:7-DoF 关节轨迹
- 推理时间:几秒到几十秒

完全符合 Action 模型:
```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`](99-embodied-ai.md)。

---

## 接下来读

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

---

---

## 📖 阅读路径导航

> 💡 这是仓库 `doc/` 下所有文档的推荐阅读顺序。[返回 README 总导航](../README.md#-23-篇文档怎么读)
>
> ⏱ **本文预计阅读时间**: 60 分钟
> 📍 **当前位置**: 第 15 / 24 篇

- ⏮ **上一篇**: [Service 深度](../30-services.md)
- ⏭ **下一篇**: [坐标变换](../50-tf2.md)