ROS2_learn/doc/99-embodied-ai.md

# 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 通信基础仍然稳**。

加油,具身智能之旅! 🚀

---

---

## 📖 阅读路径导航

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

- ⏮ **上一篇**: [测试金字塔](90-testing.md)
- ⏭ **下一篇**: [三机部署实操](100-embedded-deployment.md)