# MOVE_AI

青岛理工大学 QUT MOVE 战队 RoboMaster 比赛上位机代码仓库

支持步兵/英雄/无人机、哨兵、雷达等兵种，集成导航、自瞄、决策等功能。

---

## 📋 目录

- [系统架构](#系统架构)
- [功能模块](#功能模块)
- [快速开始](#快速开始)
- [启动流程](#启动流程)
- [话题接口](#话题接口)
- [测试命令](#测试命令)

---

## 🏗️ 系统架构

```
┌─────────────────────────────────────────────────────────────────┐
│                          下位机 (MCU)                            │
│                    (STM32 + 电机 + IMU + 传感器)                 │
└────────────────────────────┬────────────────────────────────────┘
                             │ 串口通信
                             ↓
┌─────────────────────────────────────────────────────────────────┐
│                    rm_serial_driver (串口驱动)                   │
│  ┌──────────────────────────────────────────────────────────┐  │
│  │ 发布: /data_mcu (MCU状态)                                 │  │
│  │       /data_ref (裁判系统数据)                            │  │
│  │       /gimbal/state (云台状态)                            │  │
│  │ 订阅: /data_ai (综合控制指令)                             │  │
│  └──────────────────────────────────────────────────────────┘  │
└───────┬─────────────────────────────────────────────┬───────────┘
        │                                             │
        ↓                                             ↓
┌──────────────────┐                         ┌──────────────────┐
│   rm_vision      │                         │  rm_decision     │
│   (视觉系统)      │                         │  (决策节点)       │
├──────────────────┤                         ├──────────────────┤
│ 订阅:             │                         │ 订阅:             │
│ /gimbal/state    │                         │ /data_mcu        │
│                  │                         │ /data_ref        │
│ 发布:             │                         │ /vision/aim      │
│ /vision/aim ─────┼─────────────────────────→ /nav_status      │
│ (云台控制)        │                         │ /cmd_vel_move    │
└──────────────────┘                         │                  │
                                             │ 发布:             │
┌──────────────────┐                         │ /data_ai ────────┼──┐
│  rm_simple_move  │                         │ /nav_goal        │  │
│  (运动控制)       │                         └────────┬─────────┘  │
├──────────────────┤                                  │            │
│ 订阅:             │                                  │            │
│ /nav_goal ←──────┼──────────────────────────────────┘            │
│                  │                                               │
│ 发布:             │                                               │
│ /cmd_vel_move ───┼───────────────────────────────────────────────┘
│ /nav_status      │
└────────┬─────────┘
         │
         ↓
┌──────────────────┐
│   Nav2 Stack     │
│   (导航系统)      │
├──────────────────┤
│ - 路径规划        │
│ - 避障            │
│ - 定位            │
└──────────────────┘
```

---

## 🔧 功能模块

### 1. **rm_serial_driver** - 串口驱动
- 与下位机 MCU 进行串口通信
- 接收 MCU 状态、裁判系统数据、云台状态
- 发送综合控制指令到 MCU

### 2. **rm_vision** - 视觉系统
- 自瞄识别与跟踪
- 能量机关识别
- 输出云台控制指令（yaw/pitch 角度、速度、加速度）

### 3. **rm_simple_move** - 运动控制
- **PID 模式**：直接速度控制
- **NAV 模式**：调用 Nav2 进行路径规划和导航
- 输出底盘速度指令

### 4. **rm_decision** - 决策节点（核心控制器）
- 整合视觉自瞄控制（云台）
- 整合运动控制指令（底盘）
- 根据裁判系统、导航状态做决策
- 输出统一的 `/data_ai` 控制指令

### 5. **Nav2** - 导航系统
- 全局路径规划
- 局部避障
- SLAM 定位

---

## 🚀 快速开始

### 环境要求
- Ubuntu 22.04
- ROS 2 Humble
- C++17

### 编译
```bash
cd /home/robofish/MOVE_AI
colcon build --symlink-install
source install/setup.bash
```

---

## 📡 启动流程

### 方式一：分步启动（推荐调试）

#### 1️⃣ 启动串口驱动
```bash
ros2 launch rm_serial_driver serial_driver.launch.py
```

#### 2️⃣ 启动导航系统
```bash
ros2 launch rm_nav_bringup nav.launch.py
```

#### 3️⃣ 启动视觉系统

**Sentry 哨兵（推荐）：**
```bash
ros2 launch rm_vision_bringup sentry_vision.launch.py
```

**其他机器人类型：**
```bash
# 标准步兵
ros2 launch rm_vision_bringup vision.launch.py vision_type:=standard_mpc

# 平衡步兵
ros2 launch rm_vision_bringup vision.launch.py vision_type:=balance_infantry_mpc

# 自瞄调试模式
ros2 launch rm_vision_bringup vision.launch.py vision_type:=auto_aim_debug_mpc

# 能量机关调试
ros2 launch rm_vision_bringup vision.launch.py vision_type:=auto_buff_debug_mpc

# 自定义配置文件
ros2 launch rm_vision_bringup vision.launch.py config:=sentry.yaml
```

#### 4️⃣ 启动决策节点
```bash
ros2 launch rm_decision decision.launch.py
```

### 方式二：一键启动（待实现）
```bash
ros2 launch rm_bringup full_system.launch.py
```

---

## 🎯 标定工具

视觉系统提供了完整的标定工具链：

### 相机标定
```bash
# 1. 采集标定图像（按 's' 保存，'q' 退出）
ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=capture

# 2. 标定相机内参
ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_camera

# 3. 手眼标定
ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_handeye

# 4. 机器人-世界手眼标定
ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_robotworld_handeye

# 5. 视频分割工具
ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=split_video
```

**自定义参数：**
```bash
# 指定配置文件和输出目录
ros2 launch rm_vision_bringup calibration.launch.py \
  calibration_type:=capture \
  config:=calibration.yaml \
  output_folder:=my_calibration_data
```

---

## 📊 话题接口

### 核心话题

| 话题名称 | 消息类型 | 发布者 | 订阅者 | 说明 |
|---------|---------|--------|--------|------|
| `/data_mcu` | `rm_msgs/DataMCU` | serial_driver | decision | MCU 状态数据 |
| `/data_ref` | `rm_msgs/DataRef` | serial_driver | decision | 裁判系统数据 |
| `/gimbal/state` | `rm_msgs/GimbalState` | serial_driver | vision | 云台状态（姿态、速度） |
| `/vision/aim` | `rm_msgs/DataAim` | vision | decision | 视觉自瞄控制（仅云台） |
| `/nav_goal` | `rm_msgs/NavGoal` | decision | simple_move | 导航目标 |
| `/nav_status` | `rm_msgs/NavStatus` | simple_move | decision | 导航状态反馈 |
| `/cmd_vel_move` | `geometry_msgs/Twist` | simple_move | decision | 底盘速度指令 |
| `/data_ai` | `rm_msgs/DataAI` | decision | serial_driver | 综合控制指令（云台+底盘） |

### 数据流向

```
视觉控制流:
MCU → /gimbal/state → Vision → /vision/aim → Decision → /data_ai → MCU

底盘控制流:
Decision → /nav_goal → SimpleMove → /cmd_vel_move → Decision → /data_ai → MCU

导航反馈流:
SimpleMove → /nav_status → Decision
```

---

## 🧪 测试命令

### 查看话题列表
```bash
ros2 topic list
```

### 监控关键话题
```bash
# 查看决策输出
ros2 topic echo /data_ai

# 查看底盘速度
ros2 topic echo /cmd_vel_move

# 查看视觉自瞄
ros2 topic echo /vision/aim

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

### 发送导航目标

#### NAV 模式（路径规划）
```bash
ros2 topic pub --once /nav_goal rm_msgs/msg/NavGoal \
  "{mode: 1, target_x: 4.0, target_y: -4.0, target_angle: 0.0, max_speed: 1.0, tolerance: 0.1}"
```

#### PID 模式（直接控制）
```bash
ros2 topic pub --once /nav_goal rm_msgs/msg/NavGoal \
  "{mode: 0, target_x: 1.0, target_y: 0.5, target_angle: 0.0, max_speed: 0.5, tolerance: 0.05}"
```

### 可视化
```bash
# 启动 RViz 查看导航
rviz2 -d $(ros2 pkg prefix rm_bringup)/share/rm_bringup/rviz/nav.rviz

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

---

## 📝 消息定义

### DataAim（视觉自瞄 - 仅云台）
```
uint8 mode              # 0:空闲 1:自瞄 2:小符 3:大符
float32 yaw             # 偏航角
float32 yaw_vel         # 偏航角速度
float32 yaw_acc         # 偏航角加速度
float32 pitch           # 俯仰角
float32 pitch_vel       # 俯仰角速度
float32 pitch_acc       # 俯仰角加速度
```

### DataAI（综合控制 - 云台+底盘）
```
uint8 mode              # 云台模式
float32 yaw, yaw_vel, yaw_acc
float32 pitch, pitch_vel, pitch_acc
float32 vx, vy, wz      # 底盘速度
uint8 reserved          # 底盘模式
```

### NavGoal（导航目标）
```
uint8 MODE_PID = 0      # PID 直接控制
uint8 MODE_NAV = 1      # Nav2 导航
uint8 mode
float32 target_x, target_y, target_angle
float32 max_speed, tolerance
```

### ChassisMode（底盘模式）
```
uint8 MODE_MANUAL = 0   # 手动控制
uint8 MODE_FOLLOW = 1   # 跟随云台
uint8 MODE_SPIN = 2     # 小陀螺
uint8 MODE_NAV = 3      # 导航模式
```

---

## 🐛 调试技巧

### 检查节点状态
```bash
ros2 node list
ros2 node info /rm_decision
```

### 检查话题连接
```bash
ros2 topic info /data_ai
```

### 查看参数
```bash
ros2 param list /rm_decision
ros2 param get /rm_decision enable_aim
```

### 日志级别
```bash
ros2 run rm_decision decision_node --ros-args --log-level debug
```

---

## 📂 项目结构

```
MOVE_AI/
├── src/
│   ├── rm_serial_driver/    # 串口驱动
│   ├── rm_vision/           # 视觉系统
│   ├── rm_nav/              # 导航相关
│   │   └── rm_simple_move/  # 运动控制
│   ├── rm_decision/         # 决策节点
│   ├── rm_msgs/             # 消息定义
│   └── rm_bringup/          # 启动文件
└── README.md
```

---

## 🤝 贡献

欢迎提交 Issue 和 Pull Request！

## 📄 许可证

本项目采用 MIT 许可证。

---

**青岛理工大学 QUT MOVE 战队** | RoboMaster 2026