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/00-overview.md
+270
View File
@@ -0,0 +1,270 @@
# 00 · 项目架构与设计取舍(完全指南)
> **目标**:5 分钟内搞清楚本仓库"为什么这么设计、每一层做什么、对比 ROS2 全栈还差什么"。
---
## 目录
- [1. 一句话定位](#1-一句话定位)
- [2. 双层解耦架构](#2-双层解耦架构)
- [3. 包设计全景](#3-包设计全景)
- [4. 通信拓扑](#4-通信拓扑)
- [5. 文件布局详解](#5-文件布局详解)
- [6. ROS2 全栈 vs 本仓库](#6-ros2-全栈-vs-本仓库)
- [7. 何时用本仓库](#7-何时用本仓库)
- [8. 设计原则](#8-设计原则)
- [9. 下一步](#9-下一步)
---
## 1. 一句话定位
> **ROS2 Humble 7 包全栈实战** — Topic / Service / Action / TF2 / URDF / Vision 六大通信范式 +
> Python/C++ 双语言互通 + 端到端 launch + **10/10 测试 100% 通过** + 15 篇文档 —
> 为 VLA / 具身智能开发铺平最后一段学习路径。
---
## 2. 双层解耦架构
本仓库用**两个互不污染的环境**把"Python 开发"与"ROS2 运行"分开:
```
┌─────────────────────────────────────────────────────────────┐
│ 本机 Windows / Linux (你正在用的机器) │
│ ┌───────────────────────────────────────────────────────┐ │
│ │ .venv (Python 3.10 virtualenv) │ │
│ │ ruff · black · mypy · flake8 · pytest · numpy │ │
│ │ + pip install -e src/py_pubsub ... (源码可编辑) │ │
│ └───────────────────────────────────────────────────────┘ │
│ IDE: VSCode (Pylance / pyright) · PyCharm │
│ 配置文件: pyproject.toml · pyrightconfig.json · .flake8 │
└────────────────────────────┬────────────────────────────────┘
│ bind mount: D:\xs\ros2 ↔ /root/ros2_ws
┌────────────────────────────▼────────────────────────────────┐
│ Docker (osrf/ros:humble-desktop) │
│ ┌───────────────────────────────────────────────────────┐ │
│ │ ROS2 apt (系统 Python) │ │
│ │ rclcpp · rclpy · tf2 · cv_bridge · ros2-control ... │ │
│ │ /opt/ros/humble + ROS_DOMAIN_ID + RMW │ │
│ └───────────────────────────────────────────────────────┘ │
│ ┌───────────────────────────────────────────────────────┐ │
│ │ colcon build / install / test │ │
│ │ src/py_pubsub src/cpp_pubsub src/py_srv │ │
│ │ src/py_action_demo src/cpp_robot_tf2 │ │
│ │ src/py_vision_demo src/bringup │ │
│ │ ↓ │ │
│ │ install/<pkg>/share/<pkg>/launch/*.py │ │
│ │ install/<pkg>/lib/<pkg>/<exec> │ │
│ └───────────────────────────────────────────────────────┘ │
│ ┌───────────────────────────────────────────────────────┐ │
│ │ 11 个 ROS2 节点(运行时) │ │
│ │ talker/listener × 2 (py+cpp) │ │
│ │ service server / action server │ │
│ │ joint_state_publisher + robot_state_publisher │ │
│ │ + tf2_listener + fake_camera + image_processor │ │
│ └───────────────────────────────────────────────────────┘ │
└─────────────────────────────────────────────────────────────┘
```
### 2.1 为什么这样设计
| 层 | 用途 | 为什么独立 |
|---|---|---|
| **本机 venv** | IDE 代码跳转、单元测试、lint、格式化 | rclpy 没 Windows wheels;venv 隔离掉"装不上 ROS 客户端"的事实 |
| **Docker ROS2** | 跑 ROS2 节点、launch、colcon test、端到端验证 | 一次 build 处处跑,Linux + Windows + macOS 一致 |
详见 [`02-virtualenv.md`](02-virtualenv.md)。
---
## 3. 包设计全景
| 包 | 角色 | build_type | 关键依赖 |
|---|---|---|---|
| **py_pubsub** | Topic pub/sub(Python) | ament_python | rclpy, std_msgs |
| **cpp_pubsub** | Topic pub/sub(C++) | ament_cmake | rclcpp, std_msgs |
| **py_srv** | Service(Python) | ament_python | rclpy, example_interfaces |
| **py_action_demo** | Action 三件套(Python) | ament_python | rclpy, example_interfaces |
| **cpp_robot_tf2** | URDF + TF2 + JointState(C++) | ament_cmake | rclcpp, tf2, geometry_msgs, sensor_msgs |
| **py_vision_demo** | sensor_msgs/Image(Python) | ament_python | rclpy, sensor_msgs, cv_bridge |
| **bringup** | 顶层 launch 聚合(Python) | ament_python | launch, launch_ros |
**设计原则**:
- 每个包独立可 build / test,**不强依赖其他 demo 包**
- 跨包互通依赖 DDS(**不需要 import 别的包代码**)
- bringup **只负责组合**,不重复节点实现
---
## 4. 通信拓扑
ROS2 所有节点通过 **DDS**(默认 fastdds)做发布订阅,**不直接 import 别的节点代码**:
```
┌──────────────────────────────────────────────────────────┐
│ DDS 总线 │
│ /chatter /image_raw /tf /tf_static │
│ /joint_states /parameter_events /rosout │
└────────┬────────┬──────────┬──────────┬────────┬────────┘
│ │ │ │ │
┌─────▼───┐ ┌──▼──────┐ ┌▼────────┐ ┌▼─────┐ ┌▼─────────┐
│talker_py│ │fake_cam │ │joint_pub│ │srv │ │fibonacci │
│listener │ │image_ │ │robot_ │ │server│ │_server │
│ _py │ │processor│ │state_pub│ │ │ │ │
├─────────┤ ├─────────┤ ├─────────┤ └──────┘ └──────────┘
│talker_ │
│ cpp │ ← topic 互通(跨语言)
│listener │
│ _cpp │
└─────────┘
```
---
## 5. 文件布局详解
```
D:\xs\ros2\
├── README.md ← 项目入口 + 架构 + 启动命令
├── AGENTS.md ← 铁律 + 工作流(必读)
├── pyproject.toml ← PEP 621 workspace + 工具配置
├── requirements.txt / -dev.txt ← venv 依赖
├── .flake8 / pyrightconfig.json ← lint / 类型检查配置
├── .gitignore
│
├── docker/
│ ├── Dockerfile ← ROS2 Humble 镜像
│ ├── docker-compose.yml ← bind mount + host network
│ └── *_e2e.log ← 端到端验证日志(46KB 等)
│
├── tools/ ← 本机开发工具
│ ├── setup_venv.sh / .ps1 ← 一键 venv
│
├── build.sh / start.sh / start.ps1
│
├── src/
│ ├── py_pubsub/ ← Topic (Python)
│ ├── cpp_pubsub/ ← Topic (C++)
│ ├── py_srv/ ← Service (Python)
│ ├── py_action_demo/ ← Action (Python)
│ ├── cpp_robot_tf2/ ← TF2 + URDF (C++)
│ ├── py_vision_demo/ ← Image (Python)
│ └── bringup/ ← launch 聚合 (Python)
│
└── doc/ ← 15 篇深度文档
├── 00-overview.md ← 本篇
├── 01-quickstart.md ← 5 分钟上手
├── 02-virtualenv.md ← venv 工作流
├── 10-concepts.md ← 核心概念地图
├── 20-topics.md ← Topic 深度
├── 30-services.md ← Service 深度
├── 40-actions.md ← Action 深度
├── 50-tf2.md ← TF2 坐标变换
├── 60-urdf.md ← URDF 机器人模型
├── 70-launch.md ← launch 文件
├── 80-package-build.md ← colcon / ament
├── 85-docker.md ← Docker 容器化
├── 90-testing.md ← 测试金字塔
├── 99-embodied-ai.md ← VLA / 机器人路径
└── 100-embedded-deployment.md ← 三机部署实操
```
---
## 6. ROS2 全栈 vs 本仓库
**坦白说:本仓库不是完全体**。覆盖 ~70% 的 ROS2 + 机械臂入门知识。
### 6.1 ✅ 已覆盖
| 知识点 | 状态 |
|---|---|
| ROS2 通信(Topic / Service / Action / Parameter) | ✅ |
| TF2 坐标变换 + URDF | ✅ |
| sensor_msgs/Image + cv_bridge | ✅ |
| JointState 发布 | ✅ |
| Launch + 测试 + Docker + venv | ✅ |
| 跨语言互通 | ✅ |
| 文档齐全(15 篇) | ✅ |
### 6.2 ❌ 没覆盖(下一步)
| 知识点 | 缺 |
|---|---|
| **ros2_control** + JointTrajectoryController | ❌ |
| **MoveIt2** 运动规划(IK / 避障) | ❌ |
| **Gazebo** / ros_gz 物理仿真 | ❌ |
| 真实 6-DoF 机械臂 URDF | ❌(只有 3 关节玩具) |
| Gripper / 末端执行器控制 | ❌ |
| 手眼标定 | ❌ |
| 力控 / Force-Torque | ❌ |
| GraspNet / 6-DoF 抓取 | ❌ |
| VLA 模型接入(OpenVLA / Pi0) | ❌(只有路径规划) |
| 移动底盘(Nav2 / SLAM) | ❌ |
| 多臂协调 | ❌ |
| rosbridge / Foxglove | ❌ |
**完整学习路径**: 见 [`99-embodied-ai.md`](99-embodied-ai.md) — 5 个阶段 / 3-5 个月。
### 6.3 实际项目必装的额外 apt 包
```bash
sudo apt install ros-humble-ros2-control
sudo apt install ros-humble-ros2-controllers
sudo apt install ros-humble-moveit
sudo apt install ros-humble-ros-gz
sudo apt install ros-humble-navigation2
```
---
## 7. 何时用本仓库
| 场景 | 推荐 |
|---|---|
| 第 1 次学 ROS2,想完整跑通一个最小 ROS2 系统 | ✅ 推荐 |
| 想搞懂"跨语言互通"是怎么工作的 | ✅ 推荐 |
| 准备做机器人 / 具身智能项目,需要先打基础 | ✅ 推荐 |
| 公司已经在用 ROS2,需要一个 demo 教学 | ✅ 推荐 |
| 想直接做 MoveIt2 / Nav2 的仿真 | ⚠️ 本仓库打底,然后装 ROS2 Humble 官方包 |
| 想直接做产品化 ROS2 | ⚠️ 本仓库做参考,真实项目要按需重构 |
---
## 8. 设计原则
### 8.1 包设计
- 一个 demo 一个包
- 跨包不 import,只走 DDS
- 标准消息为主(example_interfaces),避免自定义 msg
### 8.2 命名
- 包名小写下划线(`py_pubsub`, `cpp_robot_tf2`)
- **包名不能叫 `launch`**(与 ROS2 自带包同名,ament 索引冲突)
- 节点名小写下划线 + 后缀(`talker_py`, `joint_state_publisher_cpp`)
### 8.3 测试
- 每个 Python 包 4 个测试用例
- 每个 C++ 包 2 个 gtest
- 集成测试用 in-process spin(避免 launch_testing 的 shutdown 二次调用问题)
### 8.4 文档
- 中文 + 详细中文注释
- 每篇 doc 有目录 + "5 分钟摘要" + "踩过的坑"
- 代码与文档严格同步(交叉引用源文件路径)
---
## 9. 下一步
| 你想做什么 | 看 |
|---|---|
| 5 分钟跑通 | [`01-quickstart.md`](01-quickstart.md) |
| 用 venv 不污染系统 | [`02-virtualenv.md`](02-virtualenv.md) |
| 弄懂 ROS2 核心概念 | [`10-concepts.md`](10-concepts.md) |
| 深读 Topic / Service / Action | [`20-topics.md`](20-topics.md), [`30-services.md`](30-services.md), [`40-actions.md`](40-actions.md) |
| 做机器人 / TF / URDF | [`50-tf2.md`](50-tf2.md), [`60-urdf.md`](60-urdf.md) |
| 三机部署到 RDK X5 / RK3506 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
| 进入具身智能 / VLA | [`99-embodied-ai.md`](99-embodied-ai.md) |
doc/01-quickstart.md
+459
View File
@@ -0,0 +1,459 @@
# 01 · 5 分钟上手 Quickstart(完整图文版)
> **目标**:从"零"到"看到第一个 ROS2 消息流",每步带**预期输出 + 排错**,5 分钟内完成。
---
## 目录
- [Step 0: 准备清单](#step-0-准备清单)
- [Step 1: 安装 Docker](#step-1-安装-docker)
- [Step 2: 拉取并构建镜像](#step-2-拉取并构建镜像)
- [Step 3: 启动容器](#step-3-启动容器)
- [Step 4: 在容器内编译](#step-4-在容器内编译)
- [Step 5: 跑你的第一个 demo](#step-5-跑你的第一个-demo)
- [Step 6: 本机 venv 开发工作流](#step-6-本机-venv-开发工作流)
- [Step 7: 验证清单](#step-7-验证清单)
- [Step 8: 常见问题 FAQ](#step-8-常见问题-faq)
---
## Step 0: 准备清单
| 项 | 需要 |
|---|---|
| **操作系统** | Windows 10/11 · macOS 12+ · Linux(Ubuntu 20.04+) |
| **Docker** | Docker Desktop(Win/macOS)或 Docker Engine(Linux) |
| **磁盘空间** | 8 GB 可用(Docker 镜像 + 构建缓存) |
| **网络** | 首次构建需要下载 ~3 GB 镜像 |
| **时间** | 首次构建 5-10 分钟;后续秒级 |
> 💡 Windows 用户:在 BIOS 里开启虚拟化(Intel VT-x / AMD-V),否则 Docker 起不来。
---
## Step 1: 安装 Docker
### 1.1 Windows
1. 下载 [Docker Desktop for Windows](https://www.docker.com/products/docker-desktop/)
2. 双击安装,需要 WSL 2 后端(安装时它会提示)
3. 重启电脑
4. 启动 Docker Desktop,**等到右下角鲸鱼图标不再转动**
### 1.2 macOS
```bash
brew install --cask docker
# 启动 Docker Desktop
```
### 1.3 Linux
```bash
# Ubuntu
sudo apt install docker.io docker-compose-plugin
sudo systemctl enable --now docker
sudo usermod -aG docker $USER
# 重新登录后生效
```
### 1.4 验证 Docker 装好
```bash
docker --version
# Docker version 29.4.3, build ...
docker compose version
# Docker Compose version v5.1.3
```
**若报 `Cannot connect to Docker daemon`**: 启动 Docker Desktop(Win/macOS)或 `sudo systemctl start docker`(Linux)。
---
## Step 2: 拉取并构建镜像
### 2.1 进入项目目录
```bash
# Windows PowerShell
cd D:\xs\ros2
# Linux / macOS
cd /path/to/ros2
```
### 2.2 构建镜像(首次 5-10 分钟)
```bash
docker compose -f docker/docker-compose.yml build
```
**这一步做了什么**:
1. 拉 `osrf/ros:humble-desktop` 基础镜像(约 2 GB)
2. 装 `colcon-common-extensions`、`colcon-argcomplete` 等开发工具
3. 打 tag 为 `ros2-humble-dev:latest`
**预期输出(末尾)**:
```
#10 exporting layers 1.5s done
#10 exporting manifest sha256:xxxxx 0.0s done
#10 exporting config sha256:xxxxx 0.0s done
#10 naming to docker.io/library/ros2-humble-dev:latest done
Image ros2-humble-dev:latest Built
```
**若报 `ERROR: pull access denied`**:
- 检查网络(可能在国内,需要配 Docker 镜像加速器)
- 或: `docker pull osrf/ros:humble-desktop` 单步测试
### 2.3 验证镜像
```bash
docker images
# REPOSITORY TAG IMAGE ID CREATED SIZE
# ros2-humble-dev latest abc123def456 1 minute ago 3.4GB
# osrf/ros humble-desktop 789xyz... 3 weeks ago 2.1GB
```
---
## Step 3: 启动容器
```bash
docker compose -f docker/docker-compose.yml up -d
```
**参数说明**:
- `up -d`:后台启动(`-d` = detached)
- `container_name: ros2_dev`:容器名叫这个
- `network_mode: host`:DDS multicast 必须
**预期**:
```
[+] Running 2/2
✔ Container ros2_dev Created
✔ Container ros2_dev Started
```
**验证容器在跑**:
```bash
docker ps
# CONTAINER ID IMAGE NAMES ...
# abc123def456 ros2-humble-dev:latest ros2_dev Up X seconds
```
**进入开发终端**:
```bash
docker exec -it ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && exec bash"
```
你应该看到类似:
```
root@docker-desktop:/root/ros2_ws#
```
> 💡 **小技巧**: `docker exec -it ros2_dev bash` 进入容器;**`exec` 前要保证容器在运行**(`docker ps` 看到 `Up` 状态)。
---
## Step 4: 在容器内编译
### 4.1 准备编译
```bash
# 在容器内
source /opt/ros/humble/setup.bash # 加载 ROS2 环境
cd /root/ros2_ws # 进入工作空间
```
### 4.2 编译所有包
```bash
bash build.sh
```
**预期输出(末尾)**:
```
[INFO] [launch]: Default logging verbosity is set to INFO
Finished <<< py_pubsub [9.5s]
Finished <<< cpp_pubsub [42s]
Finished <<< py_srv [11s]
Finished <<< py_action_demo [17s]
Finished <<< cpp_robot_tf2 [49s]
Finished <<< py_vision_demo [11s]
Finished <<< bringup [18s]
Summary: 7 packages finished [2min 30s]
```
**编译产物位置**:
```
/root/ros2_ws/install/ ← source 这个目录才能用 ros2 命令
├── py_pubsub/
├── cpp_pubsub/
├── py_srv/
├── py_action_demo/
├── cpp_robot_tf2/
├── py_vision_demo/
└── bringup/
```
### 4.3 跑测试(可选)
```bash
source /opt/ros/humble/setup.bash
source /root/ros2_ws/install/setup.bash
colcon test --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup
```
**预期**:
```
Summary: 7 packages finished [25s]
0 packages failed
```
---
## Step 5: 跑你的第一个 demo — Topic 跨语言互通
### 5.1 启动 4 个节点(2 Python + 2 C++)
```bash
source /opt/ros/humble/setup.bash
source /root/ros2_ws/install/setup.bash
ros2 launch bringup pubsub_launch.py
```
**预期输出**:
```
[INFO] [launch]: All log files can be found below /root/.ros/log/2026-08-03-...
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [talker-1]: process started with pid [56]
[INFO] [listener-2]: process started with pid [58]
[INFO] [talker-3]: process started with pid [60]
[INFO] [listener-4]: process started with pid [62]
[talker-1] [INFO] [...] talker_py started -> topic=chatter, period=500ms
[listener-2] [INFO] [...] listener_py subscribed <- chatter
[talker-3] [INFO] [...] talker_cpp started -> topic=chatter, period=500ms
[listener-4] [INFO] [...] listener_cpp subscribed <- chatter
[listener-2] [INFO] [...] recv: "Hello from PY, seq=0"
[listener-4] [INFO] [...] recv: "Hello from C++, seq=0"
[listener-4] [INFO] [...] recv: "Hello from PY, seq=0" ← **跨语言互通!**
[listener-2] [INFO] [...] recv: "Hello from C++, seq=0" ← **跨语言互通!**
```
按 **Ctrl+C** 退出。
### 5.2 在另一个终端验证
**新开一个 PowerShell/终端**,运行:
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && source /root/ros2_ws/install/setup.bash && ros2 topic list --no-daemon"
```
**预期输出**:
```
/chatter
/joint_states
/parameter_events
/rosout
/tf
```
**看 chatter 频率**:
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && source /root/ros2_ws/install/setup.bash && ros2 topic hz /chatter --no-daemon"
```
**预期**:
```
average rate: 4.000
min: 0.250s max: 0.260s std dev: 0.00302s window: 10
```
(4Hz = 2 talker × 2Hz)
### 5.3 看通信拓扑(可视化)
**新终端**:
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && source /root/ros2_ws/install/setup.bash && ros2 run rqt_graph rqt_graph"
```
**会看到**(rqt_graph GUI):
- `talker_py` → `chatter` → `listener_py`
- `talker_cpp` → `chatter` → `listener_cpp`
- 4 节点 2 topic 互通
---
## Step 6: 本机 venv 开发工作流
### 6.1 创建 venv(不污染系统 Python)
```powershell
# Windows
cd D:\xs\ros2
powershell .\tools\setup_venv.ps1
```
```bash
# Linux / macOS / WSL
cd /path/to/ros2
./tools/setup_venv.sh
```
**预期**:
```
==> Python: Python 3.10.x
==> Creating venv at .venv
==> Upgrading pip + installing requirements-dev.txt
OK py_pubsub
OK py_srv
OK py_action_demo
OK py_vision_demo
✅ venv ready. Activate with:
.\.venv\Scripts\Activate.ps1
```
### 6.2 激活 venv
```powershell
# Windows
.\.venv\Scripts\Activate.ps1
# Linux / macOS / WSL
source .venv/bin/activate
```
**预期**: 终端前缀出现 `(.venv)`,如:
```
(.venv) PS D:\xs\ros2>
```
### 6.3 在 venv 里能做什么 / 不能做什么
| 能做 | 不能做 |
|---|---|
| 编辑 Python 源码,IDE 自动补全 | `import rclpy` (rclpy 没 Windows wheels) |
| 跑 ruff / black / mypy | 跑 ROS2 节点 / launch 文件 |
| 装纯 Python 包(numpy / torch / opencv-python) | 跑 ros2 CLI 命令 |
### 6.4 配置 VSCode / PyCharm
**VSCode**:
- `Ctrl+Shift+P` → "Python: Select Interpreter" → 选 `.venv\Scripts\python.exe`
- 装扩展:Python, Pylance, ROS(可选)
- `pyrightconfig.json` 已配好
**PyCharm**:
- `Settings → Project → Python Interpreter` → 选 `.venv\Scripts\python.exe`
---
## Step 7: 验证清单
按这个清单逐项打勾,全过才算"5 分钟上手成功":
- [ ] `docker ps` 看到 `ros2_dev` 容器 `Up`
- [ ] `docker exec ros2_dev echo hello` 输出 `hello`
- [ ] `bash build.sh` 7 packages 全 build 成功
- [ ] `colcon test` 全过(7 packages, 0 failed)
- [ ] `ros2 launch bringup pubsub_launch.py` 启动 4 节点
- [ ] `ros2 topic list` 看到 `/chatter`
- [ ] `ros2 topic hz /chatter` 显示 ~4Hz
- [ ] `ros2 topic info chatter -v` 看到 Python + C++ pub/sub
- [ ] venv 装好且激活,`python -c "import sys; print(sys.executable)"` 显示 `.venv` 路径
如果有任何一项 ✗,看 [Step 8 FAQ](#step-8-常见问题-faq)。
---
## Step 8: 常见问题 FAQ
### Q1: 容器启动失败 `Cannot connect to Docker daemon`
**原因**: Docker Desktop 没运行 / WSL2 没启动
**解决**:
- Win/macOS:启动 Docker Desktop,等右下角图标稳定
- Linux:`sudo systemctl start docker`
### Q2: 构建镜像很慢,卡在 `pulling image`
**原因**: 网络慢 / 在国内
**解决**:
```bash
# 加 Docker 镜像加速器
# Docker Desktop → Settings → Docker Engine,加:
{
"registry-mirrors": ["https://docker.mirrors.ustc.edu.cn"]
}
```
然后 `docker compose build` 重试。
### Q3: `bash build.sh` 报 `rcl_xxx not found`
**原因**: 没 source ROS2
**解决**:
```bash
source /opt/ros/humble/setup.bash
bash build.sh
```
**或者** 把这句加进 `~/.bashrc`:
```bash
echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc
```
### Q4: 容器里 `ros2` 命令找不到
**原因**: 没 source `install/setup.bash`
**解决**:
```bash
source /root/ros2_ws/install/setup.bash
ros2 --help # 应该输出帮助
```
### Q5: `ros2 topic hz` 显示 0 Hz
**原因**: 没节点 publish 数据
**解决**: 确认 launch 起来了,看到 `[INFO] pub: "Hello from..."` 日志
### Q6: Windows venv 里 `import rclpy` 飘红
**原因**: rclpy 没 Windows wheels
**解决**: 正常现象,不影响阅读代码。要跑 rclpy 在容器里跑
### Q7: 容器里 `pip install` 装不上 ROS2 包
**原因**: 你在容器 venv,ROS2 包来自 apt
**解决**:
```bash
# 不在容器 venv 里
exit # 退出 venv
# 或用 apt
sudo apt install ros-humble-<pkg>
```
### Q8: 容器里中文显示乱码
**原因**: 容器没装中文字体
**解决**: 在容器内 `export LANG=C.UTF-8 LC_ALL=C.UTF-8`
### Q9: `colcon build` 报 `CMakeCache` 错误
**原因**: 之前构建的 CMakeCache 有问题
**解决**:
```bash
cd /root/ros2_ws
rm -rf build install log
colcon build --symlink-install
```
### Q10: 容器跑一段时间后磁盘满了
**原因**: `build/` `install/` `log/` 默认在本目录
**解决**:
```bash
# 进容器清理
cd /root/ros2_ws && rm -rf build install log
bash build.sh
# 或清理 Docker
docker system prune -a
```
---
## 下一步
5 分钟上手成功!现在你可以:
- 阅读 [`doc/10-concepts.md`](10-concepts.md) — 理解 ROS2 核心概念
- 阅读 [`doc/20-topics.md`](20-topics.md) — Topic 深度
- 阅读 [`doc/100-embedded-deployment.md`](100-embedded-deployment.md) — 三机部署实操
- 阅读 [`doc/99-embodied-ai.md`](99-embodied-ai.md) — 具身智能路径
**动手尝试**: 改一下 `py_pubsub` 里 talker 的 `period_ms`,观察 `/chatter` 频率变化。这是理解 ROS2 参数的最快方式。
加油,ROS2 之旅开始! 🚀
doc/02-virtualenv.md
+277
View File
@@ -0,0 +1,277 @@
# 02 · 本机 venv 工作流(不污染系统 Python 完全指南)
> **目标**:用标准 Python venv 在 Windows / Linux / macOS 上做 ROS2 开发,**完全不污染系统 Python**。
---
## 目录
- [1. 为什么需要 venv](#1-为什么需要-venv)
- [2. 双轨制 — venv vs colcon](#2-双轨制--venv-vs-colcon)
- [3. 创建 venv(Win / Linux)](#3-创建-venvwin--linux)
- [4. venv 里装了什么](#4-venv-里装了什么)
- [5. IDE 配置](#5-ide-配置)
- [6. 在 venv 里能做什么 / 不能做什么](#6-在-venv-里能做什么--不能做什么)
- [7. 在容器里跑 ROS2 测试](#7-在容器里跑-ros2-测试)
- [8. 依赖锁文件](#8-依赖锁文件)
- [9. .venv 加进 .gitignore](#9-venv-加进-gitignore)
- [10. 故障排查](#10-故障排查)
- [11. 在本仓库里跑](#11-在本仓库里跑)
---
## 1. 为什么需要 venv
ROS2 的 Python 客户端库(`rclpy` / `sensor_msgs` / `tf2_ros` / `cv_bridge` 等)**只在 Linux 平台通过 apt 提供**,
Windows 上没有官方 pip wheels。
如果直接 `pip install` 到系统 Python:
- 失败(找不到 wheel)
- 或者和别的项目冲突(版本冲突)
- 或者哪天升级 Python 把 ROS 客户端搞挂了
**venv 的目的**:本机 Python 工具(ruff / black / pytest / mypy 等)放隔离环境,与 ROS2 运行解耦。
---
## 2. 双轨制 — venv vs colcon
| 工作 | 用什么 | 跑在哪 |
|---|---|---|
| 编辑代码、写测试 | **本机 venv + IDE** | Win/macOS/Linux |
| 跑 ROS2 节点 / colcon build / colcon test | **Docker 容器** | Linux |
| pytest (纯逻辑、不需要 ROS2) | 本机 venv | Win/macOS/Linux |
| pytest (需要 rclpy) | **Docker 容器** | Linux |
```
本机: venv (开发) Docker: colcon (运行)
───────────────── ────────────────────
ruff / black / mypy colcon build
pytest (纯逻辑) colcon test
IDE 跳转 ros2 run / launch
rclpy / sensor_msgs / tf2_ros
```
---
## 3. 创建 venv
### 3.1 Windows
```powershell
cd D:\xs\ros2
powershell .\tools\setup_venv.ps1
# 激活
.\.venv\Scripts\Activate.ps1
# 验证
python -c "import sys; print(sys.executable)"
# 期望: D:\xs\ros2\.venv\Scripts\python.exe (不是 C:\Program Files\Python310\python.exe)
```
### 3.2 Linux / macOS / WSL
```bash
cd /path/to/ros2
./tools/setup_venv.sh
source .venv/bin/activate
python -c "import sys; print(sys.executable)"
# 期望: /path/to/ros2/.venv/bin/python
```
### 3.3 在 Docker 容器内
容器内系统 Python 已经隔离,venv 多此一举。直接用系统 Python 即可。
---
## 4. venv 里装了什么
`tools/setup_venv.{sh,ps1}` 会做:
1. `python -m venv .venv` — 创建虚拟环境
2. `pip install --upgrade pip setuptools wheel`
3. `pip install -r requirements-dev.txt` — 装开发工具:
- `ruff`(超快 linter,替代 flake8 + isort)
- `black`(格式化)
- `mypy`(类型检查)
- `pytest` + `pytest-cov` + `pytest-timeout`
- `pre-commit`(git hook,可选)
4. `pip install -e src/py_pubsub src/py_srv src/py_action_demo src/py_vision_demo`
把本项目 ROS2 Python 包装到 venv(**可编辑模式** — 改源码立即生效,不需要重装)
`--no-deps` 跳过 ROS 客户端依赖(因为 Windows 装不上)
---
## 5. IDE 配置
### 5.1 VSCode
`pyrightconfig.json` 已配置好,直接:
```json
{
"extraPaths": [
"/opt/ros/humble/lib/python3.10/site-packages" // 容器内
]
}
```
设置 VSCode 的 Python 解释器:`Ctrl+Shift+P` → "Python: Select Interpreter" → 选择 `.venv\Scripts\python.exe`
### 5.2 PyCharm
`Settings → Project → Python Interpreter → Add → Existing environment` → 选 `.venv\Scripts\python.exe`
### 5.3 看代码跳转
- 跳转到 `rclpy.Node` 定义:`Ctrl+点击` — pyright 会去 `/opt/ros/humble/lib/python3.10/site-packages` 找
- **容器外**:Windows 上 import rclpy 仍然飘红,但不影响阅读源码
- **容器内(开发容器)**:跳转正常,因为 `/opt/ros/...` 路径存在
---
## 6. 在 venv 里能做什么 / 不能做什么
| 能做 | 不能做 |
|---|---|
| 编辑 Python 源码,IDE 自动补全 | `import rclpy` (rclpy 没 Windows wheels) |
| 跑 ruff / black / mypy | 跑 ROS2 节点 / launch 文件 |
| 装纯 Python 包(numpy / torch / opencv-python) | 跑 ros2 CLI 命令 |
| 在容器内 venv(可访问 rclpy):跑纯 pytest | |
---
## 7. 在容器里跑 ROS2 测试
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && cd /root/ros2_ws && colcon test --packages-select py_pubsub"
```
colcon test 会自动跑每个包的 `test/` 目录下的测试。本仓库:
- `py_*` 包 → pytest
- `cpp_*` 包 → ament_cmake_gtest(CTest)
- 期望全部 PASSED。
---
## 8. 依赖锁文件
生产环境建议加 `requirements.lock`(`pip freeze > requirements.lock`),精确锁版本。本仓库的
`requirements*.txt` 只列宽松版本(`>=`):
| 文件 | 用途 |
|---|---|
| `requirements.txt` | 运行时纯 Python 依赖(numpy / pytest) |
| `requirements-dev.txt` | 开发工具(ruff / black / mypy) |
---
## 9. .venv 加进 .gitignore
```gitignore
.venv/
```
已加。提交代码不会带 venv 目录。
---
## 10. 故障排查
### 10.1 `python: command not found`
- Windows:安装 Python 3.10+ 并勾选 "Add to PATH"
- Linux: `sudo apt install python3 python3-venv`
### 10.2 `venv` 模块找不到
```bash
# Debian/Ubuntu
sudo apt install python3-venv
# Fedora/RHEL
sudo dnf install python3-virtualenv
```
### 10.3 venv 里 `import rclpy` 仍然飘红
正常。rclpy 在 Windows 没有 pip wheels,需要靠 apt(在容器内)。开发时不影响阅读,
要跑 ROS2 测试就在容器里跑。
### 10.4 venv 占空间
通常 200-500 MB。删掉重建:`rm -rf .venv && tools/setup_venv.sh`。
### 10.5 激活 venv 后 pip 装不上
确认 `which python` / `which pip` 指向 `.venv/bin/`:
```bash
which python # /path/.venv/bin/python
which pip # /path/.venv/bin/pip
```
如果指向系统 Python,venv 没生效。
---
## 11. 在本仓库里跑
### 11.1 创建 venv
```powershell
cd D:\xs\ros2
powershell .\tools\setup_venv.ps1
```
输出:
```
==> Python: Python 3.10.x
==> Creating venv at .venv
==> Upgrading pip + installing requirements-dev.txt
OK py_pubsub
OK py_srv
OK py_action_demo
OK py_vision_demo
✅ venv ready. Activate with:
.\.venv\Scripts\Activate.ps1
```
### 11.2 激活 + 验证
```powershell
.\.venv\Scripts\Activate.ps1
python --version # Python 3.10.x (venv)
pip list # 已装的包
ruff check src/ # linter
mypy src/ # 类型检查(部分飘红是 rclpy 缺,忽略)
```
### 11.3 在 venv 里跑纯 Python 测试(不需要 ROS2)
```bash
# 比如写一个纯算法测试
pytest src/<pkg>/test/test_my_algorithm.py
```
### 11.4 完整工作流
```powershell
# 1. 本机开发:venv + IDE
cd D:\xs\ros2
.\.venv\Scripts\Activate.ps1
code . # VSCode 打开
# 2. 改完代码,跑容器测试
docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon build --packages-select <pkg>"
docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon test --packages-select <pkg>"
# 3. 跑 demo
docker exec ros2_dev bash -lc "source install/setup.bash && ros2 launch bringup <file>"
```
---
## 接下来读
| 主题 | 文档 |
|---|---|
| 项目总览 | [`00-overview.md`](00-overview.md) |
| 5 分钟上手 | [`01-quickstart.md`](01-quickstart.md) |
| 测试策略 | [`90-testing.md`](90-testing.md) |
| Docker | [`85-docker.md`](85-docker.md) |
doc/10-concepts.md
+864
View File
@@ -0,0 +1,864 @@
# 10 · ROS2 核心概念速览(完整概念地图)
> **目标**:30 分钟内把 ROS2 所有核心概念装进脑子里,后续文档都能"秒懂"。
---
## 目录
- [一图总览](#一图总览)
- [1. 计算图(Computational Graph)](#1-计算图computational-graph)
- [2. Node(节点)](#2-node节点)
- [3. Topic(话题)](#3-topic话题)
- [4. Service(服务)](#4-service服务)
- [5. Action(动作)](#5-action动作)
- [6. Parameter(参数)](#6-parameter参数)
- [7. TF2(坐标变换)](#7-tf2坐标变换)
- [8. URDF(机器人模型)](#8-urdf机器人模型)
- [9. sensor_msgs(常用消息类型)](#9-sensor_msgs常用消息类型)
- [10. Launch(启动文件)](#10-launch启动文件)
- [11. DDS / QoS / 时间(底层)](#11-dds--qos--时间底层)
- [12. 6 大概念怎么一起工作](#12-6-大概念怎么一起工作)
- [13. 概念速查表](#13-概念速查表)
---
## 一图总览
```
┌──────────────────────────────────────────────────────────────────┐
│ DDS 总线 (默认 fastdds) │
│ │
│ Topic /chatter Topic /image_raw Topic /tf │
│ Topic /joint_states Service /add_two_ints │
│ Action /fibonacci Parameter server │
└───────┬─────────────────┬──────────────────┬─────────────────────┘
│ │ │
▼ ▼ ▼
┌─────────┐ ┌──────────┐ ┌───────────┐
│ talker │ │ camera │ │ tf listener│
│ listener│ │processor │ │joint_pub │
│ (Node) │ │ (Node) │ │ (Node) │
└─────────┘ └──────────┘ └───────────┘
```
**6 大概念**:Node / Topic / Service / Action / Parameter / TF。
下面 12 节拆解。
---
## 1. 计算图(Computational Graph)
### 1.1 是什么
ROS2 程序由**节点**(Node)组成,节点之间通过**话题/服务/动作**连接,形成一张"计算图"。
图不是静态配置,是**运行时由 DDS 自动发现**。
### 1.2 看计算图
```bash
# 命令行看拓扑
ros2 node list
ros2 topic list
ros2 service list
ros2 action list
# 可视化(GUI)
ros2 run rqt_graph rqt_graph
```
### 1.3 三个关键术语
| 术语 | 含义 |
|---|---|
| **Node** | 一个独立运行的程序 |
| **Edge** | Node 之间的连接(Topic / Service / Action) |
| **Discovery** | DDS 自动找节点,无需中心注册 |
---
## 2. Node(节点)
### 2.1 是什么
ROS2 程序的**基本单元**。一个 Node = 一个进程 = 一项业务功能。
### 2.2 怎么写(Python)
```python
import rclpy
from rclpy.node import Node
class MyNode(Node):
def __init__(self):
super().__init__('my_node_name') # 节点名,在 ROS Domain 内必须唯一
# 在这里创建 publisher / subscription / timer / param
self.timer_ = self.create_timer(0.5, self.tick)
def tick(self):
self.get_logger().info('tick')
def main():
rclpy.init() # 全局初始化
node = MyNode()
rclpy.spin(node) # 进入事件循环,阻塞
rclpy.shutdown() # 退出
if __name__ == '__main__':
main()
```
### 2.3 怎么写(C++)
```cpp
#include "rclcpp/rclcpp.hpp"
class MyNode : public rclcpp::Node {
public:
MyNode() : rclcpp::Node("my_node_name") {
timer_ = this->create_wall_timer(
500ms, std::bind(&MyNode::tick, this));
}
private:
void tick() {
RCLCPP_INFO(this->get_logger(), "tick");
}
rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char * argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MyNode>());
rclcpp::shutdown();
return 0;
}
```
### 2.4 关键 API
| API | 用途 |
|---|---|
| `super().__init__('name')` | 节点构造时设名字 |
| `create_publisher(...)` | 创建发布者 |
| `create_subscription(...)` | 创建订阅者 |
| `create_service(...)` | 创建服务端 |
| `create_action_server(...)` | 创建动作服务端 |
| `create_timer(period, cb)` | 创建定时器 |
| `declare_parameter(...)` | 声明参数 |
| `get_logger().info(...)` | 打日志 |
| `get_clock()` | 取时钟 |
| `rclpy.spin(node)` | 进入事件循环,处理所有回调 |
### 2.5 生命周期
```
rclpy.init() ─┐
├─ 实例化 Node ─ 创建 pub/sub/service ─ spin 等待回调
实例化 Node ─┘
│
▼
rclpy.spin(node) ── 阻塞,处理 timer / subscription / service 回调
│
▼ (Ctrl+C 或 shutdown)
rclpy.shutdown() ── 清理
```
### 2.6 本仓库对照
- Python:[`src/py_pubsub/py_pubsub/publisher_member_function.py`](../src/py_pubsub/py_pubsub/publisher_member_function.py)
- C++:[`src/cpp_pubsub/src/publisher_member_function.cpp`](../src/cpp_pubsub/src/publisher_member_function.cpp)
---
## 3. Topic(话题)
### 3.1 是什么
**异步、多对多、单向**的发布订阅通道。
- 异步:`publish()` 不阻塞
- 多对多:1 个 publisher,任意多个 subscriber
- 单向:消息只从 pub 到 sub,反方向不通
### 3.2 通信模型
```
Publisher ──publish()──> Topic (chatter) ──callback(msg)──> Subscriber
────────────────────────────────────────────────────
异步,非阻塞 多对多,单向 事件循环触发
```
### 3.3 vs 其他通信方式
| | Topic | Service | Action |
|---|---|---|---|
| 同步 | 异步 | 同步 | 异步 |
| 一对多 | ✅ | ❌ | ❌ |
| 反馈 | ❌ | ❌ | ✅ |
| 取消 | N/A | ❌ | ✅ |
### 3.4 Python API
```python
# Publisher
pub = node.create_publisher(
std_msgs.msg.String, # 消息类型
'chatter', # topic 名
10 # QoS 深度
)
# 构造 + 发送
msg = std_msgs.msg.String()
msg.data = 'hello'
pub.publish(msg) # 异步,不等任何东西
# Subscriber
sub = node.create_subscription(
std_msgs.msg.String, # 类型
'chatter', # topic 名
callback, # 回调函数
10 # QoS 深度
)
def callback(msg):
node.get_logger().info(f'recv: {msg.data}')
```
### 3.5 C++ API
```cpp
// Publisher
auto pub = node->create_publisher<std_msgs::msg::String>("chatter", 10);
auto msg = std_msgs::msg::String();
msg.data = "hello";
pub->publish(msg);
// Subscriber
auto sub = node->create_subscription<std_msgs::msg::String>(
"chatter", 10,
[this](const std_msgs::msg::String::SharedPtr msg) {
RCLCPP_INFO(this->get_logger(), "recv: %s", msg->data.c_str());
});
```
### 3.6 QoS(服务质量)速查
| 维度 | 取值 | 默认 | 影响 |
|---|---|---|---|
| Reliability | RELIABLE / BEST_EFFORT | RELIABLE | 必须 pub/sub 一致 |
| History | KEEP_LAST(N) / KEEP_ALL | KEEP_LAST(10) | 队列大小 |
| Durability | VOLATILE / TRANSIENT_LOCAL | VOLATILE | 晚订阅者是否收到旧数据 |
**兼容规则**:
- RELIABLE ↔ RELIABLE ✅
- BEST_EFFORT ↔ BEST_EFFORT ✅
- BEST_EFFORT → RELIABLE ✅(sub 容忍丢)
- RELIABLE → BEST_EFFORT ❌(sub 不发 ACK,pub 报错)
### 3.7 常用 CLI
```bash
ros2 topic list # 所有 topic
ros2 topic info <topic> -v # 类型 + pub/sub 列表
ros2 topic echo <topic> # 实时打印
ros2 topic hz <topic> # 频率 Hz
ros2 topic bw <topic> # 带宽 bytes/s
ros2 topic pub <topic> <type> "<msg>" --once # 发一条测试
ros2 bag record <topic> # 录包
ros2 bag play <bag> # 回放
```
### 3.8 本仓库对照
- 包:[`src/py_pubsub/`](../src/py_pubsub/), [`src/cpp_pubsub/`](../src/cpp_pubsub/)
- 跨语言互通验证:[`docker/bringup_e2e.log`](../docker/bringup_e2e.log)
**深度**: [`doc/20-topics.md`](20-topics.md)
---
## 4. Service(服务)
### 4.1 是什么
**同步、一对一、双向**的请求/响应。
- 一次性调用 + 等结果
- 几毫秒到几秒
### 4.2 通信模型
```
Client ──call(req)──> Server
◀──response────
同步(阻塞),一次一答
```
### 4.3 .srv 文件定义
```srv
# example_interfaces/srv/AddTwoInts.srv
int64 a # Request 字段
int64 b
---
int64 sum # Response 字段
```
- `---` 上 = Request
- `---` 下 = Response
### 4.4 Server 端
```python
from example_interfaces.srv import AddTwoInts
srv = node.create_service(
AddTwoInts, # 服务类型
'add_two_ints', # 服务名
callback # 回调签名:callback(req, resp) -> resp
)
def callback(request, response):
response.sum = request.a + request.b
return response # 必须 return response
```
### 4.5 Client 端(异步风格)
```python
client = node.create_client(AddTwoInts, 'add_two_ints')
while not client.wait_for_service(timeout_sec=1.0):
node.get_logger().info('waiting...')
req = AddTwoInts.Request()
req.a = 12; req.b = 30
future = client.call_async(req)
# 必须 spin 让 future 完成
rclpy.spin_until_future_complete(node, future, timeout_sec=5.0)
result = future.result()
print(result.sum) # 42
```
### 4.6 何时用
| 场景 | 用 Service |
|---|---|
| 拍照(几 ms) | ✅ |
| 关节角度查询 | ✅ |
| 计算查询 | ✅ |
| 周期性相机帧 | ❌ → Topic |
| 长任务(几秒到几分钟) | ❌ → Action |
### 4.7 常用 CLI
```bash
ros2 service list
ros2 service type <name> # 类型
ros2 service call <name> <type> "<req>"
# 例:
ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 12, b: 30}"
```
**深度**: [`doc/30-services.md`](30-services.md)
---
## 5. Action(动作)
### 5.1 是什么
Service 的"长任务"版本:
- client 发 Goal
- server 周期性推 Feedback
- 最终返回 Result
- **client 可中途 cancel**
适合:抓取、导航、SLAM。
### 5.2 通信模型
```
Client Server
│ Goal │
├────────────────────>│
│ ▼ 执行中
│ Feedback │
│<────────────────────┤
│ Feedback │
│<────────────────────┤
│ Result │
│<────────────────────┤
│ cancel (可选) │
├────────────────────>│
```
### 5.3 .action 文件定义
```action
# example_interfaces/action/Fibonacci.action
int32 order
---
int32[] sequence # Result
---
int32[] sequence # Feedback
```
| 段 | 含义 |
|---|---|
| 第一段 | Goal(client 发) |
| `---` | 分隔 |
| 第二段 | Result(server 最终给一次) |
| `---` | 分隔 |
| 第三段 | Feedback(server 周期性推 0..N 次) |
### 5.4 Server 端
```python
import rclpy.action
action_server = rclpy.action.ActionServer(
node,
Fibonacci, # ActionType
'fibonacci', # action 名
execute_callback # 签名:cb(goal_handle) -> result
)
def execute_callback(goal_handle):
order = goal_handle.request.order
feedback = Fibonacci.Feedback()
result = Fibonacci.Result()
sequence = [0, 1]
for i in range(1, order):
if goal_handle.is_cancel_requested:
goal_handle.canceled()
return Fibonacci.Result()
sequence.append(sequence[i] + sequence[i-1])
feedback.sequence = sequence
goal_handle.publish_feedback(feedback)
goal_handle.succeed()
result.sequence = sequence
return result
```
### 5.5 Client 端
```python
client = ActionClient(node, Fibonacci, 'fibonacci')
client.wait_for_server()
goal = Fibonacci.Goal()
goal.order = 6
future = client.send_goal_async(
goal,
feedback_callback=lambda msg: print('fb:', msg.feedback.sequence)
)
future.add_done_callback(cb_goal_response)
```
### 5.6 状态机
```
PENDING → ACCEPTED → EXECUTING → SUCCEEDED
ABORTED
CANCELED
```
### 5.7 MultiThreadedExecutor(关键!)
Action server 的 execute + publish_feedback 都在主线程。**用 MultiThreadedExecutor**
避免 Feedback 卡死:
```python
from rclpy.executors import MultiThreadedExecutor
executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(node)
executor.spin()
```
### 5.8 何时用 vs Service
| 持续 | 用 |
|---|---|
| < 几秒 | Service |
| 几秒 ~ 几小时 | Action |
**深度**: [`doc/40-actions.md`](40-actions.md)
---
## 6. Parameter(参数)
### 6.1 是什么
节点的**运行时配置项**。声明后可以运行时改,不重编译。
### 6.2 声明 / 读取 / 写
```python
class Talker(Node):
def __init__(self):
super().__init__('talker_py')
# 声明参数 + 默认值
self.declare_parameter('period_ms', 500)
self.declare_parameter('topic', 'chatter')
# 读取
period = self.get_parameter('period_ms').value
topic = self.get_parameter('topic').value
def change_param(self, new_period):
# 运行时改
param = rclpy.parameter.Parameter(
'period_ms', rclpy.Parameter.Type.INTEGER, new_period
)
self.set_parameters([param])
```
### 6.3 CLI 改参数
```bash
ros2 param list # 节点的所有参数
ros2 param get /talker_py period_ms
ros2 param set /talker_py period_ms 200
ros2 param describe /talker_py period_ms
ros2 param dump /talker_py > params.yaml # 导出
ros2 param load /talker_py params.yaml # 加载
```
### 6.4 launch 中覆盖
```python
Node(
package='py_pubsub',
executable='talker',
parameters=[{'period_ms': 200, 'topic': 'chatter'}] # 覆盖
)
```
CLI 启动:
```bash
ros2 run py_pubsub talker --ros-args -p period_ms:=200 -p topic:=hello
```
### 6.5 YAML 文件
```yaml
# config/params.yaml
talker_py:
ros__parameters:
period_ms: 200
topic: chatter
```
```python
Node(package='py_pubsub', executable='talker',
parameters=[get_package_share_directory('my_pkg') + '/config/params.yaml'])
```
---
## 7. TF2(坐标变换)
### 7.1 是什么
管理**机器人所有坐标系**之间相对位姿的工具。维护一棵 **TF tree**。
### 7.2 典型 TF tree
```
world
└─ map (SLAM)
└─ odom (AMCL / 里程计)
└─ base_link (底盘)
├─ base_footprint
├─ laser_frame (激光雷达)
├─ camera_optical_frame (相机)
└─ arm_base (机械臂)
└─ shoulder (关节 1)
└─ upper_arm (关节 2)
└─ wrist (关节 3)
└─ gripper (夹爪)
```
### 7.3 C++ Listener API
```cpp
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
tf2_ros::Buffer buffer(node->get_clock());
tf2_ros::TransformListener listener(buffer, node);
// 查"gripper 在 base_link 下"
geometry_msgs::msg::TransformStamped tf = buffer.lookupTransform(
"base_link", // target frame
"gripper", // source frame
tf2::TimePointZero); // latest
RCLCPP_INFO(node->get_logger(),
"gripper in base_link: x=%.3f y=%.3f z=%.3f",
tf.transform.translation.x, ...);
```
### 7.4 几何变换(把点变换到其他系)
```cpp
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
geometry_msgs::msg::PointStamped p_in, p_out;
p_in.header.frame_id = "world";
p_in.point.x = 1.0; p_in.point.y = 2.0; p_in.point.z = 3.0;
p_out = buffer.transform(p_in, "robot");
// p_out 在 robot 系下
```
### 7.5 Python Listener
```python
from tf2_ros import Buffer, TransformListener
buffer = Buffer()
listener = TransformListener(buffer, node)
try:
tf = buffer.lookup_transform(
'base_link', 'gripper', rclpy.time.Time(),
timeout=rclpy.duration.Duration(seconds=1.0))
print(tf.transform.translation)
except Exception as e:
node.get_logger().warn(f'lookup failed: {e}')
```
### 7.6 静态 TF(不变的关系)
```bash
# 命令行:激光雷达固定在底盘
ros2 run tf2_ros static_transform_publisher \
--x 0.1 --y 0 --z 0.2 --roll 0 --pitch 0 --yaw 0 \
--frame-id base_link --child-frame-id laser_frame
```
```python
# launch 里
Node(package='tf2_ros', executable='static_transform_publisher',
arguments=['--x', '0.1', '--y', '0', '--z', '0.2',
'--frame-id', 'base_link', '--child-frame-id', 'laser_frame'])
```
### 7.7 动态 TF(URDF + JointState)
`robot_state_publisher` (ROS2 系统包):
- 输入:URDF + `/joint_states`
- 输出:动态 TF 到 `/tf`
```bash
sudo apt install ros-humble-robot-state-publisher
ros2 run robot_state_publisher robot_state_publisher \
--ros-args -p robot_description:="$(xacro arm.urdf)"
```
### 7.8 调试命令
```bash
ros2 run tf2_tools view_frames # 生成 frames.pdf
ros2 run tf2_ros tf2_echo base_link gripper # 实时打印
ros2 topic hz /tf # /tf 频率
ros2 topic info /tf_static -v # 静态 TF 列表
```
**深度**: [`doc/50-tf2.md`](50-tf2.md)
---
## 8. URDF(机器人模型)
### 8.1 是什么
XML 格式的机器人描述:link / joint / 视觉 / 碰撞 / 物理参数。
### 8.2 最小 URDF
```xml
<?xml version="1.0"?>
<robot name="my_arm">
<link name="base_link">
<visual><geometry><box size="0.1 0.1 0.1"/></geometry></visual>
<collision><geometry><box size="0.1 0.1 0.1"/></geometry></collision>
<inertial><mass value="1.0"/><inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/></inertial>
</link>
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0.05"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="1.0" velocity="1.0"/>
</joint>
<link name="link1">
<visual><geometry><box size="0.05 0.05 0.1"/></geometry></visual>
<collision><geometry><box size="0.05 0.05 0.1"/></geometry></collision>
<inertial><mass value="0.5"/><inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.005"/></inertial>
</link>
</robot>
```
### 8.3 关节类型
| type | 含义 | DoF |
|---|---|---|
| `revolute` | 转动(有限位) | 1 |
| `continuous` | 转动(无限位) | 1 |
| `prismatic` | 滑动 | 1 |
| `fixed` | 固定 | 0 |
| `floating` | 6 DoF | 6 |
### 8.4 校验
```bash
sudo apt install liburdfdom-tools
check_urdf my_arm.urdf # 校验 URDF
urdf_to_graphiz my_arm.urdf # 生成 PDF/PNG 图
```
**深度**: [`doc/60-urdf.md`](60-urdf.md)
---
## 9. sensor_msgs(常用消息类型)
| 消息 | 关键字段 | 用途 |
|---|---|---|
| `std_msgs/String` | `string data` | 通用 |
| `std_msgs/Header` | `stamp`, `frame_id` | 时间戳 + 坐标系 |
| `sensor_msgs/Image` | `height/width/encoding/step/data` | 相机图像 |
| `sensor_msgs/CameraInfo` | `K/D/R/P` | 相机内参/外参 |
| `sensor_msgs/PointCloud2` | `height/width/data/is_bigendian` | 3D 点云 |
| `sensor_msgs/JointState` | `name[]/position[]/velocity[]/effort[]` | 关节状态 |
| `sensor_msgs/Imu` | `orientation/angular_velocity/linear_acceleration` | IMU |
| `geometry_msgs/Pose` | `Point position + Quaternion orientation` | 位姿 |
| `geometry_msgs/PoseStamped` | `Header + Pose` | 带时间戳位姿 |
| `geometry_msgs/Twist` | `Vector3 linear + Vector3 angular` | 速度指令 |
| `geometry_msgs/Transform` | `Vector3 translation + Quaternion rotation` | TF 变换 |
| `nav_msgs/Odometry` | `pose + twist + covariances` | 里程计 |
| `nav_msgs/Path` | `PoseStamped[]` | 路径 |
| `trajectory_msgs/JointTrajectory` | `JointTrajectoryPoint[]` | 关节轨迹(ros2_control 用) |
| `trajectory_msgs/JointTrajectoryPoint` | `positions[]/velocities[]/accelerations[]/effort[]` | 单点轨迹 |
| `vision_msgs/Detection2DArray` | `detections[]` | 目标检测结果 |
---
## 10. Launch(启动文件)
### 10.1 是什么
Python 脚本,描述"启动哪些节点 + 参数",取代 ROS1 XML。
### 10.2 最小 launch
```python
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='my_pkg',
executable='my_node',
name='my_node',
output='screen',
parameters=[{'param1': 'value1'}],
),
])
```
### 10.3 启动方式
```bash
ros2 launch <pkg> <launch_file.py>
ros2 launch <pkg> <launch_file.py> topic:=hello period_ms:=200
```
**深度**: [`doc/70-launch.md`](70-launch.md)
---
## 11. DDS / QoS / 时间(底层)
### 11.1 DDS 是什么
ROS2 默认用 **DDS**(Data Distribution Service)做底层通信。
本仓库默认 **FastDDS**(`rmw_fastrtps_cpp`)。
DDS 提供的核心能力:
- 自动节点发现(基于 UDP multicast)
- 多种 QoS(可靠 / 尽力而为)
- 实时性
### 11.2 RMW(ROS Middleware)
ROS2 用 RMW 抽象层,RMW 是 DDS 的 ROS 包装:
| RMW 实现 | 包 | 适用 |
|---|---|---|
| `rmw_fastrtps_cpp` | `ros-humble-rmw-fastrtps-cpp`(默认) | 通用 |
| `rmw_cyclonedds_cpp` | `ros-humble-rmw-cyclonedds-cpp` | 跨网段更稳 |
切换:
```bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
```
### 11.3 时间
| 概念 | 含义 | 用法 |
|---|---|---|
| `system time` | 墙钟时间(默认) | 仿真时需配 `use_sim_time: true` |
| `sim time` | 仿真时钟(`/clock` topic) | Gazebo / 录包回放 |
| `Header.stamp` | 消息时间戳 | TF 同步、消息时间关联 |
---
## 12. 6 大概念怎么一起工作
```
┌────────────────────────────────────────────────┐
│ application │
│ (决策 / 控制 / 感知 / 学习) │
└─────────┬──────────────────────────────────────┘
│ 用 TF / Topic / Service / Action / Parameter
┌─────────▼──────────────────────────────────────┐
│ ROS2 中间件 (rclcpp/rclpy) │
│ ┌────────────────────────────────────────┐ │
│ │ Topic Pub/Sub (异步多对多单向) │ │
│ │ Service Req/Resp (同步一对一双向) │ │
│ │ Action Goal/Feedback/Result (长任务) │ │
│ │ Param 配置 │ │
│ │ TF2 坐标变换树 │ │
│ └────────────────────────────────────────┘ │
│ ┌────────────────────────────────────────┐ │
│ │ DDS (默认 fastdds) │ │
│ └────────────────────────────────────────┘ │
└────────────────────────────────────────────────┘
│ 走 UDP multicast / unicast
┌─────────▼──────────────────────────────────────┐
│ 网络 │
└─────────────────────────────────────────────────┘
```
---
## 13. 概念速查表
| 概念 | 一句话 | API | 深度文档 |
|---|---|---|---|
| Node | 一个进程 = 一项业务 | `rclpy.spin(node)` | 本篇 §2 |
| Topic | 异步多对多单向 | `create_publisher / create_subscription` | [`20-topics`](20-topics.md) |
| Service | 同步 1对1 双向 | `create_service / create_client` | [`30-services`](30-services.md) |
| Action | 长任务 + 反馈 + 可取消 | `ActionServer / ActionClient` | [`40-actions`](40-actions.md) |
| Parameter | 运行时配置 | `declare_parameter / set_parameter` | 本篇 §6 |
| TF2 | 坐标系树 | `Buffer.lookup_transform` | [`50-tf2`](50-tf2.md) |
| URDF | 机器人 XML 描述 | `check_urdf` | [`60-urdf`](60-urdf.md) |
| Launch | 启动脚本 | `ros2 launch <pkg> <file>` | [`70-launch`](70-launch.md) |
| DDS | 通信中间件 | `RMW_IMPLEMENTATION=` | 本篇 §11 |
---
## 接下来读
按主题深读:
| 你想深挖什么 | 看 |
|---|---|
| Topic 实现细节、QoS、跨语言互通 | [`20-topics.md`](20-topics.md) |
| Service 实现、req/resp 时序 | [`30-services.md`](30-services.md) |
| Action 三件套、cancel、MultiThreadedExecutor | [`40-actions.md`](40-actions.md) |
| TF tree / lookup / static_transform | [`50-tf2.md`](50-tf2.md) |
| URDF / xacro / robot_state_publisher | [`60-urdf.md`](60-urdf.md) |
| launch 嵌套、参数覆盖、事件 | [`70-launch.md`](70-launch.md) |
| 三机部署(PC + RDK X5 + RK3506) | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
| 进入具身智能 / VLA / 机器人 | [`99-embodied-ai.md`](99-embodied-ai.md) |
doc/100-embedded-deployment.md
+918
View File
@@ -0,0 +1,918 @@
# 100 · 嵌入式部署:RDK X5 + RK3506 + ROS2(完全实操指南)
> **本篇目标**:从"硬件到手"到"三机 ROS2 互通、RK3506 控制真实电机"全流程跑通。
> 假设你手上有:1 台 PC + 1 台 RDK X5 + 2 台 RK3506。本指南按 **step-by-step** 走,每步给出**预期输出 + 排错**。
---
## 目录
- [第 0 章: 准备与检查清单](#0-准备与检查清单)
- [第 1 章: 三层架构详解](#1-三层架构详解)
- [第 2 章: PC 主控完整配置](#2-pc-主控完整配置)
- [第 3 章: RDK X5 边缘 AI 配置](#3-rdk-x5-边缘-ai-配置)
- [第 4 章: RK3506 精简 ROS2 配置(核心)](#4-rk3506-精简-ros2-配置核心)
- [第 5 章: 三机 LAN DDS 组网](#5-三机-lan-dds-组网)
- [第 6 章: 跨机 Demo 联调](#6-跨机-demo-联调)
- [第 7 章: RK3506 接真实电机 / 编码器](#7-rk3506-接真实电机--编码器)
- [第 8 章: ros2_control 闭环](#8-ros2_control-闭环)
- [第 9 章: 故障排查大全](#9-故障排查大全)
- [第 10 章: 性能调优 Checklist](#10-性能调优-checklist)
- [附录 A: 采购清单](#附录-a-采购清单)
- [附录 B: 关键命令速查](#附录-b-关键命令速查)
---
## 0. 准备与检查清单
### 0.1 你需要的硬件
- [ ] PC 一台(Ubuntu 22.04 / Windows + Docker)
- [ ] RDK X5 一台(已烧 Ubuntu 22.04 官方镜像)
- [ ] RK3506 两台(已烧 Linux 镜像,默认账户 `root` 或 `user`)
- [ ] 网线 + 路由器 / 交换机(三个设备同网段)
- [ ] RK3506 ↔ PC / RDK X5 的 USB-TTL 串口线(debug 用)
### 0.2 网络规划(提前固定)
```
PC 192.168.1.10
RDK X5 192.168.1.20
RK3506-1 192.168.1.31
RK3506-2 192.168.1.32
子网掩码 255.255.255.0
ROS_DOMAIN_ID = 0 (三层共用)
```
> 💡 **强烈建议固定 IP**(路由器 DHCP 静态分配 或 各板 `/etc/network/interfaces` 写死),避免重启后地址变了连不上。
### 0.3 先确认每台设备能 SSH / 访问
```bash
# PC
ssh user@192.168.1.10 # 应该能登
# RDK X5
ssh user@192.168.1.20
# RK3506
ssh user@192.168.1.31
ssh user@192.168.1.32
```
每台板子**先 ping 互通**:
```bash
ping -c 3 192.168.1.20 # 在 PC 上 ping RDK X5
ping -c 3 192.168.1.31 # 在 PC 上 ping RK3506 #1
```
**预期**: `0% packet loss`。
**若不通**: 检查网线、IP、子网掩码、路由器是否拦 IGMP。
---
## 1. 三层架构详解
### 1.1 为什么分层?
| 层 | 设备 | 跑什么 | 不跑什么 | 为什么 |
|---|---|---|---|---|
| **PC (主控)** | x86 8GB+ | MoveIt2 / Nav2 / RViz / 仿真 / 编译 | 真电机驱动 | 算力大但不便实时控制硬件 |
| **RDK X5 (边缘 AI)** | ARM + 5 TOPS NPU | YOLO / SAM / Whisper / SLAM | MoveIt2 大规划 | NPU 加速推理,分担 PC 算力 |
| **RK3506 (实时控制)** | ARM 3 核 512MB | JointState / PID 闭环 / 编码器读取 | AI 推理 / RViz | 资源紧,但本地控制实时性好 |
### 1.2 数据流(典型抓取场景)
```
┌─────────────────────────────────────────┐
│ PC │
│ RViz 可视化 + 决策 + MoveIt2 规划 │
└─────┬───────────────────────┬───────────┘
│ /grasp_pose │ /joint_trajectory
│ (DDS) │ (DDS)
┌───────────────────────▼─────────┐ ┌─────────▼───────────┐
│ RDK X5 │ │ RK3506 #1 │
│ vision_node: │ │ joint_state_publisher│
│ YOLO 检测 → publish /grasp │ │ 编码器 → /joint_states│
│ (可选) VLA 推理(NPU 加速) │ │ 订阅 /cmd_vel 控制电机│
└─────────────────────────────────┘ └─────────────────────┘
┌─────────────────────┐
│ RK3506 #2 │
│ NPU 推理 / 备用控制│
│ (YOLO-Lite / SAM) │
└─────────────────────┘
```
### 1.3 通信方式(三选一)
| 方式 | 优点 | 缺点 | 用法 |
|---|---|---|---|
| **Multicast 默认** | 零配置,自动发现 | 路由器/防火墙可能拦 | 同网段 + 默认 |
| **Unicast discovery server** | 跨网段可用 | 要起 discovery server | 多 VLAN / 跨子网 |
| **Unicast `ROS_STATIC_PEERS`** | 简单、稳 | 节点列表要预先列 | 小规模 LAN |
**推荐先试 multicast,不通再切 unicast**。下面 Step 5 详解。
---
## 2. PC 主控完整配置
### 2.1 你的工作环境(选项 A: Linux)
假设你用 **Ubuntu 22.04** 真机(不是 Docker)。
```bash
# 2.1.1 装 ROS2 Humble
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt install curl -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update
sudo apt install ros-humble-desktop python3-colcon-common-extensions -y
# 验证
source /opt/ros/humble/setup.bash
ros2 --help # 应该输出 ros2 命令帮助
```
### 2.2 你的工作环境(选项 B: Windows + Docker)——本仓库主路径
如果你用的是本仓库的 Docker 工作流,环境已就绪:
```powershell
docker exec -it ros2_dev bash -lc "source /opt/ros/humble/setup.bash && source /root/ros2_ws/install/setup.bash && exec bash"
```
后续章节都假设你**在 Docker 容器内**操作。**PC 的"ROS_DOMAIN_ID=0"和"DDS 自动发现"对 LAN 上其他设备是开放的**——因为 docker-compose 用 `network_mode: host`,容器用宿主机网络栈。
### 2.3 PC 上启动本仓库的 bringup 全套
```bash
docker exec ros2_dev bash -lc "cd /root/ros2_ws && source install/setup.bash && ros2 launch bringup full_demo_launch.py"
```
预期输出(日志截取):
```
[INFO] [launch]: All log files can be found below /root/.ros/log/2026-08-03-...
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [talker-1]: process started with pid [56]
[INFO] [listener-2]: process started with pid [58]
[INFO] [talker-3]: process started with pid [60]
[INFO] [listener-4]: process started with pid [62]
[talker-3] [INFO] [...] talker_cpp started -> topic=chatter, period=500ms
[listener-4] [INFO] [...] listener_cpp subscribed <- chatter
```
11 个节点跑起来。如果 LAN 上有 RDK X5 / RK3506 节点,它们会自动被发现。
### 2.4 PC 端跨机网络验证
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 node list --no-daemon"
```
预期(假设 RDK X5 还没装,只有 PC 自己):
```
/joint_state_publisher_cpp
/talker_py
/talker_cpp
/listener_py
/listener_cpp
/tf2_listener_cpp
/fake_camera_py
/image_processor_py
/fibonacci_action_server_py
/add_two_ints_server_py
```
如果 RDK X5 装好 ROS2 后,**它的节点也会出现在这里**。
---
## 3. RDK X5 边缘 AI 配置
### 3.1 准备工作
```bash
# SSH 到 RDK X5
ssh user@192.168.1.20
# 检查系统
uname -a
# Linux rdk-x5 5.10.xxx ...
cat /etc/os-release
# Ubuntu 22.04.4 LTS ... (地瓜官方镜像)
# 网络
ip addr show | grep inet
# 应该看到 192.168.1.20/24
```
### 3.2 装 ROS2(完整版)
```bash
# 同 PC 步骤,装桌面版(包含 RViz 远程)
sudo apt install ros-humble-desktop -y
# 加装本仓库相关包
sudo apt install -y \
ros-humble-cv-bridge \
ros-humble-tf2-ros \
ros-humble-tf2-tools \
ros-humble-ros-gz \
ros-humble-nav2-bringup \
ros-humble-moveit \
ros-humble-image-transport \
ros-humble-vision-msgs \
ros-humble-sensor-msgs \
ros-humble-geometry-msgs \
ros-humble-rmw-fastrtps-cpp \
python3-colcon-common-extensions
```
预计下载 ~500MB,装 5-10 分钟。
### 3.3 拷本仓库源码 + colcon build
```bash
# 从 PC 拷(在 PC 上)
scp -r D:\xs\ros2 user@192.168.1.20:~/ros2_ws
# 注意:D:\xs\ros2 拷到 RDK X5 后变成 ~/ros2_ws
# 在 RDK X5 上(SSH 进)
cd ~/ros2_ws
source /opt/ros/humble/setup.bash
# ARM 编译,首次 5-10 分钟
colcon build --symlink-install \
--packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup
# 预期输出(末尾)
Summary: 7 packages finished [8m 32s]
```
> ⚠️ **arm64 兼容性**: 全部 7 个包都用纯 Python + 标准 C++,**没有架构专属代码**,ARM 上 build 应该一次过。如果遇到 `Could NOT find X`,检查 apt 包名。
### 3.4 RDK X5 上启动 vision demo
```bash
# SSH 到 RDK X5
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
# 设 ROS_DOMAIN_ID(虽然默认是 0,但显式更稳)
export ROS_DOMAIN_ID=0
# 启动 fake_camera (sensor_msgs/Image 发布者)
ros2 run py_vision_demo fake_camera
# 预期
[INFO] [fake_camera_py]: fake_camera_py started: 320x240 @ 10fps -> /image_raw
```
### 3.5 PC 上验证 RDK X5 节点可见
```bash
# 在 PC / 容器内
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 node list --no-daemon"
# 应该看到:
# /fake_camera_py (来自 RDK X5)
# /talker_py /listener_py ... (本地)
# 看 RDK X5 上 fake_camera 发的 /image_raw 频率
ros2 topic hz /image_raw --no-daemon
# 预期:average rate: 10.000
```
### 3.6 RDK X5 跑 YOLO 推理(扩展)
如果 RDK X5 装了 ultralytics + YOLO 模型:
```bash
# 安装
pip install ultralytics --break-system-packages # 如果是 Buildroot 系统不带 pip,先 apt install python3-pip
# 准备模型
# 把 yolov8n.pt 拷到 ~/models/
# 写一个 yolo_detector 节点(参考 py_vision_demo 写法):
# 订阅 /camera/color/image_raw (或 fake_camera 的 /image_raw)
# 推理 → publish /detections (vision_msgs/Detection2DArray)
```
这部分**需要你写新包**,参考 [`src/py_vision_demo/`](../src/py_vision_demo/)。
---
## 4. RK3506 精简 ROS2 配置(核心)
### 4.1 为什么 RK3506 要"精简"
| 配置 | 占用 RAM |
|---|---|
| Linux 基础系统 | ~80 MB |
| ROS2 rclpy 客户端 | ~50-80 MB |
| FastDDS 参与者(单节点) | ~30-50 MB |
| 默认 daemon (`ros2 daemon`) | ~50 MB ← **必须关** |
| RViz / rqt | ~300 MB ← **绝对不装** |
| MoveIt2 / Nav2 | 200-500 MB ← **不装** |
不精简 = 直接 OOM。
### 4.2 准备 RK3506
```bash
ssh user@192.168.1.31
uname -a
# Linux rk3506 5.10.xxx ...
free -h
# total used free shared buff/cache available
# Mem: 484Mi 95Mi 240Mi 1.0Mi 148Mi 380Mi
df -h /
# /dev/root 7.4G 1.2G 5.8G /
# 看是不是 aarch64(必须)
uname -m
# aarch64
```
### 4.3 装精简 ROS2
```bash
# 4.3.1 装 ROS2 apt 源(同 PC)
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt install curl -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=arm64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update
# 4.3.2 装精简 ROS2 包(注意:没有 desktop)
sudo apt install -y \
ros-humble-ros-base \
ros-humble-cv-bridge \
ros-humble-tf2-ros \
ros-humble-sensor-msgs \
ros-humble-geometry-msgs \
ros-humble-rmw-fastrtps-cpp \
ros-humble-ros2control \
python3-colcon-common-extensions
# 检查内存
free -h
# 装完后大约用掉 ~250-280 MB(系统+ROS2+daemon)
```
### 4.4 关闭 ROS2 daemon(省 ~50MB)
ROS2 默认启动 `ros2 daemon` 加速 `ros2 ...` 命令调用,但每节点起一个 daemon 浪费内存。
```bash
# 停 + 禁启 daemon
pkill -9 -f ros2_daemon 2>/dev/null
echo 'unset ROS_DAEMON_PYTHON_OR_EXECUTABLE' >> ~/.bashrc
# 或 alias,直接绕过 daemon
cat >> ~/.bashrc <<'EOF'
alias ros2='ros2 --no-daemon'
EOF
source ~/.bashrc
# 验证: 不再起 daemon
ps aux | grep -v grep | grep -i daemon | head -3
# 应该空
```
### 4.5 加 FastDDS 单播发现配置
RK3506 默认走 multicast,但某些网络环境 multicast 不通(如路由器拦了)。**改用 unicast 显式指定 PC + RDK X5** 更稳。
```bash
mkdir -p ~/.ros
cat > ~/.ros/fastdds.xml <<'EOF'
<?xml version="1.0" encoding="UTF-8" ?>
<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
<transport_descriptors>
<transport_descriptor>
<transport_id>udp_transport</transport_id>
<type>UDPv4</type>
</transport_descriptor>
</transport_descriptors>
<participant profile_name="rk3506_participant" is_default_profile="true">
<rtps>
<builtin>
<initialPeersList>
<initialPeer>
<address>192.168.1.10</address>
<port>7400</port>
</initialPeer>
<initialPeer>
<address>192.168.1.20</address>
<port>7400</port>
</initialPeer>
</initialPeersList>
<discovery_config>
<discoveryProtocol>SIMPLE</discoveryProtocol>
<discoveryStrategy>STATIC</discoveryStrategy>
<leaseDuration>30</leaseDuration>
</discovery_config>
<metatrafficUnicastLocatorList>
<locator>
<udpv4>
<address>0.0.0.0</address>
<port>7400</port>
</udpv4>
</locator>
</metatrafficUnicastLocatorList>
</builtin>
</rtps>
</participant>
</profiles>
EOF
# 加进 .bashrc
cat >> ~/.bashrc <<'EOF'
export ROS_DOMAIN_ID=0
export ROS_LOCALHOST_ONLY=0
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
export FASTRTPS_DEFAULT_PROFILES_FILE=~/.ros/fastdds.xml
EOF
source ~/.bashrc
```
> 💡 `discoveryStrategy=STATIC` 让 RK3506 只找列表里的两个 IP,**不会到处广播**,省网络+省电。
### 4.6 把本仓库拷到 RK3506 + build
```bash
# 在 PC 上(假设在 D:\xs\ros2)
scp -r D:\xs\ros2 user@192.168.1.31:~/ros2_ws
# SSH 到 RK3506
ssh user@192.168.1.31
cd ~/ros2_ws
source /opt/ros/humble/setup.bash
# 编译(ARM,首次 5-10 分钟)
colcon build --symlink-install \
--packages-select py_pubsub cpp_pubsub cpp_robot_tf2 bringup
# 预期
Summary: 4 packages finished [6m 12s]
```
> ⚠️ **如果 RK3506 资源太紧编译失败**,只 build 1-2 个必要的包:
> ```bash
> colcon build --packages-select cpp_robot_tf2
> ```
### 4.7 RK3506 上启动 joint_state_publisher
```bash
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
export ROS_DOMAIN_ID=0
# 跑 cpp_robot_tf2 的关节发布者
ros2 run cpp_robot_tf2 joint_state_publisher
# 预期
[joint_state_publisher_cpp]: joint_state_publisher_cpp started, joints: 3
```
> 注: 这是**仿真关节**(3 关节 sin 运动),真实电机时改成读编码器(见 [第 7 章](#7-rk3506-接真实电机--编码器))。
### 4.8 PC 上验证 RK3506 节点可见
```bash
# 在 PC / Docker 内
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 node list --no-daemon"
# 应该看到:
# /joint_state_publisher_cpp ← 来自 RK3506 (192.168.1.31)
# /talker_py /listener_py ... ← 本地 PC 节点
```
**看不到时**: 看下面 [第 9 章 故障排查](#9-故障排查大全)。
---
## 5. 三机 LAN DDS 组网
### 5.1 验证 multicast 通(默认方式)
```bash
# 在 RK3506 上
sudo tcpdump -i eth0 -n udp port 7400 -c 20
# 应该看到来自 PC (192.168.1.10) 和 RDK X5 (192.168.1.20) 的 UDP 包
# Ctrl+C 停止
```
**如果 multicast 不通**: 走 [4.5 单播配置](#45-加-fastdds-单播发现配置)。
### 5.2 三机互相"看见"节点
| 操作 | PC | RDK X5 | RK3506 #1 |
|---|---|---|---|
| `ros2 node list --no-daemon` | 见 PC + RDK + RK3506 节点 | 见 PC + RDK + RK3506 节点 | 见 PC + RDK + RK3506 节点 |
**预期**: 三机每台的 `node list` 输出**完全一致**(都看到 3 台的节点)。
### 5.3 跨机 topic 频率
```bash
# 在 RK3506 上看 /chatter 频率(由 PC 的 talker 发布)
ros2 topic hz /chatter --no-daemon
# average rate: 4.000 (4Hz = 2 talker × 2Hz)
```
**延迟**: 跨机 topic echo 一般 < 5ms(LAN);> 50ms 说明网络出问题。
---
## 6. 跨机 Demo 联调
### 6.1 场景 A:PC 视觉 → RK3506 控制(简化版)
```bash
# === 在 PC (Docker) ===
docker exec ros2_dev bash -lc "cd /root/ros2_ws && source install/setup.bash && ros2 run py_vision_demo fake_camera"
# === 在 RK3506 ===
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 topic echo /image_raw --no-daemon # 看到 PC 发的图像
# === 在 PC ===
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 run py_vision_demo image_processor"
# 看到 PC 自己处理 + (如果有 RDK X5 / RK3506 也跑 image_processor,都能收到)
```
### 6.2 场景 B:RDK X5 视觉 + RK3506 电机(推荐起手)
```bash
# === 在 RDK X5 (SSH) ===
ssh user@192.168.1.20
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 run py_vision_demo fake_camera # 或你写的 YOLO 节点
# === 在 RK3506 #1 (SSH) ===
ssh user@192.168.1.31
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 run cpp_robot_tf2 joint_state_publisher
# (后续:接真实电机时改读编码器)
# === 在 PC 上监控全部 ===
docker exec ros2_dev bash -lc "ros2 node list --no-daemon"
# 看到 /fake_camera_py (RDK X5), /joint_state_publisher_cpp (RK3506)
docker exec ros2_dev bash -lc "ros2 topic hz /image_raw /joint_states --no-daemon"
# /image_raw: ~10Hz
# /joint_states: ~20Hz
```
### 6.3 场景 C:三机跑本仓库 full_demo
```bash
# === PC ===
docker exec ros2_dev bash -lc "cd /root/ros2_ws && source install/setup.bash && ros2 launch bringup full_demo_launch.py"
# 11 个 PC 节点
# === RDK X5 ===
ssh user@192.168.1.20
ros2 run py_vision_demo fake_camera # 1 节点
# === RK3506 #1 ===
ssh user@192.168.1.31
ros2 run cpp_robot_tf2 joint_state_publisher # 1 节点
# === RK3506 #2 ===
ssh user@192.168.1.32
ros2 run cpp_robot_tf2 tf2_listener # 1 节点
# === 在任一设备看 ===
ros2 node list --no-daemon
# 看到 PC 的 11 + RDK X5 的 1 + RK3506 的 2 = 14 节点!
```
---
## 7. RK3506 接真实电机 / 编码器
### 7.1 硬件接线(典型 6-DoF 机械臂)
```
总控舵机 (例如 LewanSoul/Hiwonder LX-16):
- 1 根总线(Bus Servo): 6 个舵机串联,数据线 Tx/Rx 共用
- 电源 7-12V(单独供电,不要从 RK3506 取)
- 接线到 RK3506 串口(UART): TXD/RXD/GND
编码器(可选,闭环舵机内部已有):
- SPI 接口(Magnetic encoder AS5048A)
- 或通过总线舵机协议反馈
RK3506:
- /dev/ttyS0 / /dev/ttyS1 (硬件串口)
- /dev/ttyUSB0 (USB-TTL 调试线)
```
### 7.2 写一个电机驱动节点(C++)
参考本仓库 `cpp_robot_tf2/src/joint_state_publisher.cpp`(模板)。新建 `motor_driver` 包:
```cpp
// ~/ros2_ws/src/motor_driver/src/motor_driver.cpp
#include <chrono>
#include <cstdio>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
class MotorDriver : public rclcpp::Node {
public:
MotorDriver() : rclcpp::Node("motor_driver") {
pub_ = this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);
// 打开串口(假设 /dev/ttyS0 是总线舵机)
fd_ = open("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY);
if (fd_ < 0) {
RCLCPP_FATAL(this->get_logger(), "Cannot open /dev/ttyS0");
return;
}
struct termios opts;
tcgetattr(fd_, &opts);
cfsetispeed(&opts, B115200);
cfsetospeed(&opts, B115200);
opts.c_cflag |= (CLOCAL | CREAD);
opts.c_cflag &= ~PARENB;
opts.c_cflag &= ~CSTOPB;
opts.c_cflag &= ~CSIZE;
opts.c_cflag |= CS8;
tcsetattr(fd_, TCSANOW, &opts);
timer_ = this->create_wall_timer(50ms, std::bind(&MotorDriver::tick, this));
// 订阅 cmd
sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
"/cmd_joint_states", 10,
[this](const sensor_msgs::msg::JointState::SharedPtr msg) {
// 发送舵机命令(总线舵机协议)
send_servo_cmd(msg);
});
}
private:
void tick() {
// 1) 读舵机当前角度(总线舵机协议 INSTRUCTION 0x02 read position)
// 2) 构造 JointState 消息
auto msg = sensor_msgs::msg::JointState();
msg.header.stamp = this->now();
msg.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"};
msg.position.resize(6);
// 读 6 个舵机当前角(伪代码)
for (int i = 0; i < 6; i++) msg.position[i] = read_servo_angle(i);
pub_->publish(msg);
}
void send_servo_cmd(const sensor_msgs::msg::JointState::SharedPtr& msg) {
// 总线舵机协议: 0x55 0x55 len id cmd param...
// 这里写 6 个舵机的目标角度
for (size_t i = 0; i < msg->position.size() && i < 6; i++) {
uint8_t id = i + 1; // 舵机 ID 1-6
uint16_t angle = static_cast<uint16_t>((msg->position[i] + 3.14159) / 6.28318 * 1000);
uint8_t buf[8] = {0x55, 0x55, 0x08, id, 0x03, 0x1E, (uint8_t)(angle & 0xFF), (uint8_t)(angle >> 8)};
write(fd_, buf, 8);
}
}
double read_servo_angle(int id) {
// 实际读位置:发送读指令 → 等待响应 → 解析
return 0.0; // 简化
}
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr sub_;
rclcpp::TimerBase::SharedPtr timer_;
int fd_ = -1;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MotorDriver>());
rclcpp::shutdown();
return 0;
}
```
### 7.3 上电测试(务必小心!)
```
1. 舵机先**单独供电**,不上机械臂(用编程器/数据线模式调零)
2. RK3506 接 USB-TTL 调试线,登录 ssh
3. 启动 motor_driver 节点(默认发布 0° 关节角)
4. 手动旋转舵机,看 /joint_states 是否变化
5. 测试 `/cmd_joint_states` 订阅(在 PC 上 ros2 topic pub)
ros2 topic pub /cmd_joint_states sensor_msgs/msg/JointState "{name: ['j1'], position: [1.57]}"
6. 确认舵机收到命令并转动
7. 装上机械臂,运行 MoveIt2 demo
```
---
## 8. ros2_control 闭环
### 8.1 ros2_control 是什么
`ros2_control` 是电机驱动的**抽象层**。你写一个 `HardwareInterface`,RK3506 的真驱动接进去,
高层节点(PC 的 MoveIt2)只看到标准接口(`position_cmd` / `position_state`),不关心底层是舵机 / 步进电机 / 谐波减速。
```
┌──────────────┐ ┌──────────────────────────┐ ┌─────────────┐
│ MoveIt2 │ ──→ │ ros2_control │ ──→ │ 真电机驱动 │
│ /joint_ │ std │ ControllerManager │ │ (Bus Servo)│
│ trajectory │ if │ (JointTrajectoryController)│ │ │
└──────────────┘ └──────────────────────────┘ └─────────────┘
```
### 8.2 在 RK3506 上跑 controller_manager
```bash
sudo apt install ros-humble-ros2-control ros-humble-ros2-controllers -y
```
写一个 `motor_hw_interface` 包(本仓库后续可加),声明 6 个关节的 hardware interface。
### 8.3 PC 上跑 MoveIt2(规划)
```bash
sudo apt install ros-humble-moveit -y
# 启动 MoveIt(配 6-DoF SRDF)
ros2 launch moveit2_tutorials demo.launch.py
```
PC 算轨迹 → 通过 `FollowJointTrajectory` Action 发给 RK3506 → RK3506 controller_manager 执行。
---
## 9. 故障排查大全
### 9.1 看不到其他机器的节点
```bash
# 步骤 1: 网络通?
ping -c 3 192.168.1.31
# 步骤 2: 端口 7400 通?
nc -zv 192.168.1.31 7400
# 步骤 3: multicast 通?
tcpdump -i eth0 -n udp port 7400 -c 5
# 步骤 4: ROS_DOMAIN_ID 一致?
echo $ROS_DOMAIN_ID # 三机都要一样
```
### 9.2 multicast 不通
切单播 (4.5 节):
```bash
export ROS_STATIC_PEERS="192.168.1.10:7400;192.168.1.20:7400"
```
### 9.3 RK3506 内存不足
```bash
free -h
# 看哪些进程吃内存
ps aux --sort=-%mem | head -10
# 杀掉
pkill -9 -f ros2_daemon
pkill -9 -f ros2 # 注意:会杀掉所有 ROS2 进程,先确认其他节点不跑
```
### 9.4 编译失败
```bash
# 列报错包名,查 apt 是否装齐
dpkg -l | grep ros-humble-rclcpp
# 装缺失依赖
sudo rosdep init && rosdep update
sudo rosdep install -i --from-paths src/
# 清理重 build
cd ~/ros2_ws && rm -rf build install log
colcon build --symlink-install
```
### 9.5 节点启动后立刻死
```bash
ros2 run <pkg> <node> --log-level debug
# 看具体 traceback
```
常见原因:
- 参数缺失(`use_sim_time` 未设)
- 缺少文件(URDF 路径错)
- 网络端口冲突
---
## 10. 性能调优 Checklist
| 项 | 目标 | 命令 |
|---|---|---|
| RAM 占用 | < 350MB | `free -h` |
| CPU 空闲 | > 60% | `top -bn1 \| head -20` |
| TF lookup 延迟 | < 5ms | `ros2 topic delay /tf --no-daemon` |
| DDS heartbeat | < 100ms | `ros2 daemon stop; ros2 --no-daemon ...` |
| 电机响应延迟 | < 50ms | 录 topic 时间戳,看从 PC 发出到 RK3506 转动 |
---
## 附录 A: 采购清单
| 项 | 数量 | 备注 |
|---|---|---|
| RDK X5 | 1 | 地瓜机器人官方 |
| RK3506 开发板 | 2 | 注意要 aarch64 Linux 镜像 |
| 6-DoF 机械臂套件 | 1 | 推荐 LewanSoul/Hiwonder 铝架 |
| 总线舵机(6 个) | 1 套 | 型号 LX-16 / LX-224 |
| 舵机电源(7-12V 5A+) | 1 | **必须独立供电** |
| USB-TTL 串口线 | 2 | RK3506 debug 用 |
| 网线 + 路由器 | 1 套 | 三机同网段 |
| 急停按钮 | 1 | 接 RK3506 GPIO |
---
## 附录 B: 关键命令速查
```bash
# 三机通用
source /opt/ros/humble/setup.bash
ros2 node list --no-daemon
ros2 topic list --no-daemon
ros2 topic hz /topic --no-daemon
ros2 topic echo /topic --no-daemon
# RK3506 内存
free -h
ps aux --sort=-%mem | head
# 网络
ip addr show
ping -c 3 <ip>
nc -zv <ip> 7400
# DDS 配置
cat ~/.ros/fastdds.xml
echo $ROS_STATIC_PEERS
# colcon build
cd ~/ros2_ws && colcon build --packages-select <pkg>
cd ~/ros2_ws && rm -rf build install log && colcon build
# ros2_control
ros2 control list_controllers
ros2 control list_hardware_interfaces
# MoveIt2
ros2 launch moveit2_tutorials demo.launch.py
```
---
## 下一步建议
读完本篇你能做到:
✅ 三机 ROS2 互通
✅ RK3506 接真实电机
✅ PC + RDK X5 + RK3506 完整工作流
**接着学习**:
- ros2_control 完整配置(`doc/99-embodied-ai.md` 路径)
- MoveIt2 SRDF 生成 + 路径规划
- Gazebo 物理仿真验证(在 PC 上)
- NPU 推理在 RDK X5 / RK3506 #2 上跑(YOLO-Lite)
如需在本仓库加 `ros2_control_demo` / `motor_driver` 包,后续 PR 即可。
doc/20-topics.md
+635
View File
@@ -0,0 +1,635 @@
# 20 · Topic 深度:pub/sub(完全指南)
> **目标**:吃透 ROS2 pub/sub,涵盖消息定义、QoS、跨语言互通、常见坑,学完直接写工业级代码。
---
## 目录
- [1. 通信模型](#1-通信模型)
- [2. 消息类型](#2-消息类型)
- [3. Publisher API(Python / C++)](#3-publisher-apipython--c)
- [4. Subscriber API(Python / C++)](#4-subscriber-apipython--c)
- [5. QoS 详解(必备)](#5-qos-详解必备)
- [6. 跨语言互通(核心特性)](#6-跨语言互通核心特性)
- [7. 完整实战:写一个传感器数据流](#7-完整实战写一个传感器数据流)
- [8. 调试命令大全](#8-调试命令大全)
- [9. 常见坑 + 解决方案](#9-常见坑--解决方案)
- [10. 在本仓库里跑](#10-在本仓库里跑)
- [11. 进阶:可靠通信 / 录制 / 跨机](#11-进阶可靠通信--录制--跨机)
---
## 1. 通信模型
### 1.1 一句话
Topic 是 **异步、多对多、单向**的发布订阅通道。
```
Publisher ──publish()──> Topic (chatter) ──callback(msg)──> Subscriber
────────────────────────────────────────────────────
异步,非阻塞 多对多,单向 事件循环触发
```
### 1.2 三种通信对比
| 维度 | Topic | Service | Action |
|---|---|---|---|
| 同步 | **异步** | 同步(阻塞) | 异步(long-running) |
| 方向 | 单向 pub→sub | 双向 req/resp | 双向 goal/fb/result |
| 一对多 | ✅ | ❌ 一对一 | ❌ 一对一 |
| 取消 | N/A | ❌ | ✅ |
| 进度反馈 | N/A | ❌ | ✅ |
| 适合 | 传感器流、状态 | 短查询 | 长任务 |
### 1.3 何时用 Topic
✅ **适合**:
- 周期性传感器数据(相机、IMU、激光雷达、关节状态)
- 状态发布(机器人位置、电池电量)
- 持续监控数据(诊断、log)
❌ **不适合**:
- 一次性 req/resp(用 Service)
- 长任务(用 Action)
- 需要反馈进度(用 Action)
---
## 2. 消息类型
### 2.1 标准消息(ROS2 自带)
| 包 | 常用消息 |
|---|---|
| `std_msgs` | `String`, `Bool`, `Int32`, `Float64`, `Header` |
| `geometry_msgs` | `Point`, `Quaternion`, `Pose`, `Twist`, `Transform`, `Vector3` |
| `sensor_msgs` | `Image`, `JointState`, `Imu`, `PointCloud2`, `CameraInfo`, `LaserScan` |
| `nav_msgs` | `Odometry`, `Path`, `OccupancyGrid` |
| `trajectory_msgs` | `JointTrajectory`, `JointTrajectoryPoint` |
### 2.2 消息结构示例
```python
# sensor_msgs/msg/Image
msg = sensor_msgs.msg.Image()
msg.header.stamp = node.get_clock().now().to_msg()
msg.header.frame_id = "camera_optical_frame"
msg.height = 480
msg.width = 640
msg.encoding = "bgr8" # OpenCV 默认
msg.is_bigendian = 0
msg.step = 640 * 3 # width * bytes_per_pixel
msg.data = bgr_array.tobytes() # numpy → bytes
```
### 2.3 自定义消息(本仓库不用)
如果需要自定义消息:
1. 在包内 `msg/MyMsg.msg` 定义字段
2. `package.xml` 加 `<build_depend>rosidl_default_generators</build_depend>` + `<exec_depend>rosidl_default_runtime</exec_depend>`
3. `CMakeLists.txt` 调 `rosidl_generate_interfaces(...)`
4. `colcon build` 后 Python/C++ 自动生成类
本仓库**只使用标准消息**,简化学习曲线。
---
## 3. Publisher API(Python / C++)
### 3.1 Python
```python
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
class Talker(Node):
def __init__(self):
super().__init__('talker_py')
# 1) 声明参数
self.declare_parameter('period_ms', 500)
self.declare_parameter('topic', 'chatter')
# 2) 取参数
period = self.get_parameter('period_ms').value
topic = self.get_parameter('topic').value
# 3) 创建 Publisher
# create_publisher(msg_type, topic, qos_depth)
self.publisher_ = self.create_publisher(String, topic, 10)
# 4) 创建定时器,周期性 publish
self.timer_ = self.create_timer(period / 1000.0, self.timer_callback)
def timer_callback(self):
msg = String()
msg.data = f'Hello, count={self.count}'
self.publisher_.publish(msg) # 异步,不等
self.count += 1
def main():
rclpy.init()
node = Talker()
try:
rclpy.spin(node) # 进入事件循环
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
```
### 3.2 C++
```cpp
#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using namespace std::chrono_literals;
class Talker : public rclcpp::Node {
public:
Talker() : rclcpp::Node("talker_cpp"), count_(0) {
// 1) 声明 + 读参数
this->declare_parameter<int>("period_ms", 500);
this->declare_parameter<std::string>("topic", "chatter");
int period = this->get_parameter("period_ms").as_int();
std::string topic = this->get_parameter("topic").as_string();
// 2) 创建 Publisher
publisher_ = this->create_publisher<std_msgs::msg::String>(topic, 10);
// 3) 定时器
timer_ = this->create_wall_timer(
std::chrono::milliseconds(period),
std::bind(&Talker::timer_callback, this));
}
private:
void timer_callback() {
auto msg = std_msgs::msg::String();
msg.data = "Hello from C++, seq=" + std::to_string(count_++);
publisher_->publish(msg);
}
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
size_t count_;
};
int main(int argc, char * argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Talker>());
rclcpp::shutdown();
return 0;
}
```
### 3.3 关键 API 速查
| Python | C++ | 用途 |
|---|---|---|
| `create_publisher(MsgType, name, depth)` | `create_publisher<MsgType>(name, depth)` | 创建 publisher |
| `publisher.publish(msg)` | `publisher->publish(msg)` | 异步发消息 |
| `publisher.get_subscription_count()` | 同 | 看有几个订阅者 |
| `destroy_publisher()` | 同 | 销毁 |
---
## 4. Subscriber API(Python / C++)
### 4.1 Python
```python
class Listener(Node):
def __init__(self):
super().__init__('listener_py')
self.declare_parameter('topic', 'chatter')
topic = self.get_parameter('topic').value
# create_subscription(msg_type, topic, callback, qos_depth)
# callback 签名: callback(msg)
self.subscription = self.create_subscription(
String, topic, self.listener_callback, 10)
def listener_callback(self, msg):
self.get_logger().info(f'recv: "{msg.data}"')
# 这里做处理:解析、入队、下发指令、可视化等
```
### 4.2 C++
```cpp
class Listener : public rclcpp::Node {
public:
Listener() : rclcpp::Node("listener_cpp") {
this->declare_parameter<std::string>("topic", "chatter");
std::string topic = this->get_parameter("topic").as_string();
// create_subscription<T>(topic, depth, callback)
// callback 签名: [](const T::SharedPtr msg) { ... }
subscription_ = this->create_subscription<std_msgs::msg::String>(
topic, 10,
[this](const std_msgs::msg::String::SharedPtr msg) {
RCLCPP_INFO(this->get_logger(), "recv: \"%s\"", msg->data.c_str());
});
}
private:
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};
```
### 4.3 关键 API 速查
| Python | C++ | 用途 |
|---|---|---|
| `create_subscription(MsgType, name, cb, depth)` | `create_subscription<T>(name, depth, cb)` | 创建订阅者 |
| callback 签名: `cb(msg)` | `[](const T::SharedPtr msg) { ... }` | 消息回调 |
### 4.4 重要原则
- **回调里别阻塞**:回调在主线程里跑,长时间阻塞会让 timer / 其他 callback 卡死
- **回调里别抛异常**:rclcpp 会捕获但仍可能崩
- **复杂处理入队**:把消息放到 queue,另起线程消费
---
## 5. QoS 详解(必备)
### 5.1 五个维度
| 维度 | 取值 | 默认 | 含义 |
|---|---|---|---|
| **Reliability** | RELIABLE / BEST_EFFORT | RELIABLE | 必须投递 / 丢一帧无所谓 |
| **History** | KEEP_LAST(N) / KEEP_ALL | KEEP_LAST(10) | 队列策略 |
| **Durability** | VOLATILE / TRANSIENT_LOCAL | VOLATILE | 晚订阅者是否收旧数据 |
| **Deadline** | Duration | ∞ | 最长多久发一次 |
| **Lifespan** | Duration | ∞ | 多旧的消息失效 |
### 5.2 常用组合
```python
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
# 视频流:丢一帧没关系,要最新
sensor_qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1
)
# 控制指令:必须到达
control_qos = QoSProfile(
reliability=ReliabilityPolicy.RELIABLE,
history=HistoryPolicy.KEEP_LAST,
depth=10
)
```
### 5.3 兼容规则(关键!)
| Publisher | Subscriber | 结果 |
|---|---|---|
| RELIABLE | RELIABLE | ✅ |
| BEST_EFFORT | BEST_EFFORT | ✅ |
| RELIABLE | BEST_EFFORT | ❌ (sub 不发 ACK,pub 报 QoS incompatible) |
| BEST_EFFORT | RELIABLE | ✅ (sub 容忍丢) |
**报错样例**:
```
[WARN] ... New subscription discovered on this topic with incompatible QoS ...
```
**怎么查**: `ros2 topic info /topic -v` 看 pub/sub 各自 QoS。
### 5.4 本仓库 QoS 策略
所有 demo **都用默认 QoS**(RELIABLE + KEEP_LAST(10))。
- 优点:跨语言互通零障碍
- 缺点:高频场景需调优
**实战**: 视频流改 `BEST_EFFORT + depth=1`;关节控制用 `RELIABLE + depth=1`。
---
## 6. 跨语言互通(核心特性)
### 6.1 为什么能互通
ROS2 用 **DDS** 做底层,Python / C++ / 其他语言只是同一消息的不同"视图"。
**消息定义在 .msg/.srv/.action 里,所有语言按这个定义自动生成代码**。
### 6.2 互通条件
1. ✅ 消息类型一致(`std_msgs/String` 的 Python/C++ 字段名都是 `data`)
2. ✅ Topic 名一致
3. ✅ QoS 兼容
4. ✅ ROS_DOMAIN_ID 一致(`ROS_DOMAIN_ID` 环境变量)
### 6.3 验证互通
```bash
# 启 4 节点(pubsub_launch.py)
ros2 launch bringup pubsub_launch.py
# 看 /chatter 的 pub/sub 列表
ros2 topic info /chatter -v
```
**预期**:
```
Publication count: 2
Subscription count: 2
Node name: listener_py Node namespace: /
Publisher count: 0
Node name: talker_py Node namespace: /
Publisher count: 1
Node name: listener_cpp Node namespace: /
Publisher count: 0
Node name: talker_cpp Node namespace: /
Publisher count: 1
```
看到 **talker_py + talker_cpp** 两个 publisher, **listener_py + listener_cpp** 两个 subscriber。
### 6.4 看跨语言消息流
```bash
# 终端 1
ros2 launch bringup pubsub_launch.py
# 终端 2
ros2 topic echo /chatter
# 预期:
# data: 'Hello from PY, seq=42'
# data: 'Hello from C++, seq=12'
# data: 'Hello from PY, seq=43'
# data: 'Hello from C++, seq=13'
```
**Python talker 发** → C++ listener 收到 ✅
**C++ talker 发** → Python listener 收到 ✅
端到端日志: [`docker/bringup_e2e.log`](../docker/bringup_e2e.log)
---
## 7. 完整实战:写一个传感器数据流
假设你做一个激光雷达节点:
### 7.1 Publisher(传感器端)
```python
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
import random
class FakeLidar(Node):
def __init__(self):
super().__init__('fake_lidar')
self.declare_parameter('rate_hz', 10)
rate = self.get_parameter('rate_hz').value
self.pub_ = self.create_publisher(
LaserScan, '/scan', 10)
self.timer_ = self.create_timer(1.0/rate, self.tick)
def tick(self):
scan = LaserScan()
scan.header.stamp = self.get_clock().now().to_msg()
scan.header.frame_id = 'laser_frame'
scan.angle_min = -3.14159
scan.angle_max = 3.14159
scan.angle_increment = 0.01
scan.time_increment = 0.0
scan.range_min = 0.05
scan.range_max = 30.0
scan.ranges = [random.uniform(0.5, 5.0) for _ in range(629)]
self.pub_.publish(scan)
```
### 7.2 Subscriber(消费端)
```python
class LidarProcessor(Node):
def __init__(self):
super().__init__('lidar_processor')
self.sub_ = self.create_subscription(
LaserScan, '/scan', self.cb, 10)
def cb(self, msg):
# 找最近障碍
nearest = min(msg.ranges)
self.get_logger().info(f'nearest obstacle: {nearest:.2f}m')
# 这里可以做:避障规划、点云处理、可视化
```
### 7.3 完整 run
```bash
# 终端 1:传感器
ros2 run my_pkg fake_lidar
# 终端 2:处理
ros2 run my_pkg lidar_processor
# 终端 3:验证
ros2 topic hz /scan # 10 Hz
ros2 topic echo /scan # 看数据(不要全打印,会很乱)
```
---
## 8. 调试命令大全
```bash
# 列所有 topic
ros2 topic list
# 看 topic 元数据(类型 + pub/sub + QoS)
ros2 topic info /chatter -v
# 实时打印消息内容
ros2 topic echo /chatter
# 测频率(平均 / 最小 / 最大 / 标准差)
ros2 topic hz /chatter
# 测带宽(每秒多少 KB)
ros2 topic bw /chatter
# 发一条测试消息
ros2 topic pub /chatter std_msgs/String "{data: 'hello'}" --once
# 录包
ros2 bag record /chatter -o my_bag
# 录包回放
ros2 bag play my_bag
# 看当前所有节点
ros2 node list
# 看节点发布的 topic
ros2 node info /talker_py
```
---
## 9. 常见坑 + 解决方案
### 9.1 收不到消息(最常见)
**症状**: `ros2 topic echo` 没输出,但 `ros2 topic list` 看得到。
**排查步骤**:
```bash
# 1) 看 pub/sub 列表
ros2 topic info /topic -v
# 2) 看 QoS 是否兼容
# 如果 QoS incompatible,WARN 日志会打印
# 3) 看节点是否活着
ros2 node list
```
**常见原因**:
- ❌ **节点名重复**: `Node 'x' already exists` → 改名或加 namespace
- ❌ **Topic 名拼写错误**:大小写、`/` 区分
- ❌ **QoS 不兼容**:改成 default 或两边匹配
- ❌ **消息类型不匹配**:Publisher 是 `std_msgs/String`,Subscriber 是 `std_msgs/Int32` → 静默不匹配
- ❌ **Publisher 还没起来**:等 1-2s DDS discovery
### 9.2 Docker 内 localhost 不互通
**症状**: 两个容器里节点互相看不到。
**解决**: 用 `network_mode: host`(本仓库已配),或 `ROS_STATIC_PEERS` 单播。
### 9.3 DDS QoS 兼容性错
**报错**:
```
[WARN] ... Incompatible QoS ... (PolicyKind=RMW_QOS_POLICY_RELIABILITY)
```
**解决**: 双方 Reliability 一致。
```python
from rclpy.qos import QoSProfile, ReliabilityPolicy
qos = QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, depth=1)
self.pub_ = self.create_publisher(String, 'topic', qos)
```
### 9.4 Cyclone DDS 没装导致 build 失败
CMake 错误:
```
Could not find ROS middleware implementation 'rmw_cyclonedds_cpp'
```
**解决**:
- 不要设 `RMW_IMPLEMENTATION=rmw_cyclonedds_cpp`(本仓库默认 fastdds)
- 切 RMW 时务必 `rm -rf build/ install/ log/`
### 9.5 Callback 阻塞导致节点"卡死"
**症状**: 节点启动后什么都不做,其他 timer / callback 也不响应。
**原因**: callback 里跑同步阻塞代码(long I/O、time.sleep)。
**解决**: 用 `MultiThreadedExecutor`,callback 里只入队 + 另起线程处理。
```python
from rclpy.executors import MultiThreadedExecutor
exec_ = MultiThreadedExecutor(num_threads=4)
exec_.add_node(node)
exec_.spin()
```
---
## 10. 在本仓库里跑
### 10.1 启动 4 节点跨语言 demo
```bash
docker exec ros2_dev bash -lc "cd /root/ros2_ws && source install/setup.bash && ros2 launch bringup pubsub_launch.py"
```
### 10.2 源码位置
- Python pub: [`src/py_pubsub/py_pubsub/publisher_member_function.py`](../src/py_pubsub/py_pubsub/publisher_member_function.py)
- Python sub: [`src/py_pubsub/py_pubsub/subscriber_member_function.py`](../src/py_pubsub/py_pubsub/subscriber_member_function.py)
- C++ pub: [`src/cpp_pubsub/src/publisher_member_function.cpp`](../src/cpp_pubsub/src/publisher_member_function.cpp)
- C++ sub: [`src/cpp_pubsub/src/subscriber_member_function.cpp`](../src/cpp_pubsub/src/subscriber_member_function.cpp)
- launch: [`src/bringup/launch/pubsub_launch.py`](../src/bringup/launch/pubsub_launch.py)
### 10.3 端到端日志
[`docker/bringup_e2e.log`](../docker/bringup_e2e.log) — 跨语言互通验证。
---
## 11. 进阶:可靠通信 / 录制 / 跨机
### 11.1 Reliable 通信设置
```python
# 默认就是 RELIABLE,显式写:
from rclpy.qos import QoSProfile, ReliabilityPolicy
qos = QoSProfile(
reliability=ReliabilityPolicy.RELIABLE,
history=HistoryPolicy.KEEP_LAST,
depth=10
)
```
### 11.2 录制 + 回放(bag)
```bash
ros2 bag record -o my_bag /chatter /tf /joint_states
# 输出: my_bag_0.db3 + metadata.yaml
ros2 bag play my_bag --loop # --loop 循环回放
ros2 bag info my_bag # 看消息数 / 时间 / 类型
```
**注意**: 回放时,topic 真实 pub 也要在,否则 recorder 找不到对应 publisher。bag 不会保存节点,只保存消息。
### 11.3 跨机 DDS
默认走 UDP multicast,同网段自动发现。
**跨子网**: 用 unicast discovery。
PC:
```bash
export ROS_DISCOVERY_SERVER=192.168.1.20:11811
```
RDK X5: 启 discovery server:
```bash
ros2 run discovery_server discovery_server --address 0.0.0.0 --port 11811
```
**跨 LAN + 防火墙**: 用 `ROS_STATIC_PEERS` 静态发现。
详见 [`doc/100-embedded-deployment.md`](100-embedded-deployment.md) §5。
---
## 接下来读
| 主题 | 文档 |
|---|---|
| Service 深度 | [`30-services.md`](30-services.md) |
| Action 深度 | [`40-actions.md`](40-actions.md) |
| TF2 坐标变换 | [`50-tf2.md`](50-tf2.md) |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
| 具身智能路径 | [`99-embodied-ai.md`](99-embodied-ai.md) |
doc/30-services.md
+510
View File
@@ -0,0 +1,510 @@
# 30 · Service 深度:同步 req/resp(完全指南)
> **目标**:吃透 ROS2 Service,涵盖 .srv 定义、Server/Client API、Future、wait_for_service、调试,学完能写工业级 Service。
---
## 目录
- [1. 通信模型](#1-通信模型)
- [2. .srv 文件定义](#2-srv-文件定义)
- [3. Server 端(Python / C++)](#3-server-端python--c)
- [4. Client 端(Python / C++)](#4-client-端python--c)
- [5. 同步 vs 异步调用](#5-同步-vs-异步调用)
- [6. wait_for_service 详解](#6-wait_for_service-详解)
- [7. QoS + Service 名约定](#7-qos--service-名约定)
- [8. 实战:写一个拍照 + 计算服务](#8-实战写一个拍照--计算服务)
- [9. 调试命令](#9-调试命令)
- [10. 常见坑](#10-常见坑)
- [11. 在本仓库里跑](#11-在本仓库里跑)
- [12. 进阶:异步服务端 + 回调式 client](#12-进阶异步服务端--回调式-client)
---
## 1. 通信模型
### 1.1 一句话
Service 是 **同步、一对一、双向**的请求/响应。
- 一次性调用 + 等结果
- 几毫秒到几秒
```
Client ──call(req)──> Service Server
◀──response────
─────────────────
同步(阻塞),一次一答
```
### 1.2 vs Topic / Action
| 维度 | Service | Topic | Action |
|---|---|---|---|
| 同步性 | **同步(等响应)** | 异步 | 异步(long) |
| 方向 | **双向** req/resp | 单向 pub→sub | 双向 goal/fb/result |
| 1对多 | ❌ 一对一 | ✅ | ❌ |
| 反馈进度 | ❌ | ❌ | ✅ |
| 可取消 | ❌ | N/A | ✅ |
### 1.3 何时用
✅ **适合**:
- 拍照(几 ms)
- 关节角度查询
- 短计算(几十 ms)
- 开关 / 触发动作
❌ **不适合**:
- 周期性相机帧(用 Topic)
- 长任务(用 Action)
- 需要进度(用 Action)
---
## 2. .srv 文件定义
### 2.1 格式
```
Request 字段
---
Response 字段
```
### 2.2 标准 .srv 示例
```srv
# example_interfaces/srv/AddTwoInts.srv
int64 a # Request
int64 b
---
int64 sum # Response
```
### 2.3 复杂示例(多个字段)
```srv
# example_interfaces/srv/SetBool.srv
bool data
---
bool success
string message
```
### 2.4 本仓库用的 Service
`example_interfaces/srv/AddTwoInts`:
- Request: `int64 a`, `int64 b`
- Response: `int64 sum`
---
## 3. Server 端(Python / C++)
### 3.1 Python
```python
import rclpy
from rclpy.node import Node
from example_interfaces.srv import AddTwoInts
class AddTwoIntsServer(Node):
def __init__(self):
super().__init__('add_two_ints_server_py')
# create_service(srv_type, srv_name, callback)
# callback 签名: callback(request, response) -> response
self.srv = self.create_service(
AddTwoInts,
'add_two_ints',
self.add_two_ints_callback,
)
self.get_logger().info('add_two_ints_server ready')
def add_two_ints_callback(self, request, response):
response.sum = request.a + request.b
self.get_logger().info(f'{request.a} + {request.b} = {response.sum}')
return response # 必须 return response 对象
def main():
rclpy.init()
node = AddTwoIntsServer()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
```
### 3.2 C++
```cpp
#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
class Server : public rclcpp::Node {
public:
Server() : rclcpp::Node("add_two_ints_server_cpp") {
srv_ = this->create_service<example_interfaces::srv::AddTwoInts>(
"add_two_ints",
[this](
const example_interfaces::srv::AddTwoInts::Request::SharedPtr req,
example_interfaces::srv::AddTwoInts::Response::SharedPtr res
) {
res->sum = req->a + req->b;
});
}
private:
rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr srv_;
};
```
---
## 4. Client 端(Python / C++)
### 4.1 Python — 同步阻塞风格
```python
class Client(Node):
def __init__(self):
super().__init__('add_two_ints_client_py')
self.client = self.create_client(AddTwoInts, 'add_two_ints')
# 阻塞等服务端上线(1s 超时,循环等)
while not self.client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('waiting for service...')
def call_sync(self, a, b):
req = AddTwoInts.Request()
req.a = a; req.b = b
# call_async:返回 Future,不等
future = self.client.call_async(req)
# spin_until_future_complete:阻塞到 future 完成(或超时)
rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
if future.result() is None:
self.get_logger().error('service call failed')
return None
return future.result().sum
```
### 4.2 Python — 异步回调风格
```python
class AsyncClient(Node):
def __init__(self):
super().__init__('async_client')
self.client = self.create_client(AddTwoInts, 'add_two_ints')
self.client.wait_for_service()
def call_async(self, a, b):
req = AddTwoInts.Request()
req.a = a; req.b = b
future = self.client.call_async(req)
def done_cb(fut):
result = fut.result()
if result is not None:
self.get_logger().info(f'result: {result.sum}')
else:
self.get_logger().warn('failed')
future.add_done_callback(done_cb)
def main():
rclpy.init()
node = AsyncClient()
node.call_async(12, 30)
rclpy.spin(node) # 阻塞,直到 callback 调 shutdown
```
### 4.3 C++
```cpp
class Client : public rclcpp::Node {
public:
Client() : rclcpp::Node("client") {
client_ = this->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
while (!client_->wait_for_service(std::chrono::seconds(1))) {
RCLCPP_INFO(this->get_logger(), "waiting...");
}
}
int64_t call(int64_t a, int64_t b) {
auto req = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
req->a = a; req->b = b;
auto future = client_->async_send_request(req);
if (rclcpp::spin_until_future_complete(
this->shared_from_this(), future, 5s) == rclcpp::FutureReturnCode::SUCCESS) {
return future.get()->sum;
}
return -1;
}
};
```
### 4.4 关键 API 速查
| Python | C++ |
|---|---|
| `create_client(SrvType, name)` | `create_client<T>(name)` |
| `client.wait_for_service(timeout_sec=N)` | `client->wait_for_service(1s)` |
| `client.call_async(req)` | `client->async_send_request(req)` |
| `future.result()` | `future.get()` |
| `rclpy.spin_until_future_complete(node, future, timeout_sec)` | `rclcpp::spin_until_future_complete(this, future, 5s)` |
| `future.add_done_callback(cb)` | (用 `bind`) |
---
## 5. 同步 vs 异步调用
| 方式 | 适用 | 阻塞? |
|---|---|---|
| `call(req)` | ROS1 风格,**Python 已 deprecated** | ✅ 同步 |
| `call_async + spin_until_future_complete` | 命令行 / 单测 / 测试 | ✅ 同步但 yield |
| `call_async + add_done_callback` | 生产节点,主线程不能停 | ❌ 异步 |
**生产推荐**: `add_done_callback` 风格,主线程继续 spin 其他东西。
---
## 6. wait_for_service 详解
### 6.1 为什么需要
ROS2 节点启动到 ROS 实际可达,**需要 1-2 秒 DDS discovery**。
client 启动时 server 可能还没起,所以要先等。
### 6.2 用法对比
```python
# ❌ 错误:永久阻塞,debug 难
client.wait_for_service()
# ⚠️ 不推荐:硬超时
client.wait_for_service(timeout_sec=5.0)
# ✅ 推荐:循环 + 日志
while not client.wait_for_service(timeout_sec=1.0):
node.get_logger().info('waiting for service...')
```
### 6.3 死锁陷阱
如果 `wait_for_service()` 在 `__init__` 里**阻塞**,主线程 spin 没跑,DDS discovery 没动 → 永远 wait 不到。
**修法**: 不要在 `__init__` 阻塞,放到独立的 `wait_for_server_ready()` 方法。
---
## 7. QoS + Service 名约定
### 7.1 Service QoS
默认 RELIABLE。一致即可,不用改。
### 7.2 命名约定
| 角色 | 推荐节点名 / service 名 |
|---|---|
| Service Server | `<verb>_server` (如 `add_two_ints_server`) |
| Service Client | `<verb>_client` (如 `add_two_ints_client`) |
| Service 名 | `<action>` (如 `add_two_ints`、`take_photo`) |
---
## 8. 实战:写一个拍照 + 计算服务
### 8.1 .srv 定义(自定义)
```srv
# my_camera/srv/TakePhoto.srv
string filename
---
bool success
int32 width
int32 height
string saved_path
```
### 8.2 Server
```python
import cv2
from my_camera.srv import TakePhoto
class TakePhotoServer(Node):
def __init__(self):
super().__init__('take_photo_server')
self.srv = self.create_service(
TakePhoto, 'take_photo', self.cb)
def cb(self, req, resp):
# 1) 从相机读 frame
cap = cv2.VideoCapture(0)
ret, frame = cap.read()
if not ret:
resp.success = False
return resp
# 2) 保存
h, w = frame.shape[:2]
path = f'/tmp/{req.filename}.jpg'
cv2.imwrite(path, frame)
resp.success = True
resp.width = w
resp.height = h
resp.saved_path = path
return resp
```
### 8.3 Client
```python
class TakePhotoClient(Node):
def take(self, filename):
req = TakePhoto.Request()
req.filename = filename
future = self.client.call_async(req)
rclpy.spin_until_future_complete(self, future)
result = future.result()
if result.success:
print(f'saved {result.saved_path} ({result.width}x{result.height})')
```
---
## 9. 调试命令
```bash
ros2 service list # 所有 service
ros2 service type /add_two_ints # 服务类型
ros2 service info /add_two_ints -v # 提供方节点
ros2 service find example_interfaces/srv/AddTwoInts # 找某类型的所有 service
ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 12, b: 30}"
```
---
## 10. 常见坑
### 10.1 Future 不 done
```python
# 错:立即读 future.result()
future = client.call_async(req)
result = future.result() # 阻塞到死
# 对:spin_until_future_complete
rclpy.spin_until_future_complete(node, future, timeout_sec=5.0)
result = future.result()
```
### 10.2 服务端 shutdown 后 client 调
```python
# 服务端 rclpy.shutdown() 后,future.result() 是 None
assert future.result() is not None, 'service unavailable'
```
### 10.3 多 client 排队
Service 是 1对1。两个 client 同时调,**第二个等第一个完成**。长任务用 Action。
### 10.4 wait_for_service 死锁
不要在 `__init__` 阻塞 wait!否则 DDS discovery 没法跑。
---
## 11. 在本仓库里跑
### 11.1 启 server
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup service_launch.py"
```
### 11.2 调 service(新终端)
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts '{\"a\": 12, \"b\": 30}'"
```
**预期输出**:
```
response:
example_interfaces.srv.AddTwoInts_Response(sum=42)
```
### 11.3 源码
- Server: [`src/py_srv/py_srv/add_two_ints_server.py`](../src/py_srv/py_srv/add_two_ints_server.py)
- Client: [`src/py_srv/py_srv/add_two_ints_client.py`](../src/py_srv/py_srv/add_two_ints_client.py)
- launch: [`src/bringup/launch/service_launch.py`](../src/bringup/launch/service_launch.py)
- 测试: [`src/py_srv/test/test_srv.py`](../src/py_srv/test/test_srv.py)
### 11.4 端到端日志
[`docker/srv_e2e.log`](../docker/srv_e2e.log)
---
## 12. 进阶:异步服务端 + 回调式 client
### 12.1 异步服务端
```python
class AsyncServer(Node):
"""长时间运行的服务,内部用回调推进,不阻塞主线程。"""
def __init__(self):
super().__init__('async_server')
self.srv = self.create_service(MySrv, 'async_srv', self.cb)
def cb(self, req, resp):
# 启动后台任务做实际工作
future = self._do_work(req)
future.add_done_callback(lambda f: self._respond(f, resp))
return resp # 先返回,后续异步填充
def _do_work(self, req):
# 用 executor.spin_until_future_complete 做异步
...
```
### 12.2 回调式 client
```python
def on_response(future):
result = future.result()
print(f'result: {result.value}')
rclpy.shutdown()
future = client.call_async(req)
future.add_done_callback(on_response)
rclpy.spin(node)
```
---
## Service vs Action 怎么选
| 维度 | Service | Action |
|---|---|---|
| 持续时间 | < 几秒 | 几秒 ~ 几小时 |
| 反馈进度 | ❌ | ✅ Feedback |
| 可取消 | ❌ | ✅ |
| 适用 | 拍照、查询 | 抓取、导航 |
本仓库 [`py_srv`](../src/py_srv/) 是 Service demo;[`py_action_demo`](../src/py_action_demo/) 是 Action demo。
---
## 接下来读
| 主题 | 文档 |
|---|---|
| Topic 深度 | [`20-topics.md`](20-topics.md) |
| Action 深度 | [`40-actions.md`](40-actions.md) |
| TF2 坐标变换 | [`50-tf2.md`](50-tf2.md) |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
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) |
doc/50-tf2.md
+517
View File
@@ -0,0 +1,517 @@
# 50 · TF2 坐标变换(完全指南)
> **目标**:吃透 TF2(坐标系管理),能做"机械臂末端在哪"、"相机看到的点在机器人哪"、"手眼标定",为 MoveIt2 / ros2_control / 抓取 打基础。
---
## 目录
- [1. TF 是什么](#1-tf-是什么)
- [2. 关键概念](#2-关键概念)
- [3. TF tree 典型结构](#3-tf-tree-典型结构)
- [4. C++ API 详解](#4-c--api-详解)
- [5. Python API 详解](#5-python-api-详解)
- [6. 静态 TF vs 动态 TF](#6-静态-tf-vs-动态-tf)
- [7. URDF + JointState → TF 工作链](#7-urdf--jointstate--tf-工作链)
- [8. 调试命令大全](#8-调试命令大全)
- [9. VLA / 机器人实战](#9-vla--机器人实战)
- [10. 在本仓库里跑](#10-在本仓库里跑)
- [11. 常见坑 + 排错](#11-常见坑--排错)
- [12. 进阶:手眼标定 + 时间同步](#12-进阶手眼标定--时间同步)
---
## 1. TF 是什么
**TF (Transform Library)** = ROS 里管理"所有坐标系之间相对位姿"的工具。
**TF2** = ROS2 的下一代实现。
机器人身上有几十个坐标系(世界、底盘、激光雷达、相机、机械臂每个关节、夹爪):
- TF2 帮你算任意两个之间的相对位姿,**实时更新**
- TF2 维护一棵 **TF tree**,节点之间路径唯一
- TF2 通过 `/tf` topic 广播,通过 `/tf_static` 静态广播
---
## 2. 关键概念
### 2.1 Frame ID
每个坐标系有个名字:
- `world`、`map`、`odom`、`base_link`、`base_footprint`
- `camera_optical_frame`、`laser_frame`
- `arm_base`、`shoulder`、`elbow`、`wrist`、`gripper`
### 2.2 Transform(变换)
```
parent_frame ──transform──> child_frame
```
包含:
- **translation**(Vector3: x, y, z) - 平移
- **rotation**(Quaternion: x, y, z, w) - 旋转
- **header.stamp**(时间戳)
### 2.3 时间
- `tf2::TimePointZero` = "最新可用"
- 也可以指定具体时间戳查历史(`lookupTransform(target, source, time)`)
- 必须保证时间戳 ≤ buffer 当前时间
### 2.4 Buffer + Listener 模式
```cpp
tf2_ros::Buffer buffer(clock);
tf2_ros::TransformListener listener(buffer, node);
// lookup
auto tf = buffer.lookupTransform(target, source, tf2::TimePointZero);
```
后台 `TransformListener` 订阅 `/tf` + `/tf_static`,自动把变换塞进 buffer。
---
## 3. TF tree 典型结构
### 3.1 移动底盘 + 机械臂
```
world ← 全局参考
└─ map ← SLAM 输出
└─ odom ← 里程计 / AMCL
└─ base_link ← 底盘中心
├─ base_footprint
├─ imu_link
├─ laser_frame ← 2D 激光
├─ camera_optical_frame ← RGB 相机
└─ arm_base ← 机械臂底座
└─ shoulder_link ← 关节 1
└─ upper_arm_link ← 关节 2
└─ elbow_link ← 关节 3
└─ forearm_link
└─ wrist_link
└─ gripper_link ← 末端
```
### 3.2 树 vs 图
TF 是**树**结构:任意两个 frame 间路径唯一。
不能有闭环(两个 link 间只能有一条路径)。
---
## 4. C++ API 详解
### 4.1 Listener + Buffer
```cpp
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
class TfListenerNode : public rclcpp::Node {
public:
TfListenerNode() : rclcpp::Node("tf_listener") {
buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_, this);
}
void lookup() {
try {
auto tf = buffer_->lookupTransform(
"base_link", // target
"gripper", // source
tf2::TimePointZero); // latest
RCLCPP_INFO(get_logger(),
"gripper in base_link: x=%.3f y=%.3f z=%.3f",
tf.transform.translation.x,
tf.transform.translation.y,
tf.transform.translation.z);
} catch (const tf2::TransformException& ex) {
RCLCPP_WARN(get_logger(), "lookup failed: %s", ex.what());
}
}
private:
std::shared_ptr<tf2_ros::Buffer> buffer_;
std::shared_ptr<tf2_ros::TransformListener> listener_;
};
```
### 4.2 几何变换(把点变换到其他系)
```cpp
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
geometry_msgs::msg::PointStamped p_in, p_out;
p_in.header.frame_id = "world";
p_in.point.x = 1.0; p_in.point.y = 2.0; p_in.point.z = 3.0;
p_out = buffer_->transform(p_in, "robot");
// p_out 在 robot 系下
```
也支持 PoseStamped、Vector3、Quaternion。
### 4.3 Broadcast(自己发布 TF)
```cpp
#include "tf2_ros/transform_broadcaster.h"
geometry_msgs::msg::TransformStamped tf;
tf.header.stamp = node->now();
tf.header.frame_id = "world";
tf.child_frame_id = "robot";
tf.transform.translation.set__data(1.0, 2.0, 3.0);
tf.transform.rotation.set__data(0, 0, 0, 1.0);
broadcaster.sendTransform(tf);
```
### 4.4 Static Broadcast(只发一次)
```cpp
#include "tf2_ros/static_transform_broadcaster.h"
// 用 sendTransform 后就永久保留,ROS 系统不删
static_broadcaster.sendTransform(tf);
```
### 4.5 关键 API 速查
| API | 用途 |
|---|---|
| `tf2_ros::Buffer(clock)` | 创建时间缓冲(默认 10s 历史) |
| `tf2_ros::TransformListener(buffer, node)` | 订阅 /tf /tf_static |
| `buffer.lookupTransform(target, source, time)` | 查变换(失败抛异常) |
| `buffer.canTransform(target, source, time, timeout)` | 预检 |
| `buffer.transform(p_in, target_frame)` | 几何变换 |
| `tf2_ros::TransformBroadcaster` | 动态广播 |
| `tf2_ros::StaticTransformBroadcaster` | 静态广播 |
---
## 5. Python API 详解
```python
from tf2_ros import Buffer, TransformListener, TransformBroadcaster, StaticTransformBroadcaster
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
class TfNode(Node):
def __init__(self):
super().__init__('tf_node_py')
self.buffer = Buffer()
self.listener = TransformListener(self.buffer, self)
self.broadcaster = TransformBroadcaster(self)
self.static_broadcaster = StaticTransformBroadcaster(self)
def lookup(self):
try:
tf = self.buffer.lookup_transform(
'base_link', 'gripper',
rclpy.time.Time(),
timeout=rclpy.duration.Duration(seconds=1.0))
print(f'gripper in base_link: {tf.transform.translation}')
except Exception as e:
self.get_logger().warn(f'lookup failed: {e}')
def broadcast(self):
tf = TransformStamped()
tf.header.stamp = self.get_clock().now().to_msg()
tf.header.frame_id = 'world'
tf.child_frame_id = 'robot'
tf.transform.translation.x = 1.0
tf.transform.translation.y = 2.0
tf.transform.translation.z = 3.0
tf.transform.rotation.w = 1.0
self.broadcaster.sendTransform(tf)
```
---
## 6. 静态 TF vs 动态 TF
### 6.1 静态 TF(不变的)
- 激光雷达装在底盘上,位置不变
- 相机装在机械臂末端,相对末端不变
- **关键**: 只发一次,ROS 永久保留(用 latched topic)
```bash
ros2 run tf2_ros static_transform_publisher \
--x 0.1 --y 0 --z 0.2 --roll 0 --pitch 0 --yaw 0 \
--frame-id base_link --child-frame-id laser_frame
```
或在 launch 里:
```python
Node(package='tf2_ros', executable='static_transform_publisher',
arguments=['--x', '0.1', '--y', '0', '--z', '0.2',
'--frame-id', 'base_link', '--child-frame-id', 'laser_frame'])
```
### 6.2 动态 TF(随时间变)
- 机械臂关节 → 末端位姿实时变
- 通过 URDF + JointState + robot_state_publisher 自动算
---
## 7. URDF + JointState → TF 工作链
```
URDF (XML) JointState (msg)
│ │
│ │ /joint_states (sensor_msgs/JointState)
│ │
▼ ▼
robot_state_publisher (ROS2 系统包)
│
│ 计算各 link 之间的变换
▼
/tf (geometry_msgs/TransformStamped)
│
▼
tf2::Buffer (订阅)
│
▼
buffer.lookupTransform(target, source, time)
│
▼
"gripper in base_link: x=..., y=..., z=..."
```
### 7.1 安装 robot_state_publisher
```bash
sudo apt install ros-humble-robot-state-publisher
```
### 7.2 启动
```bash
# 读 URDF + JointState
ros2 run robot_state_publisher robot_state_publisher \
--ros-args -p robot_description:="$(xacro arm.urdf)"
# 发布 JointState(本仓库 cpp_robot_tf2 就是这个)
ros2 run cpp_robot_tf2 joint_state_publisher
```
### 7.3 看 TF 树
```bash
ros2 run tf2_tools view_frames
# 生成 frames_<timestamp>.pdf
```
---
## 8. 调试命令大全
```bash
# 实时打印某变换
ros2 run tf2_ros tf2_echo base_link gripper
# 看 /tf topic 流量
ros2 topic hz /tf
ros2 topic info /tf_static -v
# 看完整 TF tree(生成 PDF)
ros2 run tf2_tools view_frames
# 在 RViz 看(可视化)
rviz2
# Add → TF
# 静态 TF publisher(临时)
ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 0.5 \
--frame-id world --child-frame-id robot
# 看 /tf_static 列表
ros2 topic info /tf_static -v
```
---
## 9. VLA / 机器人实战
### 9.1 VLA 抓取的工作流
```
[RGB Camera] → /camera/color/image_raw
↓
[GraspNet / 6-DoF pose estimator]
↓ publish /grasp_pose (PoseStamped)
↓
[TF 工具:把 grasp_pose 从 camera_optical_frame 转到 base_link]
↓
[MoveIt2 算轨迹 → /joint_trajectory]
↓
[ros2_control + 电机]
```
### 9.2 关键代码片段
```python
# 把图像里的目标点转到 base_link
def transform_grasp_to_base(camera_pose, buffer):
p_in = PoseStamped()
p_in.header.frame_id = 'camera_optical_frame'
p_in.pose = camera_pose
p_base = buffer.transform(p_in, 'base_link')
return p_base.pose
```
### 9.3 真实项目必备
| 任务 | TF 用法 |
|---|---|
| 视觉抓取 | camera 点 → base_link 点 |
| 手眼标定 | camera_link ↔ gripper 静态 TF |
| SLAM | map → odom → base_link 动态 TF |
| 机械臂正运动学 | lookup gripper 在 base_link 下 |
| 移动底盘导航 | base_link 在 map 下 |
| 多传感器融合 | 把所有数据投到 base_link |
---
## 10. 在本仓库里跑
### 10.1 启 3 关节机械臂 demo
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup robot_launch.py"
```
**预期日志**:
```
[joint_state_publisher_cpp]: joint_state_publisher_cpp started, joints: 3
[tf2_listener_cpp]: tf2_listener_cpp started
[robot_state_publisher]: got segment base_link
[robot_state_publisher]: got segment link1
[robot_state_publisher]: got segment link2
[robot_state_publisher]: got segment gripper
[tf2_listener_cpp]: gripper in base_link: x=0.013 y=0.004 z=0.299
[tf2_listener_cpp]: gripper in base_link: x=0.019 y=0.009 z=0.298
```
### 10.2 看 TF 树(PDF)
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && source /root/ros2_ws/install/setup.bash && ros2 run tf2_tools view_frames"
# 生成 frames_<时间>.pdf
```
### 10.3 源码
- JointState pub: [`src/cpp_robot_tf2/src/joint_state_publisher.cpp`](../src/cpp_robot_tf2/src/joint_state_publisher.cpp)
- TF 监听: [`src/cpp_robot_tf2/src/tf2_listener.cpp`](../src/cpp_robot_tf2/src/tf2_listener.cpp)
- URDF: [`src/cpp_robot_tf2/urdf/simple_arm.urdf`](../src/cpp_robot_tf2/urdf/simple_arm.urdf)
- 测试: [`src/cpp_robot_tf2/test/test_tf2_lookup.cpp`](../src/cpp_robot_tf2/test/test_tf2_lookup.cpp)
### 10.4 端到端日志
[`docker/robot_e2e.log`](../docker/robot_e2e.log)
---
## 11. 常见坑 + 排错
### 11.1 "lookup failed: frame does not exist"
**原因**:该 frame 没发布。
**排错**:
```bash
# 看有哪些 frame
ros2 run tf2_tools view_frames # 生成 PDF,看完整树
# 或
ros2 topic echo /tf | head -50 # 看具体发布的变换
```
**常见原因**:
- robot_state_publisher 没起 → 启它
- JointState 没发布 → 检查 publisher
- 静态 TF publisher 没起
### 11.2 lookup 超时
**原因**: buffer 里没该变换(还没发布)。
**排错**:
- 等 2-3 秒再查
- 加 retry:
```cpp
for (int i = 0; i < 5; i++) {
try {
auto tf = buffer.lookupTransform(target, source, tf2::TimePointZero, 1s);
break;
} catch (const tf2::TransformException& ex) {
// 等
}
}
```
### 11.3 TF 跨机器漂移
**原因**: 不同机器墙钟时间不同步。
**解决**:
- 三机装 NTP,误差 < 10ms
- 用 `use_sim_time` 统一时钟源
- 用 ROS Time API 而非 `chrono::system_clock`
### 11.4 lookupTransform 报"context is not valid"
**原因**: rclpy 已经 shutdown,在 callback 里调用。
**解决**: callback 里用 try/catch,且确保 ROS 仍 alive。
### 11.5 大 TF 树性能
**原因**: 10万 frame 时 lookup 慢。
**解决**:
- 用静态 TF 减少动态节点数
- 拆分成多棵子树
- buffer cache 调大
---
## 12. 进阶:手眼标定 + 时间同步
### 12.1 手眼标定
相机装在机械臂末端(eye-in-hand)或固定在外部(eye-to-hand),需要求:
- camera_link ↔ gripper 的静态变换(eye-in-hand)
- camera_link ↔ base_link 的静态变换(eye-to-hand)
```bash
sudo apt install ros-humble-easy-handeye
# 流程:
# 1) 启动相机 + MoveIt2 + 标定板
# 2) MoveIt2 移动机械臂到多个 pose,采集数据
# 3) easy_handeye 离线计算 camera_link → gripper TF
# 4) 写进 URDF
# 5) 重启 robot_state_publisher,验证
```
### 12.2 时间同步
ROS2 默认 `use_sim_time: false`(wall clock)。多机时:
```bash
# 装 NTP
sudo apt install chrony
sudo systemctl enable chrony
# 验证
chronyc tracking
```
---
## 接下来读
| 主题 | 文档 |
|---|---|
| URDF 模型 | [`60-urdf.md`](60-urdf.md) |
| ros2_control | 在 [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 2 |
| MoveIt2 | 在 [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 2 |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
| VLA 应用 | [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 5 |
doc/60-urdf.md
+711
View File
@@ -0,0 +1,711 @@
# 60 · URDF 机器人模型(完全指南)
> **目标**:能读懂 URDF,能从零写一个 6-DoF 机械臂 URDF,理解 link / joint / xacro / 物理参数,会校验 + 可视化。
---
## 目录
- [1. URDF 是什么](#1-urdf-是什么)
- [2. URDF 三大元素](#2-urdf-三大元素)
- [3. 完整 6-DoF 机械臂示例](#3-完整-6-dof-机械臂示例)
- [4. xacro:模板化 URDF](#4-xacro模板化-urdf)
- [5. 坐标约定 REP-103 / REP-105](#5-坐标约定-rep-103--rep-105)
- [6. ros2_control 集成(加 transmission)](#6-ros2_control-集成加-transmission)
- [7. Gazebo 集成(加 gazebo 标签)](#7-gazebo-集成加-gazebo-标签)
- [8. 校验 + 可视化](#8-校验--可视化)
- [9. 工具链总结](#9-工具链总结)
- [10. MoveIt2 SRDF(规划组)](#10-moveit2-srdf规划组)
- [11. 在本仓库里跑](#11-在本仓库里跑)
- [12. 进阶:6-DoF 真机械臂 URDF 实战](#12-进阶6-dof-真机械臂-urdf-实战)
---
## 1. URDF 是什么
**URDF** (Unified Robot Description Format) = 用 XML 描述机器人:
- 有哪些**刚体段**(`<link>`)
- 刚体之间怎么连(`<joint>`)
- 视觉 / 碰撞几何(`<visual>` / `<collision>`)
- 物理参数(质量、惯性,`<inertial>`)
URDF 通过 `robot_state_publisher` 翻译成 TF tree,被 MoveIt2 / Gazebo / RViz 读取。
---
## 2. URDF 三大元素
### 2.1 `<link>` — 刚体段
| 子标签 | 用途 | 必填 |
|---|---|---|
| `<visual>` | 视觉几何(RViz / Gazebo 显示) | 推荐 |
| `<collision>` | 碰撞几何(物理仿真) | 推荐 |
| `<inertial>` | 质量 + 转动惯量(动力学仿真必填) | 仿真必填 |
| `<material>` | 视觉颜色 | 否 |
```xml
<link name="base_link">
<visual>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<geometry><box size="0.1 0.1 0.1"/></geometry>
<material name="gray">
<color rgba="0.5 0.5 0.5 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.05"/>
<geometry><box size="0.1 0.1 0.1"/></geometry>
</collision>
<inertial>
<mass value="1.0"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
</link>
```
#### 几何类型
```xml
<geometry>
<box size="w h d"/> <!-- 长方体 -->
<sphere radius="r"/> <!-- 球 -->
<cylinder radius="r" length="L"/> <!-- 圆柱 -->
<mesh filename="package://my_pkg/meshes/arm.stl" scale="0.001 0.001 0.001"/>
</geometry>
```
#### 颜色 / 材质
```xml
<material name="gray">
<color rgba="0.5 0.5 0.5 1.0"/> <!-- RGBA,0~1 -->
<texture filename="..."/> <!-- 贴图(可选) -->
</material>
```
### 2.2 `<joint>` — 关节
| type | 含义 | DoF |
|---|---|---|
| `revolute` | 转动(有限位) | 1 |
| `continuous` | 转动(无限位) | 1 |
| `prismatic` | 滑动 | 1 |
| `fixed` | 固定连接 | 0 |
| `floating` | 6 DoF | 6 |
| `planar` | 平面 3 DoF | 3 |
```xml
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0.05" rpy="0 0 0"/> <!-- 子 link 相对父 link -->
<axis xyz="0 0 1"/> <!-- 在 parent 系下定义 -->
<limit lower="-3.14" upper="3.14" <!-- 关节限位 -->
effort="100" velocity="1.0"/> <!-- 力矩 / 速度上限 -->
<dynamics damping="0.1" friction="0.0"/> <!-- 仿真摩擦 -->
</joint>
```
**重要约定**:
- `axis` 在 **parent link 系**下定义
- `origin` 是 **child 相对 parent** 的偏移
- 关节运动绕 `axis` 旋转(或沿 axis 平移)
### 2.3 `<robot>` 根
- `name`:机器人名字,在 ROS 工具中显示
- 必须包含所有 `<link>` 和 `<joint>`
### 2.4 最小 URDF 示例
```xml
<?xml version="1.0"?>
<robot name="hello_arm">
<link name="base_link">
<visual><geometry><box size="0.1 0.1 0.1"/></geometry></visual>
</link>
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0.05"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="1.0" velocity="1.0"/>
</joint>
<link name="link1">
<visual><geometry><box size="0.05 0.05 0.1"/></geometry></visual>
</link>
</robot>
```
---
## 3. 完整 6-DoF 机械臂示例
模拟 6-DoF 工业机械臂(类似 UR5 / xArm 6):
```xml
<?xml version="1.0"?>
<robot name="my_6dof_arm">
<!-- 材料定义 -->
<material name="white"><color rgba="1 1 1 1"/></material>
<material name="red"><color rgba="0.8 0.2 0.2 1"/></material>
<!-- 1) base_link:固定基座 -->
<link name="base_link">
<visual>
<geometry><cylinder radius="0.05" length="0.05"/></geometry>
<material name="white"/>
</visual>
<collision>
<geometry><cylinder radius="0.05" length="0.05"/></geometry>
</collision>
<inertial>
<mass value="1.0"/>
<inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.005"/>
</inertial>
</link>
<!-- 2) joint1:底盘转动(绕 z 轴) -->
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="shoulder_link"/>
<origin xyz="0 0 0.05"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="100" velocity="2.0"/>
</joint>
<link name="shoulder_link">
<visual>
<geometry><box size="0.05 0.05 0.05"/></geometry>
<material name="red"/>
</visual>
<collision>
<geometry><box size="0.05 0.05 0.05"/></geometry>
</collision>
<inertial>
<mass value="0.3"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
</link>
<!-- 3) joint2:肩部俯仰(绕 y 轴) -->
<joint name="joint2" type="revolute">
<parent link="shoulder_link"/>
<child link="upper_arm_link"/>
<origin xyz="0 0 0.05"/>
<axis xyz="0 1 0"/>
<limit lower="-1.57" upper="1.57" effort="100" velocity="2.0"/>
</joint>
<link name="upper_arm_link">
<visual>
<origin xyz="0 0 0.1"/>
<geometry><cylinder radius="0.025" length="0.2"/></geometry>
<material name="white"/>
</visual>
<collision>
<origin xyz="0 0 0.1"/>
<geometry><cylinder radius="0.025" length="0.2"/></geometry>
</collision>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.002" iyz="0" izz="0.0005"/>
</inertial>
</link>
<!-- 4) joint3:肘部俯仰 -->
<joint name="joint3" type="revolute">
<parent link="upper_arm_link"/>
<child link="forearm_link"/>
<origin xyz="0 0 0.2"/>
<axis xyz="0 1 0"/>
<limit lower="-2.0" upper="2.0" effort="50" velocity="2.0"/>
</joint>
<link name="forearm_link">
<visual>
<origin xyz="0 0 0.075"/>
<geometry><cylinder radius="0.02" length="0.15"/></geometry>
<material name="white"/>
</visual>
<collision>
<origin xyz="0 0 0.075"/>
<geometry><cylinder radius="0.02" length="0.15"/></geometry>
</collision>
<inertial>
<mass value="0.3"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.0003"/>
</inertial>
</link>
<!-- 5) joint4:腕部旋转 -->
<joint name="joint4" type="revolute">
<parent link="forearm_link"/>
<child link="wrist_1_link"/>
<origin xyz="0 0 0.15"/>
<axis xyz="1 0 0"/>
<limit lower="-3.14" upper="3.14" effort="20" velocity="2.0"/>
</joint>
<link name="wrist_1_link">
<visual>
<geometry><box size="0.04 0.04 0.04"/></geometry>
<material name="red"/>
</visual>
<collision><geometry><box size="0.04 0.04 0.04"/></geometry></collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.0003" ixy="0" ixz="0" iyy="0.0003" iyz="0" izz="0.0003"/>
</inertial>
</link>
<!-- 6) joint5:腕部俯仰 -->
<joint name="joint5" type="revolute">
<parent link="wrist_1_link"/>
<child link="wrist_2_link"/>
<origin xyz="0 0 0.04"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="20" velocity="2.0"/>
</joint>
<link name="wrist_2_link">
<visual><geometry><box size="0.03 0.03 0.04"/></geometry></visual>
<collision><geometry><box size="0.03 0.03 0.04"/></geometry></collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.0002"/>
</inertial>
</link>
<!-- 7) joint6:末端旋转 -->
<joint name="joint6" type="revolute">
<parent link="wrist_2_link"/>
<child link="end_effector_link"/>
<origin xyz="0 0 0.04"/>
<axis xyz="1 0 0"/>
<limit lower="-3.14" upper="3.14" effort="10" velocity="2.0"/>
</joint>
<link name="end_effector_link">
<visual>
<geometry><cylinder radius="0.02" length="0.03"/></geometry>
<material name="red"/>
</visual>
<collision>
<geometry><cylinder radius="0.02" length="0.03"/></geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="0.0001"/>
</inertial>
</link>
<!-- 8) 末端夹爪(可选 gripper) -->
<joint name="gripper_joint" type="fixed">
<parent link="end_effector_link"/>
<child link="gripper_link"/>
<origin xyz="0 0 0.025"/>
</joint>
<link name="gripper_link">
<visual><geometry><box size="0.04 0.06 0.02"/></geometry></visual>
<collision><geometry><box size="0.04 0.06 0.02"/></geometry></collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="0.0001"/>
</inertial>
</link>
</robot>
```
**约定**:
- x = 前进
- y = 左
- z = 上
- joint1 绕 z 轴,旋转"底盘朝向"
- joint2/joint3 绕 y 轴,"肩膀/肘部俯仰"
- joint4/joint5/joint6 腕部 3 DoF
- 总长度约 0.4 m,可伸展约 0.5 m
---
## 4. xacro:模板化 URDF
URDF 重复内容多(左右轮、对称连杆)。**xacro** 提供宏、变量、数学。
### 4.1 安装
xacro 通常自带 ROS2:
```bash
sudo apt install ros-humble-xacro
```
### 4.2 xacro 基础语法
```xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="arm">
<!-- 变量 -->
<xacro:property name="link_len" value="0.1"/>
<xacro:property name="PI" value="3.14159"/>
<!-- 宏 -->
<xacro:macro name="make_link" params="name color *origin">
<link name="${name}">
<visual>
<xacro:insert_block name="origin"/>
<geometry><box size="0.05 0.05 ${link_len}"/></geometry>
</visual>
</link>
</xacro:macro>
<link name="base_link"/>
<xacro:make_link name="link1" color="red">
<origin xyz="0 0 0.05"/>
</xacro:make_link>
<xacro:make_link name="link2" color="green">
<origin xyz="0 0 0.1"/>
</xacro:make_link>
<!-- 数学 -->
<xacro:property name="mass" value="${1.0 * link_len}"/>
</robot>
```
### 4.3 编译 xacro → URDF
```bash
xacro arm.xacro > arm.urdf
# 或 ros2 launch 时直接传 xacro 输出:
ros2 run robot_state_publisher robot_state_publisher \
--ros-args -p robot_description:="$(xacro /path/to/arm.xacro)"
```
---
## 5. 坐标约定 REP-103 / REP-105
ROS2 的标准约定,**违反会导致 MoveIt2 / Gazebo 算错**:
### 5.1 REP-103(单位 + 坐标轴方向)
- 长度单位:**米**(m)
- 角度单位:**弧度**(rad)
- 右手系:
- **x** = 前进
- **y** = 左
- **z** = 上
### 5.2 REP-105(坐标系语义)
| frame | 含义 |
|---|---|
| `world` | 全局参考(通常不动) |
| `map` | SLAM 输出的地图 |
| `odom` | 里程计 / AMCL 定位 |
| `base_link` | 机器人底盘中心(刚体) |
| `base_footprint` | 底盘在地面的投影(2D 导航) |
关系:
- `world → map → odom → base_link` 是固定链
- `world → map` 由 SLAM 提供(变化)
- `map → odom` 由 AMCL / 里程计提供
- `odom → base_link` 由编码器 / IMU 提供
---
## 6. ros2_control 集成(加 transmission)
`ros2_control` 需要每个关节加 `<transmission>` 标签:
```xml
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="shoulder_link"/>
<axis xyz="0 0 1"/>
<limit effort="100" velocity="2.0" lower="-3.14" upper="3.14"/>
<!-- ros2_control 必填 -->
<transmission name="trans_joint1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor_joint1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</joint>
```
`<transmission>` 描述:
- 哪个 joint
- 用哪个 hardware interface(Position / Velocity / Effort)
- 减速比(`mechanicalReduction`)
---
## 7. Gazebo 集成(加 gazebo 标签)
Gazebo Sim / ros_gz 加载 URDF 时需要 `<gazebo>` 标签 + `<transmission>`(已经加)。
```xml
<gazebo>
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
<parameters>$(find my_arm)/config/controllers.yaml</parameters>
</plugin>
</gazebo>
<!-- 每个 link 加颜色(可选) -->
<gazebo reference="shoulder_link">
<material>Gazebo/Red</material>
</gazebo>
```
---
## 8. 校验 + 可视化
### 8.1 校验
```bash
sudo apt install liburdfdom-tools
# 检查 URDF 合法性
check_urdf my_arm.urdf
# 输出 OK 即合法,会打印 link / joint 树结构
```
### 8.2 生成 PDF/PNG 图
```bash
sudo apt install liburdfdom-tools python3-urdfdom-py
urdf_to_graphiz my_arm.urdf # 生成 .pdf
# 或
urdf_to_graphiz my_arm.urdf -o my_arm.pdf
```
### 8.3 RViz 可视化
```bash
ros2 launch urdf_tutorial display.launch.py
# 或自己写 launch:
ros2 run rviz2 rviz2
# 在 RViz:
# Fixed Frame → base_link
# Add → RobotModel ← 显示 URDF
# Add → TF ← 显示 TF tree
```
### 8.4 joint_state_publisher_gui(手动调关节)
```bash
ros2 run joint_state_publisher_gui joint_state_publisher_gui
# 弹出 GUI,拖滑块调关节角,看机械臂在 RViz 里动
```
---
## 9. 工具链总结
```
xacro .xacro ───> .urdf (用 xacro 编译)
│
├──> check_urdf (校验)
├──> urdf_to_graphiz (可视化)
│
├──> robot_state_publisher
│ │
│ │ 读 URDF + /joint_states → 算 TF → /tf
│ │
│ └──> tf2::Buffer (其他节点订阅)
│
├──> MoveIt2 (规划)
│ │
│ │ 读 URDF + SRDF → 算 IK / path
│ │
│ └──> trajectory_msgs/JointTrajectory
│
└──> Gazebo Sim (仿真)
│
│ 读 URDF + gazebo 标签 → 物理仿真
```
---
## 10. MoveIt2 SRDF(规划组)
URDF 描述"机器人长啥样",SRDF 加 MoveIt2 需要的规划信息:
- **虚拟关节**:base_link → world(让 MoveIt2 把臂放在世界系)
- **规划组**(Planning Group):哪些关节一起动
- **末端执行器**:夹爪
- **预设位姿**(Pose):home / ready / extended
### 10.1 用 MoveIt Setup Assistant 生成
```bash
ros2 launch moveit_setup_assistant setup_assistant.launch.py
```
GUI 里:
1. Load URDF
2. Add Virtual Joint(base_link → world)
3. Add Planning Group("arm",含 6 joint)
4. Add End Effector(gripper)
5. Add Poses(home / ready / up)
6. **Generate** SRDF + MoveIt config
### 10.2 SRDF 简例
```xml
<robot name="my_arm">
<!-- virtual joint: world → base_link -->
<virtual_joint name="world_to_base" type="fixed"
parent_frame="world" child_link="base_link"/>
<!-- planning group -->
<group name="arm">
<chain base_link="base_link" tip_link="gripper_link"/>
<joint name="joint1"/>
<joint name="joint2"/>
...
</group>
<!-- end effector -->
<end_effector name="gripper" parent_link="end_effector_link"
group="gripper"/>
<!-- pose -->
<group_state name="home" group="arm">
<joint name="joint1" value="0"/>
<joint name="joint2" value="0"/>
...
</group_state>
</robot>
```
---
## 11. 在本仓库里跑
### 11.1 启 3 关节机械臂 demo
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup robot_launch.py"
```
节点:
- `joint_state_publisher_cpp` (本包)
- `robot_state_publisher` (系统包)
- `tf2_listener_cpp` (本包)
### 11.2 校验本仓库 URDF
```bash
docker exec ros2_dev bash -lc "check_urdf /root/ros2_ws/install/cpp_robot_tf2/share/cpp_robot_tf2/urdf/simple_arm.urdf"
```
### 11.3 源码
- URDF: [`src/cpp_robot_tf2/urdf/simple_arm.urdf`](../src/cpp_robot_tf2/urdf/simple_arm.urdf)
- launch: [`src/cpp_robot_tf2/launch/robot_tf2_launch.py`](../src/cpp_robot_tf2/launch/robot_tf2_launch.py)
### 11.4 端到端日志
[`docker/robot_e2e.log`](../docker/robot_e2e.log)
---
## 12. 进阶:6-DoF 真机械臂 URDF 实战
### 12.1 找开源 URDF
| 机械臂 | URDF 来源 |
|---|---|
| Franka Panda | `ros-planning/moveit_resources/panda` |
| UR5 / UR10 | `ros-industrial/universal_robot` |
| Ufactory xArm 6 | `xArm-Robotics/xarm_ros2` |
| Aloha | `tonyzhaozh/aerial_manipulation` |
### 12.2 URDF 适配本仓库
把开源 URDF 拷到 `src/<your_arm>/urdf/`:
```bash
cp -r <upstream>/franka_description/urdf src/my_arm/urdf/
# 加 transmission(ros2_control 必填)
# 已经在开源版本里有,但要确认 hardwareInterface 是 PositionJointInterface
```
### 12.3 加 ros2_control 配置
`src/my_arm/config/controllers.yaml`:
```yaml
controller_manager:
ros__parameters:
update_rate: 100
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateController
arm_controller:
type: position_controllers/JointGroupPositionController
arm_controller:
ros__parameters:
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
```
### 12.4 加 launch
`src/my_arm/launch/arm_bringup.launch.py`:
```python
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
pkg = get_package_share_directory('my_arm')
urdf = os.path.join(pkg, 'urdf', 'arm.urdf')
controllers = os.path.join(pkg, 'config', 'controllers.yaml')
with open(urdf, 'r') as f: urdf_content = f.read()
return LaunchDescription([
# 1) robot_state_publisher
Node(package='robot_state_publisher', executable='robot_state_publisher',
parameters=[{'robot_description': urdf_content}]),
# 2) ros2_control_node
Node(package='controller_manager', executable='ros2_control_node',
parameters=[{'robot_description': urdf_content,
'update_rate': 100}],
output='screen'),
# 3) spawn joint_state_broadcaster
Node(package='controller_manager', executable='spawner',
arguments=['joint_state_broadcaster', '-c', '/controller_manager']),
# 4) spawn arm_controller
Node(package='controller_manager', executable='spawner',
arguments=['arm_controller', '-c', '/controller_manager']),
])
```
### 12.5 加 MoveIt2
```bash
sudo apt install ros-humble-moveit
# 用 MoveIt Setup Assistant 加载你的 URDF 生成 SRDF
ros2 launch moveit_setup_assistant setup_assistant.launch.py
```
详见 [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 2。
---
## 接下来读
| 主题 | 文档 |
|---|---|
| TF 坐标变换 | [`50-tf2.md`](50-tf2.md) |
| ros2_control + MoveIt2 | [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 2 |
| launch 文件 | [`70-launch.md`](70-launch.md) |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
doc/70-launch.md
+468
View File
@@ -0,0 +1,468 @@
# 70 · Launch 文件系统(完全指南)
> **目标**:能写 ROS2 Python launch 文件,理解参数覆盖、嵌套、生命周期、条件启动。
---
## 目录
- [1. 什么是 launch](#1-什么是-launch)
- [2. 最小 launch 文件](#2-最小-launch-文件)
- [3. 启动方式](#3-启动方式)
- [4. 顶层 Actions](#4-顶层-actions)
- [5. 事件处理 / 生命周期](#5-事件处理--生命周期)
- [6. 条件启动](#6-条件启动)
- [7. 命名空间 / 多机器人](#7-命名空间--多机器人)
- [8. 常用模式(实战)](#8-常用模式实战)
- [9. ROS2 launch CLI](#9-ros2-launch-cli)
- [10. 常见坑](#10-常见坑)
- [11. 在本仓库里跑](#11-在本仓库里跑)
- [12. 进阶:复杂 launch + 自定义 Action](#12-进阶复杂-launch--自定义-action)
---
## 1. 什么是 launch
ROS1 用 XML 启动多节点繁琐。ROS2 的 launch = Python 脚本:
- 一键启多个节点 + 设参数
- 跨包嵌套复用
- 条件启动、按机器切换配置
- 生命周期管理(启动顺序、超时、失败重试)
---
## 2. 最小 launch 文件
```python
# src/<pkg>/launch/my_launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
"""ROS2 launch 框架会调这个函数生成 LaunchDescription 实例。"""
return LaunchDescription([
Node(
package='py_pubsub', # 包名
executable='talker', # 入口名(setup.py entry_points)
name='talker_py', # 节点名(可重命名避免冲突)
output='screen', # stdout/stderr 直打到终端
parameters=[ # 参数 dict
{'period_ms': 500, 'topic': 'chatter'},
],
remappings=[ # topic 重映射
('/chatter', '/my_chatter'),
],
),
])
```
**入口约定**:函数名必须是 `generate_launch_description()`,返回 `LaunchDescription`。
---
## 3. 启动方式
```bash
# 装包后,ROS 自动索引 launch 到 share/<pkg>/launch/
colcon build --packages-select py_pubsub
# 用包名 + launch 文件名(去 .py 后缀)
ros2 launch py_pubsub my_launch.py
# 加参数(覆盖 LaunchConfiguration)
ros2 launch py_pubsub my_launch.py topic:=hello period_ms:=200
```
ROS2 找 launch 文件路径:`<install>/share/<pkg>/launch/`。**launch 文件必须装到 share/<pkg>/launch/**,否则报 "file not found"。
---
## 4. 顶层 Actions
### 4.1 `Node`
```python
Node(
package=...,
executable=...,
name=...,
namespace='/', # 命名空间(可分多机器人)
output='screen', # 'screen' / 'log' / 'both'
parameters=[{...}], # 参数 dict
remappings=[(...)], # topic 重映射
arguments=[...], # 透传给 executable 的命令行参数
respawn=False, # 崩溃是否重启
respawn_delay=0,
)
```
### 4.2 `DeclareLaunchArgument` 声明命令行参数
```python
from launch.actions import DeclareLaunchArgument
DeclareLaunchArgument(
'topic', # 参数名
default_value='chatter', # 默认值
description='Topic name', # 说明(ros2 launch --help 显示)
choices=['chatter', 'hello'], # (可选)限定取值
)
```
### 4.3 `LaunchConfiguration` 取值
```python
from launch.substitutions import LaunchConfiguration
topic = LaunchConfiguration('topic')
Node(
...,
parameters=[{'topic': topic}],
)
```
### 4.4 `IncludeLaunchDescription` 嵌套
```python
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
IncludeLaunchDescription(
PythonLaunchDescriptionSource(<other_launch.py>),
launch_arguments={
'topic': 'chatter',
'period_ms': '500',
}.items(),
)
```
**用 FindPackageShare + PathJoinSubstitution**(跨包推荐):
```python
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
pkg_share = FindPackageShare('py_pubsub')
launch_path = PathJoinSubstitution([pkg_share, 'launch', 'pubsub_launch.py'])
IncludeLaunchDescription(PythonLaunchDescriptionSource(launch_path))
```
### 4.5 `ExecuteProcess` 跑 shell 命令
```python
from launch.actions import ExecuteProcess
ExecuteProcess(
cmd=['xacro', 'arm.xacro'],
output='screen',
)
```
> ⚠️ `Command(['cat', '/path'])` 偶尔会丢空格变成 `'cat/path'`,
> 建议**launch 启动前 `open().read()` 读文件**,不要 cat。
### 4.6 `OpaqueFunction` 任意函数
```python
from launch.actions import OpaqueFunction
def my_setup(context, *args, **kwargs):
# 自定义逻辑
return [...]
LaunchDescription([
OpaqueFunction(function=my_setup),
Node(...),
])
```
### 4.7 `TimerAction` 延迟启动
```python
from launch.actions import TimerAction
LaunchDescription([
Node(package='srv', executable='server'),
TimerAction(period=2.0, actions=[
Node(package='client', executable='client'),
]),
])
```
---
## 5. 事件处理 / 生命周期
### 5.1 启动顺序
ROS2 launch 按 LaunchDescription 里 action 的顺序**同步**启动(默认)。
### 5.2 事件 handler
```python
from launch import LaunchDescription
from launch.event_handlers import OnProcessExit
from launch.actions import RegisterEventHandler, LogInfo
def generate_launch_description():
server = Node(package='srv', executable='server')
return LaunchDescription([
server,
RegisterEventHandler(
OnProcessExit(
target_action=server,
on_exit=[LogInfo(msg='server exited, shutting down')],
)
),
])
```
支持的事件: `OnProcessStart` / `OnProcessExit` / `OnProcessIO` 等。
---
## 6. 条件启动
```python
from launch.conditions import IfCondition, UnlessCondition
Node(
...,
condition=IfCondition(LaunchConfiguration('use_camera')),
)
```
CLI 用 `:=true` / `:=false`:
```bash
ros2 launch my_pkg my.launch.py use_camera:=true
```
---
## 7. 命名空间 / 多机器人
### 7.1 namespace 隔离
```python
Node(
package='py_pubsub',
executable='talker',
namespace='robot1', # → topic /robot1/chatter
name='talker', # → 节点名 /robot1/talker
)
```
### 7.2 push_ros_namespace
```python
from launch.actions import PushRosNamespace
LaunchDescription([
PushRosNamespace('robot1'),
Node(...),
])
```
---
## 8. 常用模式(实战)
### 8.1 参数文件加载
```python
from ament_index_python.packages import get_package_share_directory
import os
params_file = os.path.join(
get_package_share_directory('my_pkg'),
'config', 'params.yaml'
)
Node(
package='my_pkg',
executable='node',
parameters=[params_file],
)
```
### 8.2 多节点同包
```python
nodes = [
Node(package='py_pubsub', executable='talker', name='talker_a', parameters=[{'topic': 'a'}]),
Node(package='py_pubsub', executable='listener', name='listener_a', parameters=[{'topic': 'a'}]),
]
```
### 8.3 跨包组合(`bringup` 模式)
本仓库 [`src/bringup/launch/full_demo_launch.py`](../src/bringup/launch/full_demo_launch.py):
```python
def _include(pkg_name, launch_file):
pkg = FindPackageShare(pkg_name)
path = PathJoinSubstitution([pkg, 'launch', launch_file])
return IncludeLaunchDescription(PythonLaunchDescriptionSource(path))
def generate_launch_description():
return LaunchDescription([
_include('py_pubsub', 'pubsub_launch.py'),
_include('cpp_pubsub', 'pubsub_launch.py'),
_include('py_srv', 'srv_launch.py'),
_include('py_action_demo', 'action_launch.py'),
_include('cpp_robot_tf2', 'robot_tf2_launch.py'),
_include('py_vision_demo', 'vision_launch.py'),
])
```
### 8.4 读 URDF 文件内容
```python
import os
from ament_index_python.packages import get_package_share_directory
pkg_share = get_package_share_directory('cpp_robot_tf2')
urdf_path = os.path.join(pkg_share, 'urdf', 'simple_arm.urdf')
with open(urdf_path, 'r', encoding='utf-8') as f:
robot_description = f.read()
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': robot_description}],
)
```
---
## 9. ROS2 launch CLI
```bash
# 列出所有 launch 文件
ros2 launch --show-args <pkg> <file>
# 看可用参数
ros2 launch <pkg> <file> --show-args
# 调试(详细日志)
ros2 launch -d <pkg> <file>
# 传参
ros2 launch <pkg> <file> topic:=hello period_ms:=200
```
---
## 10. 常见坑
### 10.1 launch 文件找不到
- `setup.py` 的 `data_files` 必须包含 `'share/<pkg>/launch': ['launch/*.py']`
- colcon build 后 launch 文件没复制到 install/share → 检查 build log
### 10.2 节点名冲突
同一 ROS Domain 内**节点名必须唯一**。launch 里 `name=` 重命名,或用 `namespace=` 隔离。
### 10.3 参数失效
`parameters=[{'topic': 'chatter'}]` 里 topic 是字符串;如果想"取自 LaunchConfiguration":
```python
parameters=[{'topic': LaunchConfiguration('topic')}]
```
### 10.4 `Command(['cat', path])` 空格丢了
如前所述,**改用 launch 启动前 `open().read()`**。
### 10.5 launch 启动后节点秒退
- 节点 main 函数异常(看日志)
- 参数名拼错(节点没收到参数)
- launch `output='log'` 看不到错误 → 改 `output='screen'`
### 10.6 launch 嵌套找不到文件
`IncludeLaunchDescription` 引用的 launch 文件必须在被包含包的 `share/<pkg>/launch/` 下被 colcon 实际安装。
---
## 11. 在本仓库里跑
### 11.1 各包 launch 文件清单
```
src/
├── py_pubsub/launch/pubsub_launch.py (2 节点,纯 Python)
├── cpp_pubsub/launch/pubsub_launch.py (2 节点,C++ + topic 命令行参数)
├── py_srv/launch/srv_launch.py (1 service)
├── py_action_demo/launch/action_launch.py (1 action server)
├── cpp_robot_tf2/launch/robot_tf2_launch.py (3 节点,URDF + TF)
├── py_vision_demo/launch/vision_launch.py (2 节点,cv_bridge)
└── bringup/launch/
├── pubsub_launch.py (4 节点,Topic 跨包)
├── service_launch.py
├── action_launch.py
├── robot_launch.py (嵌套 cpp_robot_tf2)
├── vision_launch.py (嵌套 py_vision_demo)
└── full_demo_launch.py (11 节点,跨所有包)
```
### 11.2 看可用参数
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup full_demo_launch.py --show-args"
```
---
## 12. 进阶:复杂 launch + 自定义 Action
### 12.1 复杂 launch 模板
```python
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
OpaqueFunction,
RegisterEventHandler,
LogInfo,
)
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.substitutions import (
LaunchConfiguration, Command, FindExecutable, PathJoinSubstitution,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def launch_setup(context, *args, **kwargs):
# 任何动态逻辑
config_file = LaunchConfiguration('config_file').perform(context)
if not os.path.exists(config_file):
return [LogInfo(msg=f'config not found: {config_file}')]
return [...]
def generate_launch_description():
return LaunchDescription([
DeclareLaunchArgument('config_file', default_value='config.yaml'),
DeclareLaunchArgument('enable_ai', default_value='true'),
Node(
package='my_pkg',
executable='main_node',
parameters=[LaunchConfiguration('config_file')],
condition=IfCondition(LaunchConfiguration('enable_ai')),
),
OpaqueFunction(function=launch_setup),
])
```
---
## 接下来读
| 主题 | 文档 |
|---|---|
| Topic | [`20-topics.md`](20-topics.md) |
| Service | [`30-services.md`](30-services.md) |
| Action | [`40-actions.md`](40-actions.md) |
| colcon / ament 包构建 | [`80-package-build.md`](80-package-build.md) |
| 测试策略 | [`90-testing.md`](90-testing.md) |
doc/80-package-build.md
+405
View File
@@ -0,0 +1,405 @@
# 80 · 包构建机制 colcon / ament(完全指南)
> **目标**:理解 ROS2 一个包从源码到 `ros2 run` 能找到的完整流程,能自己写 ament_python / ament_cmake 包。
---
## 目录
- [1. 工作空间结构](#1-工作空间结构)
- [2. 包类型](#2-包类型)
- [3. package.xml(包身份证)](#3-packagexml包身份证)
- [4. ament_python 包详解](#4-ament_python-包详解)
- [5. ament_cmake 包详解](#5-ament_cmake-包详解)
- [6. colcon build 内部流程](#6-colcon-build-内部流程)
- [7. ament_index 与 ros2 工具发现](#7-ament_index-与-ros2-工具发现)
- [8. 依赖解析与 rosdep](#8-依赖解析与-rosdep)
- [9. 自定义消息 / 服务 / Action(进阶)](#9-自定义消息--服务--action进阶)
- [10. 常见构建错误 + 解决](#10-常见构建错误--解决)
- [11. 在本仓库里跑](#11-在本仓库里跑)
- [12. 进阶:多包 workspace / release 流程](#12-进阶多包-workspace--release-流程)
---
## 1. 工作空间结构
```
ros2_ws/ ← 工作空间根
├── src/ ← 包源码(你写代码的地方)
│ ├── pkg_a/ ← 一个 ROS 包
│ ├── pkg_b/
│ └── ...
├── build/ ← colcon build 中间产物
├── install/ ← colcon install 产物(ros2 唯一识别)
├── log/ ← build/test 日志
└── ...
```
**`install/` 是唯一能被 `ros2` 命令识别的目录**。每次进新 shell 必须:
```bash
source install/setup.bash
```
---
## 2. 包类型
ROS2 有两种主流 build_type:
| build_type | 文件 | 用途 |
|---|---|---|
| **ament_python** | `setup.py` + `package.xml` | Python 包 |
| **ament_cmake** | `CMakeLists.txt` + `package.xml` | C++ 包 |
| ament_cmake_python | 两者混合 | 复杂项目 |
---
## 3. package.xml(包身份证)
```xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd"?>
<package format="3">
<name>my_pkg</name>
<version>0.1.0</version>
<description>...</description>
<maintainer email="x@y.com">Name</maintainer>
<license>Apache-2.0</license>
<depend>rclpy</depend> <!-- 构建 + 运行都需 -->
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
```
| 字段 | 含义 |
|---|---|
| `<depend>` | 构建 + 运行 |
| `<build_depend>` | 仅构建 |
| `<exec_depend>` | 仅运行 |
| `<test_depend>` | 仅测试 |
| `<build_type>` | `ament_python` / `ament_cmake` |
---
## 4. ament_python 包详解
### 4.1 目录结构
```
py_pubsub/
├── package.xml
├── setup.py ← 标准 setuptools
├── setup.cfg ← ament_python 脚本目录约定
├── resource/
│ └── py_pubsub ← 空文件,ament_index marker(必填!)
├── py_pubsub/ ← Python 模块
│ ├── __init__.py
│ └── *.py
├── launch/
│ └── *.py
└── test/
└── test_*.py
```
### 4.2 setup.py
```python
from setuptools import find_packages, setup
package_name = 'py_pubsub'
setup(
name=package_name,
version='0.1.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name + '/launch', ['launch/*.py']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='xs',
maintainer_email='dev@example.com',
description='Python talker/listener demo for ROS2 Humble',
license='Apache-2.0',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'talker = py_pubsub.publisher_member_function:main',
'listener = py_pubsub.subscriber_member_function:main',
],
},
)
```
**关键点**:
- `data_files` 必须把 `package.xml` + `launch/` 拷到 `share/<pkg>/`
- `resource/<pkg>` 空文件 → ament_index marker(必填!)
- `entry_points/console_scripts` 暴露 `ros2 run` 可执行名
### 4.3 setup.cfg
```ini
[develop]
script_dir=$base/lib/py_pubsub
[install]
install_scripts=$base/lib/py_pubsub
```
控制 `console_scripts` 装到 `$base/lib/<pkg>/`,ROS2 启动器能找到。
---
## 5. ament_cmake 包详解
### 5.1 目录结构
```
cpp_pubsub/
├── package.xml
├── CMakeLists.txt
├── src/
│ └── *.cpp
├── include/ ← 公开头文件(可选)
├── launch/
│ └── *.py
├── test/
│ └── test_*.cpp
└── config/ ← YAML 配置(可选)
```
### 5.2 CMakeLists.txt
```cmake
cmake_minimum_required(VERSION 3.16)
project(cpp_pubsub VERSION 0.1.0)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
# GCC/Clang 编译警告
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
include_directories(include)
# 节点可执行
add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp std_msgs)
add_executable(listener src/subscriber_member_function.cpp)
ament_target_dependencies(listener rclcpp std_msgs)
# 装到 install/lib/<pkg>/
install(TARGETS talker listener DESTINATION lib/${PROJECT_NAME})
# launch 文件装到 share/<pkg>/launch/
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/)
# 测试
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(test_pub_sub test/test_pub_sub.cpp)
ament_target_dependencies(test_pub_sub rclcpp std_msgs)
endif()
ament_package() ← 必填!
```
**关键**:
- `ament_target_dependencies(<target> pkg1 pkg2)` 同时设置 include path 和 link
- `install(TARGETS ...)` 把可执行装到 install
- `install(DIRECTORY launch ...)` 把 launch 装到 share
- `ament_package()` 末尾必填
---
## 6. colcon build 内部流程
```bash
colcon build --symlink-install --packages-select <pkg>
```
按依赖顺序,每个包:
```
┌──────────────────────────────────────────────────────────┐
│ 1. 发现 src/<pkg>/ 下的 package.xml │
│ 2. 读 <export><build_type>,决定 build type │
│ 3. 调用对应 build_type 的 hook: │
│ ament_python → python setup.py build + install │
│ ament_cmake → cmake + make + ament_package │
│ 4. 写 ament_index 标记 (share/ament_index/...) │
│ 5. 把产物装到 install/<pkg>/ │
└──────────────────────────────────────────────────────────┘
```
加 `--symlink-install`:
- Python:源码**软链**到 install,改源码立即生效(不用重 build)
- C++:可执行仍硬编,但 launch / config 文件软链
---
## 7. ament_index 与 ros2 工具发现
`ros2 run / launch / pkg executables / topic info` 等命令都靠 **ament_index**:
- 启动时扫 `install/<pkg>/share/ament_index/resource_index/packages/<pkg>` 文件
- 找到包路径后,扫 `install/<pkg>/lib/<pkg>/` 找可执行
- 扫 `install/<pkg>/share/<pkg>/launch/` 找 launch 文件
**手动重建索引**(极少需要):
```bash
# 索引在 install/share/ament_index/ 里,正常情况下 colcon build 自动维护
ros2 doctor --report # 看索引健康
```
---
## 8. 依赖解析与 rosdep
`package.xml` 的 `<depend>` 让 colcon 自动排构建顺序:
- 你的 `py_pubsub` 依赖 `rclpy`,`rclpy` 是 apt 包,colcon 跳过
- `py_srv` 依赖 `example_interfaces`,同上
- colcon 只 build 我们 src/ 下的包
**`rosdep`** 是 apt 包管理:
```bash
sudo rosdep init && rosdep update
sudo rosdep install -i --from-paths src/ # 装齐所有 apt 依赖
```
本仓库基础镜像 `osrf/ros:humble-desktop` 已装齐所有 ROS2 客户端,**不需要额外 rosdep**。
---
## 9. 自定义消息 / 服务 / Action(进阶)
需要:
1. 在包内建 `msg/`、`srv/`、`action/` 目录
2. 写 `.msg` / `.srv` / `.action` 文件
3. `package.xml` 加:
- `<build_depend>rosidl_default_generators</build_depend>`
- `<exec_depend>rosidl_default_runtime</exec_depend>`
4. `CMakeLists.txt` 加 `rosidl_generate_interfaces(${PROJECT_NAME} ${MSG_FILES} ...)`
5. `setup.py` `data_files` 加 `('share/<pkg>/msg': ['msg/*.msg'], ...)`
6. **ament_python 需要单独 `rosidl` 包**(混合构建)
**本仓库只用 example_interfaces 自带的 Fibonacci / AddTwoInts**(由 ROS2 系统包提供),避开这个复杂度。
---
## 10. 常见构建错误 + 解决
### 10.1 `Could not find a package configuration file provided by "rclpy"`
少装 apt 包:
```bash
sudo apt install ros-humble-rclpy # 镜像外
```
### 10.2 `Package 'X' not found in colcon build`
ament_index 没刷新。重启容器 + source install/setup.bash。
### 10.3 `ament_python` install 0 files
`setup.py` 的 `packages=` 不对。检查 `py_pubsub/` 下有 `__init__.py`。
### 10.4 launch 文件未找到
检查 `setup.py`:
```python
('share/' + package_name + '/launch', ['launch/*.py']),
```
**`launch/*.py` 是 glob**,不是 `'launch/foo.py'`。
### 10.5 ROS Domain 冲突
`ROS_DOMAIN_ID=0` 默认。多项目用 `ROS_DOMAIN_ID=42` 隔离。
### 10.6 Cyclone DDS 缓存了 CMakeCache
`Could not find ROS middleware implementation 'rmw_cyclonedds_cpp'` 但又设了 `RMW_IMPLEMENTATION=rmw_cyclonedds_cpp` → CMake 缓存了。
**修法**:
```bash
rm -rf build install log
colcon build ...
```
---
## 11. 在本仓库里跑
### 11.1 全部 build
```bash
cd /root/ros2_ws
colcon build --symlink-install \
--packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup
```
### 11.2 增量 build(只编改的)
```bash
colcon build
```
### 11.3 清理后重 build
```bash
rm -rf build install log
colcon build --symlink-install
```
### 11.4 单包 build
```bash
colcon build --packages-select py_pubsub
```
---
## 12. 进阶:多包 workspace / release 流程
### 12.1 多 workspace 覆盖
```bash
# 用 setup.bash 叠加
source /opt/ros/humble/setup.bash
source /workspace1/install/setup.bash
source /workspace2/install/setup.bash
# 后 source 的覆盖前面
```
### 12.2 Release 流程
```bash
# 1) bloom-generate 准备 release
sudo apt install python3-bloom
bloom-generate rosdebian --os-only ubuntu:jammy
# 2) 构建 .deb 包
bloom-release --rosdistro humble --track humble --new 0.1.0 my_pkg
# 3) 推送到 ROS 仓库(rosindex / GitHub)
```
**本仓库未发布**,如要 release 走标准流程。
---
## 接下来读
| 主题 | 文档 |
|---|---|
| Launch 文件 | [`70-launch.md`](70-launch.md) |
| Docker 开发 | [`85-docker.md`](85-docker.md) |
| 测试策略 | [`90-testing.md`](90-testing.md) |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
doc/85-docker.md
+405
View File
@@ -0,0 +1,405 @@
# 85 · Docker 容器化开发(完全指南)
> **目标**:吃透本仓库 Docker 配置,知道每行在做什么,能改能扩,能在 Win/macOS/Linux 一致跑 ROS2。
---
## 目录
- [1. 为什么用 Docker](#1-为什么用-docker)
- [2. 镜像构建](#2-镜像构建)
- [3. 容器编排](#3-容器编排)
- [4. 容器生命周期命令](#4-容器生命周期命令)
- [5. 一键启动脚本](#5-一键启动脚本)
- [6. 调试技巧](#6-调试技巧)
- [7. 常见坑](#7-常见坑)
- [8. 多机 / 多机器人扩展](#8-多机--多机器人扩展)
- [9. 在本仓库里跑](#9-在本仓库里跑)
- [10. 进阶:多阶段构建 + 镜像瘦身](#10-进阶多阶段构建--镜像瘦身)
---
## 1. 为什么用 Docker
| 痛点 | Docker 怎么解 |
|---|---|
| Linux/Windows/macOS 行为差异 | 镜像固定 Linux,行为一致 |
| ROS2 apt 装一堆,污染系统 | 容器内隔离 |
| 团队协作"在我机器能跑" | 镜像统一 |
| 升级 ROS 版本代价大 | 换镜像就行 |
| 跨语言/多版本测试 | 同一镜像多容器 |
本仓库基于 **`osrf/ros:humble-desktop`**(OSRF 官方维护):
- ROS2 Humble 完整运行时
- 默认 RMW: `rmw_fastrtps_cpp`
- 自带 RViz2 / rqt / colcon / rosdep
---
## 2. 镜像构建
### 2.1 Dockerfile
[`docker/Dockerfile`](../docker/Dockerfile) 在 `osrf/ros:humble-desktop` 之上加:
```dockerfile
FROM osrf/ros:humble-desktop
ENV DEBIAN_FRONTEND=noninteractive
ENV ROS_DISTRO=humble
ENV WORKSPACE=/root/ros2_ws
RUN apt-get update && apt-get install -y \
python3-colcon-common-extensions \
python3-pip \
nano \
curl \
&& rm -rf /var/lib/apt/lists/*
RUN pip3 install -U colcon-argcomplete colcon-common-extensions
RUN echo 'source /opt/ros/humble/setup.bash' >> /root/.bashrc && \
echo 'source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash' >> /root/.bashrc
WORKDIR ${WORKSPACE}
CMD ["bash"]
```
### 2.2 构建
```powershell
# Windows
docker compose -f D:\xs\ros2\docker\docker-compose.yml build
```
```bash
# Linux
docker compose -f docker/docker-compose.yml build
```
镜像名:`ros2-humble-dev:latest`,约 3GB。
### 2.3 镜像内已装
- ROS2 Humble Desktop
- `colcon-common-extensions` + `colcon-argcomplete`
- `cv_bridge` (openCV ↔ ROS Image)
- `ros-humble-tf2-tools` (`view_frames`)
- `ros-humble-robot-state-publisher` (URDF → TF)
**未装**(按需补):
- `ros-humble-rmw-cyclonedds-cpp`(默认 fastdds 够用)
- `ros-humble-ros2-control`
- `ros-humble-moveit`
- `ros-humble-navigation2`
### 2.4 修改镜像(自定义)
加包:在 `apt-get install` 加一行:
```dockerfile
RUN apt-get install -y ros-humble-ros2-control ros-humble-ros2-controllers
```
加 Python 包:
```dockerfile
RUN pip3 install ultralytics==8.0.0 numpy==1.26
```
---
## 3. 容器编排
### 3.1 docker-compose.yml
[`docker/docker-compose.yml`](../docker/docker-compose.yml):
```yaml
services:
ros2:
build: .
image: ros2-humble-dev:latest
container_name: ros2_dev
privileged: true # 调试用
stdin_open: true # docker exec -it
tty: true
network_mode: host # DDS multicast 必须
environment:
- ROS_DOMAIN_ID=0
# 不要设 RMW_IMPLEMENTATION=rmw_cyclonedds_cpp:
# 镜像没装 cyclone dds,CMake 配置会失败。
volumes:
- ..:/root/ros2_ws # bind mount 本机 D:\xs\ros2
working_dir: /root/ros2_ws
command: ["bash", "-lc", "tail -f /dev/null"] # 容器永不退
```
### 3.2 为什么 host network
ROS2 默认用 DDS multicast 在同一网段自动发现节点。多容器或跨主机时,**bridge 网络
会拦截 multicast**,导致节点看不见彼此。
`network_mode: host` 让容器用宿主机的网络栈,直接走 multicast。
### 3.3 为什么 bind mount 整个工程
`..` 是 docker-compose.yml 的上一级(`D:\xs\ros2`)。挂到容器内
`/root/ros2_ws`,**你在 Windows 改代码,容器内 colcon 立即看到**(`--symlink-install`)。
---
## 4. 容器生命周期命令
```powershell
# 构建镜像
docker compose -f D:\xs\ros2\docker\docker-compose.yml build
# 启动(后台)
docker compose -f D:\xs\ros2\docker\docker-compose.yml up -d
# 状态
docker compose -f D:\xs\ros2\docker\docker-compose.yml ps
# 进开发终端
docker exec -it ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && exec bash"
# 一次性跑命令
docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon build --packages-select py_pubsub"
# 关
docker compose -f D:\xs\ros2\docker\docker-compose.yml down
# 重启
docker compose -f D:\xs\ros2\docker\docker-compose.yml restart
# 删容器(保留镜像)
docker compose -f D:\xs\ros2\docker\docker-compose.yml down
# 删镜像
docker rmi ros2-humble-dev:latest
# 看日志
docker logs -f ros2_dev
```
---
## 5. 一键启动脚本
### 5.1 start.ps1 (Windows)
```powershell
powershell D:\xs\ros2\start.ps1
```
等价于:
```powershell
docker compose build # 首次 5-10min
docker compose up -d
docker exec ros2_dev bash -lc "cd /root/ros2_ws && bash build.sh"
docker exec -it ros2_dev bash -lc "source install/setup.bash && exec bash"
```
### 5.2 start.sh (Linux/macOS)
```bash
./start.sh
```
### 5.3 build.sh (容器内)
```bash
bash build.sh
# 1) source /opt/ros/humble/setup.bash
# 2) colcon build --symlink-install --packages-select <all>
# 3) ls install/
# 4) 打印运行命令速查
```
---
## 6. 调试技巧
### 6.1 看容器日志
```bash
docker logs ros2_dev
docker logs -f ros2_dev # 持续
```
### 6.2 进容器交互 shell
```bash
docker exec -it ros2_dev bash
# 进入后:
source /opt/ros/humble/setup.bash
cd /root/ros2_ws
source install/setup.bash
```
### 6.3 在容器内单次跑命令
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 node list"
```
### 6.4 看 DDS 流量
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 doctor --report"
```
### 6.5 看网络
```bash
docker exec ros2_dev bash -lc "ip addr; ip route"
```
### 6.6 容器与宿主机共享 GPU(可选)
```yaml
services:
ros2:
deploy:
resources:
reservations:
devices:
- driver: nvidia
count: 1
capabilities: [gpu]
```
---
## 7. 常见坑
### 7.1 容器启动后找不到 ros2 命令
容器里 `/opt/ros/humble/setup.bash` 没 source。每个 shell 都要 source,
或者写进 `~/.bashrc`(已加,新 bash 自动 source)。
### 7.2 改了源码但 colcon 看不到
确认 `--symlink-install`(默认配置里加上了)。否则需要重 build。
### 7.3 跨容器看不到节点
- 同一 ROS Domain(`ROS_DOMAIN_ID`)
- host network(本仓库用了)
- multicast 没被拦截
### 7.4 Windows bind mount 文件锁
有时 LSP / IDE 持文件锁,导致容器内 colcon 失败。关掉 IDE 或在容器内编辑。
### 7.5 容器启动很慢
- Docker Desktop 未运行
- WSL2 后端未启用
- 镜像太大(本仓库 ~3GB 正常)
### 7.6 容器 OOM
Docker Desktop → Settings → Resources → Memory 调到 ≥ 4GB。
---
## 8. 多机 / 多机器人扩展
```yaml
# docker-compose.yml 改成:
services:
robot1:
image: ros2-humble-dev:latest
container_name: ros2_robot1
network_mode: host
environment:
- ROS_DOMAIN_ID=42
- ROS_NAMESPACE=robot1
volumes:
- ./robot1_conf:/root/ros2_ws
robot2:
image: ros2-humble-dev:latest
container_name: ros2_robot2
network_mode: host
environment:
- ROS_DOMAIN_ID=42
- ROS_NAMESPACE=robot2
volumes:
- ./robot2_conf:/root/ros2_ws
central:
image: ros2-humble-dev:latest
container_name: ros2_central
network_mode: host
environment:
- ROS_DOMAIN_ID=42
command: ["bash", "-lc", "ros2 launch bringup multi_robot_launch.py"]
```
每个 container 各跑一组节点,`ROS_NAMESPACE` 隔离命名空间,
`ROS_DOMAIN_ID` 共享同一总线 → 跨容器通讯。
详见 [`doc/100-embedded-deployment.md`](100-embedded-deployment.md)。
---
## 9. 在本仓库里跑
### 9.1 一键启
```powershell
powershell D:\xs\ros2\start.ps1
```
### 9.2 进入后跑测试
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && cd /root/ros2_ws && colcon test --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup"
```
### 9.3 跑 demo
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup full_demo_launch.py"
```
---
## 10. 进阶:多阶段构建 + 镜像瘦身
### 10.1 当前镜像大小
```bash
docker images ros2-humble-dev
# SIZE: 3.4GB
```
来源:
- `osrf/ros:humble-desktop`: ~2.1GB(包含 RViz / rqt / Gazebo)
- 我们的 apt 包 + colcon: ~1.3GB
### 10.2 改用 `osrf/ros:humble-ros-base`
基础镜像改成 `humble-ros-base`(无 RViz / rqt):
```dockerfile
FROM osrf/ros:humble-ros-base
```
**SIZE 减少 ~1.5GB**。但少了 RViz / rqt / Gazebo,如果需要这些再加 apt。
### 10.3 多阶段构建(进一步瘦身)
```dockerfile
# Stage 1: build
FROM osrf/ros:humble-ros-base AS builder
# ... 装 build 工具,build 我们的包 ...
# Stage 2: runtime
FROM osrf/ros:humble-ros-base
COPY --from=builder /root/ros2_ws/install /root/ros2_ws/install
# 不带 build 工具,更小
```
适合 CI 流水线 + 生产环境。
---
## 接下来读
| 主题 | 文档 |
|---|---|
| venv 工作流 | [`02-virtualenv.md`](02-virtualenv.md) |
| 测试策略 | [`90-testing.md`](90-testing.md) |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
| 项目总览 | [`00-overview.md`](00-overview.md) |
doc/90-testing.md
+457
View File
@@ -0,0 +1,457 @@
# 90 · 测试策略(完全指南)
> **目标**:理解 ROS2 三层测试金字塔,会用 gtest / pytest / colcon test,能把本仓库的测试策略用到自己的项目。
---
## 目录
- [1. 测试金字塔](#1-测试金字塔)
- [2. C++ gtest 完整指南](#2-c-gtest-完整指南)
- [3. Python pytest 完整指南](#3-python-pytest-完整指南)
- [4. launch_testing(集成测试 launch 文件)](#4-launch_testing集成测试-launch-文件)
- [5. 本仓库测试策略详解](#5-本仓库测试策略详解)
- [6. 端到端自动化(进阶)](#6-端到端自动化进阶)
- [7. 标记 / 跳过 / 覆盖率](#7-标记--跳过--覆盖率)
- [8. CI 集成(GitHub Actions)](#8-ci-集成github-actions)
- [9. 在本仓库里跑](#9-在本仓库里跑)
- [10. 进阶:Mock DDS / 时间注入 / Fault Injection](#10-进阶mock-dds--时间注入--fault-injection)
---
## 1. 测试金字塔
```
┌────────────────┐
│ 端到端 E2E │ ← launch_testing + 真实 launch
│ (慢 / 1-2 个) │
├────────────────┤
│ 集成 in-process │ ← 同进程 spin + DDS 单跑
│ (中等 / 4-6) │
├────────────────┤
│ 单元 Unit │ ← gtest / pytest 单函数
│ (快 / 大量) │
└────────────────┘
```
| 层 | 跑在哪 | 速度 | 数量 |
|---|---|---|---|
| 单元 | 容器内 / 本机 venv | 秒级 | 几十 ~ 几百 |
| 集成 in-process | 容器内(colcon test) | 秒 ~ 分钟 | 几个 ~ 几十 |
| 端到端 E2E | launch + 真实 DDS | 分钟级 | 1-5 个核心场景 |
**本仓库**:
- 单元 + 集成:`colcon test` 一键跑(10 用例,秒级)
- 端到端:固化 5 个 `docker/*_e2e.log` 手动验证
---
## 2. C++ gtest 完整指南
### 2.1 测试文件
```cpp
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
class PubsubTest : public ::testing::Test {
protected:
static void SetUpTestSuite() {
rclcpp::init(0, nullptr); // 同进程共享一个 context
}
static void TearDownTestSuite() {
rclcpp::shutdown();
}
};
TEST_F(PubsubTest, MessageDataFieldIsNonEmpty) {
std_msgs::msg::String msg;
msg.data = "hi";
EXPECT_FALSE(msg.data.empty());
EXPECT_EQ(msg.data, "hi");
}
TEST_F(PubsubTest, PublisherOnce) {
auto node = std::make_shared<rclcpp::Node>("test_node");
auto pub = node->create_publisher<std_msgs::msg::String>("chatter", 10);
auto msg = std_msgs::msg::String();
msg.data = "unit-test";
pub->publish(msg);
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
exec->add_node(node);
end = std::chrono::steady_clock::now() + std::chrono::seconds(1);
while (std::chrono::steady_clock::now() < end) {
exec->spin_some(50ms);
}
SUCCEED();
}
```
### 2.2 CMakeLists.txt 集成
```cmake
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(test_pub_sub test/test_pub_sub.cpp)
ament_target_dependencies(test_pub_sub rclcpp std_msgs)
endif()
```
### 2.3 跑测试
```bash
colcon build --packages-select cpp_pubsub
colcon test --packages-select cpp_pubsub
# 看详细
cat build/cpp_pubsub/test_results/cpp_pubsub/test_pub_sub.gtest.xml
```
### 2.4 关键 API
| API | 用途 |
|---|---|
| `TEST(suite, name)` | 测试用例 |
| `TEST_F(FixtureName, name)` | 用 fixture |
| `EXPECT_*`(非致命) / `ASSERT_*`(致命) | 断言 |
| `SetUp() / TearDown()` | 每个用例前后 |
| `SetUpTestSuite() / TearDownTestSuite()` | 所有用例前后 |
---
## 3. Python pytest 完整指南
### 3.1 测试文件
```python
import rclpy
import pytest
from py_pubsub.publisher_member_function import Talker
@pytest.fixture(scope='module')
def ros_context():
"""rclpy 是进程级单例,模块前 init,完成后 shutdown。"""
rclpy.init()
yield
rclpy.shutdown()
def test_talker_init(ros_context):
node = Talker()
assert node.get_name() == 'talker_py'
def test_inproc_roundtrip(ros_context):
"""同进程 spin Talker + 自建 subscriber,验证消息流。"""
import time
from sensor_msgs.msg import String
talker = Talker()
received = []
sub_node = rclpy.node.Node('test_subscriber')
sub_node.create_subscription(
String, 'chatter',
lambda msg: received.append(msg.data), 10)
exec_ = rclpy.executors.SingleThreadedExecutor()
exec_.add_node(talker)
exec_.add_node(sub_node)
end = time.time() + 1.5
while time.time() < end:
exec_.spin_once(timeout_sec=0.05)
assert any('Hello from PY' in s for s in received)
```
### 3.2 同进程 spin(关键模式)
```python
exec_ = rclpy.executors.SingleThreadedExecutor()
exec_.add_node(talker)
exec_.add_node(listener)
end = time.time() + timeout_sec
while time.time() < end:
exec_.spin_once(timeout_sec=0.05)
```
**`spin_once(timeout)`** 在主循环里手动驱动事件循环,**比 `rclpy.spin()` 易控制超时**。
### 3.3 pytest 标记(marks)
```python
@pytest.mark.ros # 需要 ROS2 环境
@pytest.mark.inproc # 同进程可跑
@pytest.mark.slow # 跑得慢
```
CLI:
```bash
pytest -m "not slow"
pytest -m ros and not inproc
```
### 3.4 pytest 配置(pyproject.toml)
本仓库已在 `pyproject.toml` 配:
```toml
[tool.pytest.ini_options]
markers = [
"ros: 需要 ROS2 运行环境",
"inproc: 同进程内 spin",
]
```
---
## 4. launch_testing(集成测试 launch 文件)
```python
import launch
import launch_ros
import launch_testing
import launch_testing.actions
import pytest
@pytest.mark.launch_test
def generate_test_description():
return launch.LaunchDescription([
launch_ros.actions.Node(
package='py_pubsub',
executable='talker',
parameters=[{'period_ms': 100, 'topic': 'chatter_test'}]),
launch_ros.actions.Node(
package='py_pubsub',
executable='listener',
parameters=[{'topic': 'chatter_test'}]),
launch_testing.actions.ReadyToTest(),
])
class TestPubSub(unittest.TestCase):
def test_message_received(self, proc_output):
assert proc_output.assertWaitFor(
'recv:', timeout=5, stream='stdout'
)
```
**注意**: launch_testing 在 colcon test 环境下与节点 rclpy.shutdown() 二次调用可能冲突,本仓库改用更稳的 in-process spin。
---
## 5. 本仓库测试策略详解
### 5.1 单元 + 集成 in-process(主测试)
**Python 包** 用 pytest + in-process spin:
```python
@pytest.fixture(scope='module')
def ros_context():
rclpy.init()
yield
rclpy.shutdown()
def test_xxx(ros_context):
server = SomeServer()
client = SomeClient()
exec_ = rclpy.executors.SingleThreadedExecutor()
exec_.add_node(server); exec_.add_node(client)
# ... 触发 + 验证
```
**C++ 包** 用 gtest + ament_add_gtest:
```cpp
TEST_F(MyFixture, SpinSomeWorks) {
auto node = std::make_shared<MyNode>();
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
exec->add_node(node);
exec->spin_some(50ms);
}
```
### 5.2 端到端(固化日志,手工验证)
5 个核心场景,跑一遍固化到 `docker/`:
- `bringup_e2e.log`:Topic 4 节点跨包跨语言
- `srv_e2e.log`:Service 12+30=42
- `robot_e2e.log`:URDF + TF 实时打印
- `vision_e2e.log`:Image 流
- `full_demo_e2e.log`:11 节点一起
每次大改动后重跑 + 替换日志。
### 5.3 测试覆盖现状
| 包 | 测试 | 用例数 | 通过率 |
|---|---|---|---|
| py_pubsub | pytest + in-process | 4/4 | 100% |
| cpp_pubsub | gtest | 2/2 | 100% |
| py_srv | pytest + Service | 1/1 | 100% |
| py_action_demo | pytest + Action | 1/1 | 100% |
| cpp_robot_tf2 | gtest + TF2 | 2/2 | 100% |
| py_vision_demo | pytest + Image | 2/2 | 100% |
| bringup | launch 6 文件就绪 | OK | 100% |
**合计: 10/10 测试 100% PASSED**。
---
## 6. 端到端自动化(进阶)
```python
@pytest.mark.launch_test
def generate_test_description():
return launch.LaunchDescription([
IncludeLaunchDescription(
PythonLaunchDescriptionSource(<bringup>/launch/full_demo_launch.py)),
launch_testing.actions.ReadyToTest(),
])
class TestFullDemo(unittest.TestCase):
def test_all_nodes_running(self):
# 跑 ros2 node list 子进程,看 11 个节点
...
```
详见 [`20-topics.md`](20-topics.md) §11.3 录包回放。
---
## 7. 标记 / 跳过 / 覆盖率
### 7.1 标记
```python
@pytest.mark.skip(reason="硬件没到位")
def test_xxx():
...
@pytest.mark.skipif(sys.platform == 'win32', reason="Windows 跑不动")
def test_yyy():
...
@pytest.mark.xfail(reason="已知 bug") # 标记预期失败
def test_zzz():
...
```
### 7.2 覆盖率
```bash
# Python
pytest --cov=py_pubsub --cov-report=html src/py_pubsub/test
# 输出 build/htmlcov/index.html
```
```bash
# C++(用 lcov)
sudo apt install lcov
cd build/cpp_pubsub && lcov --capture --directory . --output-file coverage.info
genhtml coverage.info --output-directory coverage_html
```
---
## 8. CI 集成(GitHub Actions)
```yaml
# .github/workflows/test.yml
name: test
on: [push, pull_request]
jobs:
test:
runs-on: ubuntu-22.04
steps:
- uses: actions/checkout@v4
- name: Build image & test
run: |
docker compose -f docker/docker-compose.yml build
docker compose -f docker/docker-compose.yml up -d
docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon build --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup"
docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon test --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup"
```
---
## 9. 在本仓库里跑
### 9.1 跑全部测试
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && cd /root/ros2_ws && colcon test --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup"
```
### 9.2 单包测试
```bash
docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon test --packages-select cpp_pubsub"
```
### 9.3 看测试日志
```bash
# gtest
cat build/cpp_pubsub/test_results/cpp_pubsub/test_pub_sub.gtest.xml
# pytest
cat build/py_pubsub/pytest.xml
```
### 9.4 当前结果(固化)
```
✅ cpp_pubsub: 2/2 PASSED
✅ py_pubsub: 4/4 PASSED
✅ py_srv: 1/1 PASSED
✅ py_action_demo: 1/1 PASSED
✅ cpp_robot_tf2: 2/2 PASSED
✅ py_vision_demo: 2/2 PASSED
✅ bringup: OK (no tests)
合计: 10/10 PASSED
```
---
## 10. 进阶:Mock DDS / 时间注入 / Fault Injection
### 10.1 Mock DDS(测试纯逻辑)
```python
# 不起 ROS2 上下文,直接测算法
def test_message_parser():
raw = b'\x00\x01hello'
parsed = MyParser.parse(raw)
assert parsed == 'hello'
```
### 10.2 时间注入
```cpp
// C++: 用 Clock 抽象 + RosTime mock
node->set_parameter(rclcpp::Parameter("use_sim_time", true));
// 然后通过 /clock topic 推时间
```
### 10.3 Fault Injection
```python
# 杀掉 server 模拟断网
subprocess.run(['pkill', '-9', '-f', 'add_two_ints_server'])
# 看 client 怎么处理
# 慢响应模拟
# 修改 server callback sleep 时间
```
---
## 接下来读
| 主题 | 文档 |
|---|---|
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
| 具身智能路径 | [`99-embodied-ai.md`](99-embodied-ai.md) |
| Docker 开发 | [`85-docker.md`](85-docker.md) |
doc/99-embodied-ai.md
+578
View File
@@ -0,0 +1,578 @@
# 99 · 入门具身智能:从 ROS2 到 VLA / 机器人(完全学习路线)
> **目标**:为"想用本仓库入门具身智能"的开发者,画一张**清晰、可执行、有时间估算**的学习路线图。
> 读完这篇,你就知道"接下来该装什么、写什么、跑什么"。
---
## 目录
- [0. 你现在的位置](#0-你现在的位置)
- [1. ROS2 全栈知识点地图](#1-ros2-全栈知识点地图)
- [2. 5 个学习阶段(完全路线)](#2-5-个学习阶段完全路线)
- [3. 阶段 1: 通信 + TF + 视觉(本仓库已完)](#3-阶段-1-通信--tf--视觉本仓库已完)
- [4. 阶段 2: 仿真机械臂(ros2_control + MoveIt2)](#4-阶段-2-仿真机械臂ros2_control--moveit2)
- [5. 阶段 3: 物理仿真(Gazebo / ros_gz)](#5-阶段-3-物理仿真gazebo--ros_gz)
- [6. 阶段 4: 视觉 + 抓取(GraspNet / 6-DoF)](#6-阶段-4-视觉--抓取graspnet--6-dof)
- [7. 阶段 5: 真机 + VLA(OpenVLA / Pi0)](#7-阶段-5-真机--vlaopenvla--pi0)
- [8. 3 个实战项目模板(从本仓库出发)](#8-3-个实战项目模板从本仓库出发)
- [9. 学习资源 + 时间表](#9-学习资源--时间表)
- [10. FAQ](#10-faq)
---
## 0. 你现在的位置
### 已掌握 ✅(本仓库覆盖)
- ROS2 通信:Topic / Service / Action / Parameter
- TF2 坐标变换 + URDF 描述
- sensor_msgs/Image + cv_bridge
- JointState 发布(模拟)
- launch 文件 + 测试金字塔
- Docker 容器化开发
- 跨包跨语言互通
### 还没掌握 ❌(缺口)
- ros2_control 硬件驱动抽象层
- MoveIt2 运动规划(IK / path / collision)
- Gazebo / ros_gz 物理仿真
- 真实机械臂 URDF(6-7 DoF)+ Gripper
- 手眼标定(eye-in-hand / eye-to-hand)
- Cartesion path planning
- 力控 / Force-Torque sensor
- 真实电机驱动(Bus Servo / EtherCAT)
- GraspNet / 6-DoF 抓取
- VLA 模型接入(OpenVLA / Pi0)
- 移动底盘(Nav2 / SLAM)
- 多机协调 / ROS2 多机器人
---
## 1. ROS2 全栈知识点地图
```
ROS2 全栈
│
┌───────────────────────────┼───────────────────────────┐
│ │ │
通信层(本仓库) 机器人层 应用层
│ │ │
┌─────┼─────┐ ┌───────┼───────┐ ┌───────┼───────┐
│ │ │ │ │ │ │ │ │
Topic Srv Action URDF ros2_ctrl MoveIt2 Nav2 VLA Gazebo
│ │ │ │ │ │ │ │ │
└─────┴─────┘ └───────┴───────┘ └───────┴───────┘
│ │ │
7 包已覆盖 需要继续补 需要仿真/真机
```
---
## 2. 5 个学习阶段(完全路线)
```
本仓库 ──── 阶段 1 (本仓库) ──── 阶段 2 (仿真机械臂) ──── 阶段 3 (Gazebo)
│ │
│ ▼
│ 阶段 4 (视觉抓取)
│ │
│ ▼
└─────────────────────────────────────────────────── 阶段 5 (真机+VLA)
```
| 阶段 | 主题 | 工作量 | 学习产出 |
|---|---|---|---|
| **1. 通信 + TF + 视觉** | 本仓库 | 已完成 | ROS2 全栈基础 |
| **2. 仿真机械臂** | ros2_control + MoveIt2 + 6-DoF URDF | 2-3 周 | 能用 MoveIt2 算关节轨迹 |
| **3. 物理仿真** | Gazebo + ros_gz | 1-2 周 | 离线测试机械臂 / 视觉 |
| **4. 视觉 + 抓取** | GraspNet / 6-DoF + 手眼标定 | 3-4 周 | "看到 → 抓" 端到端 |
| **5. 真机 + VLA** | OpenVLA / Pi0 + ros2_control + 真电机 | 4-8 周 | 具身智能 MVP |
**总计: 3-5 个月全职工作量**(取决于每周投入时间)。
---
## 3. 阶段 1: 通信 + TF + 视觉(本仓库已完)
### 验证清单
- [x] Topic pub/sub 跨语言互通(听 cpp 收 py / 听 py 收 cpp)
- [x] Service 同步 req/resp(12+30=42 验证)
- [x] Action 三件套(Fibonacci 验证)
- [x] TF 坐标变换(gripper 在 base_link 下位置)
- [x] Image + cv_bridge(图像流验证)
- [x] 10/10 测试 100% 通过
### 你应该已经能
- 写 Topic pub/sub 节点(单语言)
- 写 launch 文件
- 用 ros2 CLI 调试
- 理解 DDS 自动发现
- 跑 colcon build + test
---
## 4. 阶段 2: 仿真机械臂(ros2_control + MoveIt2)
### 4.1 安装清单
```bash
# PC 上(本机)
sudo apt install -y \
ros-humble-ros2-control \
ros-humble-ros2-controllers \
ros-humble-moveit \
ros-humble-moveit-resources \
ros-humble-moveit-servo \
ros-humble-moveit-ros-planners
```
预计下载 ~1GB。
### 4.2 写 6-DoF 机械臂 URDF
本仓库的 `cpp_robot_tf2/urdf/simple_arm.urdf` 是 3 关节玩具臂。
升级步骤:
1. 找一款**开源 6-DoF 机械臂 URDF**(推荐):
- [`panda_arm.urdf`](https://github.com/ros-planning/moveit_resources/tree/gh-pages/panda) - Franka Panda
- [`ur5.urdf`](https://github.com/ros-industrial/universal_robot/tree/ros2-devel) - UR5
- [`xarm6.urdf`](https://github.com/xArm-Robotics/xarm_ros2) - Ufactory xArm 6
2. 把它拷到 `src/<新包>/urdf/`
3. **加 `<transmission>` 标签**(ros2_control 需要)
4. **加 `<gazebo>` 标签**(后续 Gazebo 用,可选)
5. `check_urdf <urdf>` 校验
URDF 关键改造:
```xml
<!-- 关节加 transmission -->
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="100" velocity="1.0"/>
<!-- ros2_control 需要 -->
<transmission name="trans_joint1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor_joint1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</joint>
```
### 4.3 ros2_control 配置
`src/my_arm_controller/config/my_controllers.yaml`:
```yaml
controller_manager:
ros__parameters:
update_rate: 100
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateController
arm_controller:
type: position_controllers/JointGroupPositionController
arm_controller:
ros__parameters:
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
```
### 4.4 ros2_control launch
`src/my_arm_controller/launch/arm_control.launch.py`:
```python
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
pkg_share = get_package_share_directory('my_arm_controller')
config = os.path.join(pkg_share, 'config', 'my_controllers.yaml')
urdf = os.path.join(pkg_share, 'urdf', 'arm.urdf')
with open(urdf, 'r') as f: urdf_content = f.read()
return LaunchDescription([
Node(package='controller_manager', executable='ros2_control_node',
parameters=[{'robot_description': urdf_content, 'update_rate': 100}],
output='screen'),
Node(package='controller_manager', executable='spawner',
arguments=['joint_state_broadcaster', '-c', '/controller_manager'],
output='screen'),
Node(package='controller_manager', executable='spawner',
arguments=['arm_controller', '-c', '/controller_manager'],
output='screen'),
])
```
### 4.5 MoveIt2 SRDF(从 URDF 生成)
```bash
ros2 launch moveit_setup_assistant setup_assistant.launch.py
```
在 MoveIt Setup Assistant 里:
1. Load URDF
2. Add Virtual Joint(base_link → world)
3. Add Planning Group("arm" 包含 6 个 joint)
4. Add Poses(home / ready / extended)
5. Add End Effector(gripper)
6. Add Passive Joints
7. **Generate** SRDF + MoveIt 配置
### 4.6 MoveIt2 编程
```python
# PC 上: MoveIt2 客户端 → /joint_trajectory → ros2_control → 电机
from moveit.planning import MoveItPy
from moveit.core import PlanningComponent
moveit = MoveItPy(node=your_node)
arm = moveit.get_planning_component("arm")
arm.set_start_state_to_current_state()
arm.set_goal_state_from_pose_stamped(goal_pose)
plan_result = arm.plan()
if plan_result:
arm.execute(plan_result.trajectory, blocking=True)
```
**预期效果**: 在 RViz 点目标位姿 → MoveIt2 算关节轨迹 → ros2_control 执行 → 真实机械臂(或仿真臂)运动。
### 4.7 阶段 2 验证清单
- [ ] ros2_control 6 关节都能 set position
- [ ] MoveIt2 RViz 里 plan + execute 能看到机械臂动
- [ ] 写 Python 节点调用 MoveIt2 API,能指定目标 pose 让臂到达
- [ ] 加 UnitTest 验证 motion plan 正确性
**预计 2-3 周**。完成后你能:"告诉末端去哪个 pose,MoveIt2 算出关节轨迹,ros2_control 执行"。
---
## 5. 阶段 3: 物理仿真(Gazebo / ros_gz)
### 5.1 安装
```bash
sudo apt install -y ros-humble-ros-gz
```
### 5.2 把 URDF 改成 Gazebo 模型
加 `<gazebo>` 标签 + `<transmission>`(已在阶段 2 加)→ gazebo_ros2_control 自动识别。
### 5.3 Gazebo Sim 启动
```bash
ros2 launch ros_gz_sim gz_sim.launch.py gz_args:="-r empty.sdf"
# 加载机械臂
ros2 run ros_gz_sim create -world default \
-file ~/ros2_ws/src/my_arm/urdf/arm.urdf \
-name my_arm \
-x 0 -y 0 -z 1
```
### 5.4 ros_gz_bridge
把 Gazebo 的 Topic 桥到 ROS2:
- `/clock` ←→ `/clock`(仿真时间)
- `/world/default/model/my_arm/joint_state` ← `/joint_states`
- `/cmd_vel` → Gazebo twist controller
### 5.5 仿真 + MoveIt2
```bash
# 终端 1: Gazebo
ros2 launch my_arm gazebo.launch.py
# 终端 2: ros2_control spawner
ros2 launch my_arm ros2_control.launch.py
# 终端 3: MoveIt2
ros2 launch my_arm moveit.launch.py
# 终端 4: 写 Python 脚本,MoveIt2 算轨迹 → 发到 Gazebo → 看可视化
```
**预计 1-2 周**。完成后你能"在仿真里训练 / 验证 / 演示"。
---
## 6. 阶段 4: 视觉 + 抓取(GraspNet / 6-DoF)
### 6.1 硬件升级
- 加 RGB-D 相机(RealSense D435 / Azure Kinect)
- 加力矩传感器(可选)
### 6.2 相机驱动
```bash
sudo apt install -y ros-humble-realsense2-camera
# 或:
sudo apt install -y ros-humble-azure-kinect
```
发布 `/camera/color/image_raw` + `/camera/depth/image_rect_raw` + `/camera/depth/color/points`。
### 6.3 6-DoF GraspNet 集成
```python
# grasp_detector 节点
import rospy
from sensor_msgs.msg import Image, PointCloud2
from geometry_msgs.msg import PoseStamped
class GraspNetNode:
def __init__(self):
self.image_sub = rospy.Subscriber('/camera/color/image_raw', Image, self.image_cb)
self.cloud_sub = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.cloud_cb)
self.grasp_pub = rospy.Publisher('/grasp_pose', PoseStamped, queue_size=10)
def image_cb(self, msg):
# 1) 用 GraspNet 模型推理
# 2) 输出 6-DoF grasp pose
# 3) publish /grasp_pose
pass
```
### 6.4 手眼标定(eye-in-hand)
```bash
sudo apt install -y ros-humble-easy-handeye
# 标定流程:
# 1) 启动相机 + 标定板 + MoveIt2
# 2) 移动机械臂到多个 pose
# 3) easy_handeye 离线计算 camera_link → gripper 静态 TF
# 4) 把 TF 写进 URDF
```
### 6.5 端到端抓取
```
vision_node (YOLO/GraspNet)
↓ /grasp_pose
grasp_planner (MoveIt2 算轨迹)
↓ /joint_trajectory
ros2_control (PID 闭环)
↓
电机 → 真实 / 仿真机械臂 → 抓取
```
### 6.6 阶段 4 验证
- [ ] 摄像头能 publish 图像 + 点云
- [ ] GraspNet 能输出 6-DoF grasp pose
- [ ] MoveIt2 根据 grasp pose 算出无碰撞轨迹
- [ ] 仿真里端到端跑通"看到物体 → grasp → 抓取"
**预计 3-4 周**。完成后你能做"6-DoF 抓取 demo"。
---
## 7. 阶段 5: 真机 + VLA(OpenVLA / Pi0)
### 7.1 VLA 是什么
**VLA = Vision-Language-Action**,即"看图 + 听指令 → 输出动作"的端到端模型:
- OpenVLA(7B 参数)
- RT-2(Google)
- Pi0(Physical Intelligence)
- Octo
### 7.2 VLA 在 ROS2 里的角色
```
RGB Image + "Pick up the apple" → VLA Model
↓
7-DoF Joint Trajectory
↓
ros2_control
↓
真电机
```
### 7.3 集成示例
```python
# vla_inference_node
import torch
from vla_model import OpenVLA # 或 Pi0
from sensor_msgs.msg import Image
from trajectory_msgs.msg import JointTrajectory
class VLANode:
def __init__(self):
self.model = OpenVLA.from_pretrained("openvla-7b")
self.image_sub = rospy.Subscriber('/camera/color/image_raw', Image, self.cb)
self.cmd_pub = rospy.Publisher('/joint_trajectory', JointTrajectory, queue_size=10)
def cb(self, msg):
image = self.bridge.imgmsg_to_cv2(msg, 'rgb8')
instruction = rospy.get_param('~instruction', 'pick up the apple')
action = self.model.predict(image, instruction)
self.cmd_pub.publish(self.action_to_trajectory(action))
```
### 7.4 真机部署清单
| 项 | 推荐 |
|---|---|
| 6-DoF 机械臂 | Ufactory xArm 6 / Franka Panda(二手) / 国产 6-DoF |
| 相机 | Intel RealSense D435i(深度 + RGB) |
| 控制器 | ROS2 + ros2_control(已学) |
| 主机 | PC / RDK X5(NPU 加速 VLA) |
| 网络 | 千兆 LAN(DDS + 视觉流) |
### 7.5 阶段 5 验证
- [ ] VLA 模型加载到 PC / RDK X5
- [ ] 输入图像 + 指令 → 输出关节轨迹
- [ ] 真实机械臂(或者 Gazebo)执行轨迹
- [ ] 抓取成功率 > 50%(MVP)
**预计 4-8 周**。
---
## 8. 3 个实战项目模板(从本仓库出发)
### 项目 A:桌面级 6-DoF 机械臂抓取
**目标**: 给定桌面物体的 6-DoF 位姿,机械臂抓起来放到指定位置。
```
栈:
- 本仓库(通信 + TF + 视觉)
+ 6-DoF URDF + ros2_control
+ MoveIt2 规划
+ GraspNet(可选,先用固定位姿也行)
+ Gazebo 仿真
```
**改造点**:
- `src/cpp_robot_tf2/urdf/simple_arm.urdf` → 替换为 6-DoF URDF
- 新增 `src/<your_arm>/config/controllers.yaml`
- 新增 `src/<your_arm>/launch/<arm>_bringup.launch.py`
- 写 `pick_and_place` Python 节点
### 项目 B:差速底盘 + OpenVLA 抓桌面物体
**目标**: 移动底盘 + 机械臂 + VLA,自主导航到目标 + 抓取。
```
栈:
- 本仓库
+ ros2_control (底盘 diff_drive_controller + 机械臂 controller)
+ Nav2 (底盘导航)
+ MoveIt2 (机械臂)
+ OpenVLA / Pi0 (高层策略)
```
**改造点**: 在项目 A 基础上 + 底盘 URDF + Nav2 配置 + VLA 模型集成。
### 项目 C:ALOHA 双臂遥操 + Pi0 学习
**目标**: 双机械臂 ALOHA 配置 + 遥操 + Pi0 fine-tune + 部署。
```
栈:
- 本仓库
+ 双手臂 6+6 DoF
+ Gazebo + ALOHA 模型
+ Pi0 训练数据收集(bag 录制遥操)
+ Pi0 fine-tune + 部署
```
**改造点**: 在项目 A + 镜像配置 + 双臂 URDF + 数据收集脚本。
---
## 9. 学习资源 + 时间表
### 9.1 官方文档
- [ROS2 Humble Docs](https://docs.ros.org/en/humble/) - API + tutorials
- [REP 103](https://www.ros.org/reps/rep-0103.html) - 坐标约定
- [REP 105](https://www.ros.org/reps/rep-0105.html) - TF 语义
- [REP 2002](https://www.ros.org/reps/rep-2002.html) - ROS2 设计
### 9.2 中文社区
- [鱼香 ROS](https://fishros.org/) - 入门 + 工具链最友好
- [古月居](https://www.guyuehome.com/) - ROS2 实战
- [Autolabor](https://www.autolabor.com.cn/) - 仿真教程
### 9.3 开源项目
- [ros2_control_demos](https://github.com/ros-controls/ros2_control_demos) - 完整 demo
- [moveit2_tutorials](https://github.com/moveit/moveit2_tutorials)
- [nav2_tutorials](https://github.com/ros-navigation/navigation2_tutorials)
- [ros_gz](https://github.com/gazebosim/ros_gz) - Gazebo 集成
### 9.4 VLA / 具身智能
- [OpenVLA](https://openvla.github.io/)
- [Pi0](https://physicalintelligence.company/blog/pi0)
- [Octo](https://octo-models.github.io/)
### 9.5 总时间表
| 阶段 | 工作 | 时间(全职) |
|---|---|---|
| 1 | 本仓库 | 已完 |
| 2 | ros2_control + MoveIt2 + 6-DoF | 2-3 周 |
| 3 | Gazebo | 1-2 周 |
| 4 | 视觉 + GraspNet | 3-4 周 |
| 5 | 真机 + VLA | 4-8 周 |
| **总计** | | **3-5 月** |
---
## 10. FAQ
### Q1: 我可以跳阶段吗?
可以,但**不推荐**:
- 跳过阶段 2 直接搞 VLA: VLA 输出关节轨迹,你不理解轨迹,debug 不了
- 跳过阶段 3 直接真机: 真机调试成本高(电机可能撞机)
### Q2: 必须做 Gazebo 仿真吗?
不一定,但**强烈建议**:
- 没有仿真 = 直接真机,出错成本高
- Gazebo = 离线验证,出问题不伤人
### Q3: VLA 模型有多大?
OpenVLA 7B 约 15GB,需要 GPU(NVIDIA RTX 3090+)。
轻量方案: OpenVLA-OFT,Pi0-Lite,LLaRA 等。
### Q4: 我只有 1 个 RK3506,够吗?
可以做,但建议至少:
- 1 个 RK3506: 电机控制
- 1 个 RDK X5: VLA 推理(NPU)
- 1 个 PC: 决策 + 可视化
### Q5: 真机能直接用本仓库的代码吗?
**部分可以**:
- ✅ Topic / Service / Action / TF 通信: 直接用
- ❌ JointState 发布(`cpp_robot_tf2`): 是模拟的,要改读编码器
- ❌ MoveIt2 / ros2_control: 本仓库没,需要补
---
## 下一步
按你**当前状态**选:
| 你现在的状态 | 推荐 |
|---|---|
| 学完本仓库,想做机器人 | → 阶段 2(ros2_control + MoveIt2) |
| 想做 VLA | → 阶段 2(先把硬件层打通)→ 阶段 5 |
| 只想做软件/规划算法 | → 阶段 2 + 3(全程仿真,不上硬件) |
| 想做移动底盘 | → 阶段 2(底盘) + 阶段 5(Nav2) |
每阶段做完后,**回到本仓库做一次 colcon test 验证 ROS2 通信基础仍然稳**。
加油,具身智能之旅! 🚀