# 完整启动流程文档

## 系统架构

```
┌─────────────────┐
│   rm_vision     │ 视觉节点
│  (GimbalROS)    │
└────────┬────────┘
         │ /gimbal/vision (DataAI)
         │ 云台控制指令
         ↓
┌─────────────────┐
│ rm_serial_driver│ 串口驱动
└────────┬────────┘
         │ 串口
         ↓
┌─────────────────┐
│      MCU        │ 下位机
└─────────────────┘
         ↑
         │ /data_mcu (DataMCU)
         │ 云台状态反馈
         │
┌─────────────────┐
│  rm_simple_move │ 导航节点
└─────────────────┘
         │ /cmd_vel_move (Twist)
         │ 底盘控制指令
         ↓
┌─────────────────┐
│ rm_serial_driver│
└─────────────────┘
```

## 话题映射表

| 话题 | 类型 | 方向 | 说明 |
|------|------|------|------|
| `/data_mcu` | `rm_msgs/DataMCU` | MCU → 上位机 | 云台状态（姿态、弹速等） |
| `/data_ref` | `rm_msgs/DataRef` | MCU → 上位机 | 裁判系统数据 |
| `/gimbal/vision` | `rm_msgs/DataAI` | 视觉 → MCU | 视觉云台控制指令 |
| `/cmd_vel_move` | `geometry_msgs/Twist` | 导航 → MCU | 底盘运动控制指令 |
| `/nav_goal` | `rm_msgs/NavGoal` | 决策 → 导航 | 导航目标点 |
| `/nav_status` | `rm_msgs/NavStatus` | 导航 → 决策 | 导航状态反馈 |
| `/current_pose` | `geometry_msgs/PoseStamped` | 导航 → 决策 | 当前位置 |

## 完整启动流程

### 步骤 1: 编译所有包

```bash
cd ~/MOVE_AI

# 编译消息包
colcon build --packages-select rm_msgs

# 编译串口驱动
colcon build --packages-select rm_serial_driver

# 编译导航节点
colcon build --packages-select rm_simpal_move

# 编译视觉（如果需要）
colcon build --packages-select mr_vision

# 加载环境
source install/setup.bash
```

### 步骤 2: 启动串口驱动（核心节点）

**终端 1：启动 rm_serial_driver**

```bash
source install/setup.bash
ros2 run rm_serial_driver rm_serial_driver --ros-args \
  -p device_name:=/dev/ttyACM0 \
  -p baud_rate:=115200
```

**预期输出：**
```
========================================
RM Serial Driver 启动中...
  设备: /dev/ttyACM0
  波特率: 115200
========================================
话题配置:
  发布: /data_mcu (MCU状态)
  发布: /data_ref (裁判系统)
  订阅: /gimbal/vision (视觉控制)
  订阅: /cmd_vel_move (导航控制)
✓ 串口打开成功
RM Serial Driver 启动完成！
```

**故障排查：**
```bash
# 如果串口打开失败，检查设备
ls -l /dev/ttyACM*
ls -l /dev/ttyUSB*

# 添加串口权限
sudo usermod -a -G dialout $USER
# 重新登录生效

# 或临时授权
sudo chmod 666 /dev/ttyACM0
```

### 步骤 3: 验证串口通信

**终端 2：查看 MCU 数据**

```bash
source install/setup.bash
ros2 topic echo /data_mcu
```

**预期输出：**
```yaml
mode: 0
q0: 1.0
q1: 0.0
q2: 0.0
q3: 0.0
yaw: 0.0
yaw_vel: 0.0
pitch: 0.0
pitch_vel: 0.0
bullet_speed: 15.0
bullet_count: 0
---
```

如果能看到数据，说明串口通信正常！

### 步骤 4: 启动导航节点（可选）

**终端 3：启动 rm_simple_move**

```bash
source install/setup.bash
ros2 launch rm_simpal_move simple_move.launch.py
```

**预期输出：**
```
[rm_simple_move]: 话题配置:
[rm_simple_move]:   输入: /nav_goal
[rm_simple_move]:   输出: /cmd_vel_move (统一输出)
[rm_simple_move]:   PID模式: 直接计算发布到 /cmd_vel_move
[rm_simple_move]:   NAV模式: Action /navigate_to_pose → 导航 → /cmd_vel → 转发到 /cmd_vel_move
[rm_simple_move]:   状态: /nav_status, /current_pose
[rm_simple_move]: RMSimpleMove 启动
```

### 步骤 5: 启动视觉节点（如果需要）

**终端 4：启动视觉**

```bash
source install/setup.bash

# 方式 1: 使用已修复的 standard_mpc
./build/mr_vision/standard_mpc configs/standard.yaml

# 方式 2: 使用其他视觉节点（需要先修复）
# ./build/mr_vision/balance_infantry_mpc configs/balance_infantry.yaml
```

**预期输出：**
```
[GimbalROS] Initialized with ROS2 topics
[GimbalROS]   Subscribe: /data_mcu
[GimbalROS]   Publish: /gimbal/vision
[GimbalROS] Waiting for first quaternion...
[GimbalROS] First q received.
```

