# GTdrone_ws - ROS2 PX4 无人机集群控制系统

[![ROS2](https://img.shields.io/badge/ROS2-Humble-blue)](https://docs.ros.org/en/humble/)
[![PX4](https://img.shields.io/badge/PX4-v1.14+-orange)](https://px4.io/)
[![Python](https://img.shields.io/badge/Python-3.8+-green)](https://www.python.org/)
[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](LICENSE)

基于 **ROS 2 Humble** 和 **PX4 Autopilot** 的无人机集群控制解决方案，提供从单机控制到多机编队飞行的完整开发框架。

## ✨ 核心特性

### 🚁 飞行控制
- **全自动起飞/降落** - 自动切换 Offboard 模式、解锁、位置控制
- **多坐标系支持** - NED（北东下）、ENU（东北天）、机体坐标系无缝切换
- **精确位置控制** - 支持 ENU 坐标系下的直觉飞行控制
- **多机协同** - 最多支持 50 架无人机并发控制

### 📐 编队飞行
- **内置编队模式** - 一字形、圆形、正方形、三角形编队
- **编队变换** - 支持编队整体平移、旋转、缩放
- **碰撞避免** - 基于安全距离的动态碰撞检测与规避
- **航点规划** - 支持自定义航点序列

### 📺 监控界面
- **终端监控面板** - 实时动态刷新显示，无需 GUI 环境
- **彩色状态显示** - 电池电量、飞行模式、位置状态一目了然
- **日志系统** - 任务执行日志实时记录
- **PyQt5 GUI** - 可选的图形化监控界面（支持摇杆控制）

### 🛠️ 开发友好
- **极简 API** - 继承 `SimpleSwarmDemo`，只需实现 `run()` 方法
- **自动状态管理** - 自动处理 Offboard 模式切换、解锁流程
- **线程安全** - 内部采用可重入锁保护共享状态
- **向后兼容** - 保留原始 API，现有代码无需修改

---

## 🚀 快速开始

### 环境要求

| 组件 | 版本 | 说明 |
|------|------|------|
| Ubuntu | 22.04 (推荐) | 20.04 也支持 |
| ROS 2 | Humble | 安装 `ros-humble-desktop` |
| Python | 3.8+ | 系统自带 |
| PX4 | v1.14+ | SITL 仿真或真机 |

### 1. 克隆工作空间

```bash
cd ~
git clone <your-repo-url> GTdrone_ws
cd GTdrone_ws
```

### 2. 安装依赖

```bash
# ROS 2 环境（如未安装）
sudo apt update
sudo apt install ros-humble-desktop python3-colcon-common-extensions

# Python 依赖
pip install pyqt5 numpy
```

### 3. 编译工作空间

```bash
cd ~/GTdrone_ws
colcon build
source install/setup.bash
```

### 4. 启动仿真（如使用 SITL）

```bash
# 终端 1：启动 PX4 SITL
cd ~/PX4-Autopilot
make px4_sitl_default gz_x500

# 终端 2：启动 micro-ros-agent
ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888
```

### 5. 运行演示

```bash
# 终端 3：运行正方形飞行演示
cd ~/GTdrone_ws
source install/setup.bash
python3 ~/square_demo.py
```

---

## 📖 使用指南

### 极简示例（推荐）

继承 `SimpleSwarmDemo`，只需实现 `run()` 方法：

```python
#!/usr/bin/env python3
from ros2_px4_swarm_manager import SimpleSwarmDemo, Constants


class MyFlightDemo(SimpleSwarmDemo):
    """我的飞行演示"""
    
    def run(self):
        """实现任务逻辑，监控面板自动显示"""
        
        # 1. 起飞到 3 米（自动完成 Offboard 切换和解锁）
        # 返回: (成功, 结果码, 详细信息)
        success = self.takeoff(1, 3.0, timeout=Constants.TAKEOFF_TIMEOUT)
        if not success:
            self.log("起飞失败!")
            return
        self.sleep(5)
        
        # 2. 飞往 (2, 0, 3) - ENU 坐标系
        self.goto_enu(1, 2.0, 0.0, 3.0)
        self.sleep(5)
        
        # 3. 悬停
        self.hover(1)
        self.sleep(3)
        
        # 4. 降落
        self.land(1)


if __name__ == '__main__':
    MyFlightDemo().start()
```

运行：
```bash
cd ~/GTdrone_ws && source install/setup.bash
python3 my_flight.py
```

### 正方形飞行示例

```python
class SquareFlightDemo(SimpleSwarmDemo):
    """正方形飞行演示"""
    
    def __init__(self):
        super().__init__(uav_ids=[1], refresh_rate=0.5)
        self.side = 3.0      # 边长 3 米
        self.height = 2.0    # 高度 2 米
        
    def run(self):
        uav_id = 1
        
        # 起飞
        self.takeoff(uav_id, self.height)
        self.sleep(2)
        
        # 获取起点
        data = self.controller.get_uav_data(uav_id)
        x0 = data.get('local_x', 0)
        y0 = data.get('local_y', 0)
        
        # 画正方形（顺时针）
        self.goto_enu(uav_id, x0, y0 + self.side, self.height)  # 北
        self.sleep(4)
        self.goto_enu(uav_id, x0 + self.side, y0 + self.side, self.height)  # 东
        self.sleep(4)
        self.goto_enu(uav_id, x0 + self.side, y0, self.height)  # 南
        self.sleep(4)
        self.goto_enu(uav_id, x0, y0, self.height)  # 西（回起点）
        self.sleep(4)
        
        # 降落
        self.land(uav_id)
```

### 编队飞行示例

```python
from ros2_px4_swarm_manager import SimpleSwarmDemo, FormationController


class FormationDemo(SimpleSwarmDemo):
    """三机编队演示"""
    
    def __init__(self):
        super().__init__(uav_ids=[1, 2, 3])
        
    def run(self):
        formation = FormationController(self.controller)
        
        # 三机起飞
        for uav_id in self.uav_ids:
            self.takeoff(uav_id, 3.0)
        self.sleep(8)
        
        # 组成三角形编队
        formation.set_formation_triangle(
            list(self.uav_ids), 
            side_length=4.0
        )
        self.sleep(5)
        
        # 编队整体前移
        formation.move_formation(delta_x=5.0, delta_y=0.0)
        self.sleep(10)
        
        # 编队旋转 90 度
        formation.rotate_formation(90)
        self.sleep(10)
        
        # 依次降落
        for uav_id in self.uav_ids:
            self.land(uav_id)
            self.sleep(2)
```

---

## 🎮 预置演示脚本

| 脚本 | 说明 | 运行命令 |
|------|------|----------|
| `~/square_demo.py` | 单机正方形飞行 | `python3 ~/square_demo.py` |
| `~/circle_demo.py` | 圆形航线飞行 | `python3 ~/circle_demo.py` |
| `~/square_demo_fancy.py` | 带轨迹显示的正方形 | `python3 ~/square_demo_fancy.py` |
| `~/minimal_example.py` | 最小示例（起飞-降落） | `python3 ~/minimal_example.py` |

---

## 🏗️ 架构设计

### 系统架构

```
┌─────────────────────────────────────────────────────────────┐
│                        用户应用层                            │
│              (SimpleSwarmDemo 子类)                         │
└──────────────────────────┬──────────────────────────────────┘
                           │
┌──────────────────────────▼──────────────────────────────────┐
│                      简化接口层                              │
│  ┌────────────────────────────────────────────────────────┐ │
│  │ SimpleSwarmDemo                                        │ │
│  │ • 封装 ROS 初始化和监控面板                            │ │
│  │ • 提供 takeoff/land/goto_enu 等简易 API               │ │
│  │ • 自动状态更新和日志记录                               │ │
│  └────────────────────────────────────────────────────────┘ │
└──────────────────────────┬──────────────────────────────────┘
                           │
┌──────────────────────────▼──────────────────────────────────┐
│                      核心控制层                              │
│  ┌─────────────────┬─────────────────┬────────────────────┐ │
│  │ PX4SwarmController              │ FormationController │ │
│  │ • 自动 Offboard 切换            │ • 编队算法          │ │
│  │ • 自动解锁                      │ • 碰撞检测          │ │
│  │ • 坐标转换                      │ • 队形变换          │ │
│  │ • 10Hz 设定点发布               │                     │ │
│  └─────────────────┴─────────────────┴────────────────────┘ │
└──────────────────────────┬──────────────────────────────────┘
                           │
┌──────────────────────────▼──────────────────────────────────┐
│                      ROS2 通信层                             │
│  • /uav{X}/fmu/in/offboard_control_mode                   │
│  • /uav{X}/fmu/in/trajectory_setpoint                     │
│  • /uav{X}/fmu/in/vehicle_command                         │
│  • /uav{X}/fmu/out/vehicle_status                         │
└──────────────────────────┬──────────────────────────────────┘
                           │
┌──────────────────────────▼──────────────────────────────────┐
│                      PX4 飞控层                              │
│                 (SITL 仿真 / 真机)                           │
└─────────────────────────────────────────────────────────────┘
```

### 坐标系说明

| 坐标系 | 定义 | 用途 |
|--------|------|------|
| **NED** | X北-Y东-Z下 | PX4 内部坐标系 |
| **ENU** | X东-Y北-Z上 | 用户友好坐标系（推荐） |
| **Body** | X前-Y右-Z下 | 相对机体运动 |

**转换关系**：
- ENU → NED: `(x, y, z)` → `(y, x, -z)`
- Body → NED: 使用偏航角旋转矩阵

---

## 📚 API 参考

### SimpleSwarmDemo 类

**初始化**
```python
demo = SimpleSwarmDemo(
    uav_ids=[1],        # 无人机ID列表
    refresh_rate=0.5    # 监控刷新频率（秒）
)
```

**飞行控制方法**（在子类 `run()` 中使用）

| 方法 | 参数 | 返回 | 说明 |
|------|------|------|------|
| `takeoff(uav_id, height, blocking, timeout)` | ID, 高度, 是否阻塞, 超时 | `bool` | 自动起飞 |
| `land(uav_id, blocking, timeout)` | ID, 是否阻塞, 超时 | `bool` | 降落 |
| `hover(uav_id)` | ID | - | 悬停 |
| `goto_enu(uav_id, x, y, z, yaw)` | ID, 东, 北, 高, 偏航 | - | ENU坐标飞行 |
| `goto_ned(uav_id, x, y, z, yaw)` | ID, 北, 东, 下, 偏航 | - | NED坐标飞行 |
| `sleep(seconds)` | 秒数 | - | 等待（非阻塞） |
| `log(message)` | 字符串 | - | 添加任务日志 |

**启动**
```python
demo.start()  # 自动完成 ROS 初始化、连接、执行 run()
```

### Constants 常量类

| 常量 | 值 | 说明 |
|------|-----|------|
| `SETPOINT_RATE_HZ` | 20 | 设定点发布频率 (Hz) |
| `OFFBOARD_RATE_HZ` | 10 | Offboard 定时器频率 (Hz) |
| `DEFAULT_TAKEOFF_HEIGHT` | 3.0 | 默认起飞高度 (m) |
| `HEIGHT_TOLERANCE` | 0.3 | 高度控制容差 (m) |
| `POSITION_TOLERANCE` | 0.3 | 位置控制容差 (m) |
| `TAKEOFF_TIMEOUT` | 30.0 | 起飞超时 (s) |
| `MODE_SWITCH_TIMEOUT` | 5.0 | 模式切换超时 (s) |
| `MAX_UAV_COUNT` | 50 | 最大无人机数量 |

### 枚举类型

```python
from ros2_px4_swarm_manager import FlightMode, TakeoffResult

# 飞行模式
FlightMode.MANUAL          # 0
FlightMode.POSITION        # 2
FlightMode.OFFBOARD        # 14

# 起飞结果
TakeoffResult.SUCCESS           # 成功
TakeoffResult.OFFBOARD_FAILED   # Offboard 切换失败
TakeoffResult.ARM_FAILED        # 解锁失败
TakeoffResult.HEIGHT_TIMEOUT    # 高度等待超时
```

---

## 🔧 故障排除

### 常见问题

<details>
<summary><b>Q: QGC 显示 "no offboard signal"</b></summary>

**原因**: PX4 没有收到设定点流

**解决**:
1. 确保 `OffboardControlMode` 消息包含所有字段（已修复）
2. 确保在发送模式命令前先发送至少 1 秒的设定点流（已自动处理）
3. 检查话题命名空间是否正确
</details>

<details>
<summary><b>Q: 无法进入 Offboard 模式</b></summary>

**原因**: PX4 安全设置或参数限制

**解决**:
```bash
# 在 PX4 SITL 终端执行
safety off                          # 解除安全开关
param set COM_OBL_ACT 0            # Offboard 丢失不动作
param set NAV_RCL_ACT 0            # RC 丢失不动作
param set NAV_DLL_ACT 0            # 数据链丢失不动作
param set COM_RC_IN_MODE 1         # 模拟 RC 输入
param save
reboot
```
</details>

<details>
<summary><b>Q: 起飞失败，显示 "External X" 模式</b></summary>

**原因**: 旧版本模式映射错误

**解决**: 更新到最新代码，模式映射已修复为 `NAVIGATION_STATE_OFFBOARD = 14`
</details>

<details>
<summary><b>Q: 编译错误找不到 px4_msgs</b></summary>

**解决**:
```bash
colcon build --packages-select px4_msgs
source install/setup.bash
colcon build
```
</details>

### 调试技巧

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

# 监控无人机状态
ros2 topic echo /uav1/fmu/out/vehicle_status

# 查看节点图
rqt_graph
```

---

## 📁 工作空间结构

```
GTdrone_ws/
├── src/
│   ├── px4_msgs/                 # PX4 消息定义
│   ├── px4_ros_com/              # PX4-ROS2 通信桥接
│   └── ros2_px4_swarm_manager/   # 本项目的核心包
│       ├── ros2_px4_swarm_manager/
│       │   ├── __init__.py
│       │   ├── px4_swarm_control_v2.py   # 集群控制器
│       │   ├── formation_controller.py   # 编队控制器
│       │   ├── uav_monitor.py            # 监控面板
│       │   ├── main_window_refactored.py # GUI 界面
│       │   └── demo/                     # 示例脚本
│       ├── package.xml
│       └── setup.py
├── build/                        # 编译输出
├── install/                      # 安装目录
└── README.md
```

---

## 🤝 贡献指南

欢迎提交 Issue 和 Pull Request！

1. Fork 本仓库
2. 创建特性分支：`git checkout -b feature/amazing-feature`
3. 提交更改：`git commit -m 'Add amazing feature'`
4. 推送分支：`git push origin feature/amazing-feature`
5. 创建 Pull Request

### 代码规范

- Python 代码遵循 PEP 8
- 新增功能需附带示例和文档
- 保持向后兼容性

---

## 📄 许可证

本项目采用 **Apache License 2.0** 开源许可证。

## 🙏 致谢

- [PX4 Autopilot](https://px4.io/) - 开源飞控平台
- [ROS 2](https://docs.ros.org/) - 机器人操作系统
- [px4_ros_com](https://github.com/PX4/px4_ros_com) - PX4-ROS2 桥接实现

---

<p align="center">
  <i>Powered by ROS 2 & PX4 | Made with ❤️ for drone swarm research</i>
</p>
