xiaoshuai/ROS2_learn
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

init: ROS2 learning suite

Browse Source
This commit is contained in:
xs
2026-08-03 18:09:35 +08:00
commit 5ef38ab508
95 changed files with 13322 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/40-actions.md
+752
View File
@@ -0,0 +1,752 @@
# 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) |