## 测试流程

### 测试 1: 验证话题连接

```bash
# 查看所有话题
ros2 topic list

# 应该看到：
# /data_mcu
# /data_ref
# /gimbal/vision
# /cmd_vel_move
# /nav_goal
# /nav_status
# /current_pose

# 查看话题信息
ros2 topic info /gimbal/vision
ros2 topic info /data_mcu
```

### 测试 2: 测试导航功能

```bash
# PID 模式导航到 (2, 2)
ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
  mode: 0,
  target_x: 2.0,
  target_y: 2.0,
  target_angle: 0.0,
  max_speed: 1.0,
  tolerance: 0.2
}" --once

# 查看导航状态
ros2 topic echo /nav_status

# 查看底盘控制指令
ros2 topic echo /cmd_vel_move
```

### 测试 3: 测试视觉控制（如果视觉节点运行）

```bash
# 查看视觉发送的云台控制指令
ros2 topic echo /gimbal/vision

# 应该看到：
# mode: 0/1/2
# yaw: ...
# pitch: ...
# vx, vy, wz: ...
```

## 最小启动配置

如果只需要测试串口通信和导航：

```bash
# 终端 1: 串口驱动
ros2 run rm_serial_driver rm_serial_driver

# 终端 2: 导航节点
ros2 launch rm_simpal_move simple_move.launch.py

# 终端 3: 发送导航目标
ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{mode: 0, target_x: 1.0, target_y: 1.0, target_angle: 0.0, max_speed: 1.0, tolerance: 0.2}" --once
```

## 完整启动配置（包含视觉）

```bash
# 终端 1: 串口驱动
ros2 run rm_serial_driver rm_serial_driver

# 终端 2: 导航节点
ros2 launch rm_simpal_move simple_move.launch.py

# 终端 3: 视觉节点
./build/mr_vision/standard_mpc configs/standard.yaml

# 终端 4: 监控（可选）
ros2 topic echo /data_mcu
```

## 启动脚本

创建一个启动脚本 `start_all.sh`：

```bash
#!/bin/bash

# 启动所有节点
cd ~/MOVE_AI
source install/setup.bash

# 启动串口驱动（后台）
gnome-terminal -- bash -c "source install/setup.bash && ros2 run rm_serial_driver rm_serial_driver; exec bash"

# 等待串口驱动启动
sleep 2

# 启动导航节点（后台）
gnome-terminal -- bash -c "source install/setup.bash && ros2 launch rm_simpal_move simple_move.launch.py; exec bash"

# 等待导航节点启动
sleep 2

# 启动视觉节点（如果需要）
# gnome-terminal -- bash -c "source install/setup.bash && ./build/mr_vision/standard_mpc configs/standard.yaml; exec bash"

echo "所有节点已启动！"
echo "使用 'ros2 topic list' 查看话题"
echo "使用 'ros2 topic echo /data_mcu' 查看 MCU 数据"
```

使用方法：
```bash
chmod +x start_all.sh
./start_all.sh
```

## 关闭流程

```bash
# 方式 1: 在每个终端按 Ctrl+C

# 方式 2: 使用脚本关闭所有节点
pkill -f rm_serial_driver
pkill -f rm_simple_move
pkill -f standard_mpc
```

## 常见问题

### Q1: 串口打开失败
```bash
# 检查设备
ls -l /dev/ttyACM*

# 添加权限
sudo usermod -a -G dialout $USER
# 重新登录

# 或临时授权
sudo chmod 666 /dev/ttyACM0
```

### Q2: 收不到 /data_mcu 数据
- 检查串口连接是否正常
- 检查 MCU 是否正常运行
- 检查波特率是否匹配（默认 115200）

### Q3: 视觉节点等待四元数超时
- 确保 `rm_serial_driver` 已启动
- 确保 MCU 正在发送数据
- 检查 `/data_mcu` 话题是否有数据

### Q4: 导航不动
- 检查 `/cmd_vel_move` 是否有数据
- 检查 `rm_serial_driver` 是否订阅了 `/cmd_vel_move`
- 检查 TF 变换是否正常

## 调试工具

```bash
# 查看所有节点
ros2 node list

# 查看所有话题
ros2 topic list

# 查看话题频率
ros2 topic hz /data_mcu

# 查看话题信息
ros2 topic info /gimbal/vision

# 录制数据
ros2 bag record -a

# 查看 TF 树
ros2 run tf2_tools view_frames
```

## 性能监控

```bash
# 查看节点资源占用
top -p $(pgrep -f rm_serial_driver)

# 查看话题带宽
ros2 topic bw /data_mcu

# 查看话题延迟
ros2 topic delay /data_mcu
```

## 下一步

1. ✅ 串口驱动正常运行
2. ✅ 导航节点正常运行
3. ⏳ 修复剩余的视觉节点文件
4. ⏳ 测试完整的视觉+导航+串口流程

## 文档

- **使用指南**: `GIMBAL_ROS_GUIDE.md`
- **迁移状态**: `ROS2_MIGRATION_STATUS.md`
- **本文档**: `COMPLETE_STARTUP_GUIDE.md`