# ROS2 PX4 Swarm Manager - Demo 示例

本目录包含多个无人机控制示例，从简单到复杂，帮助您快速上手。

---

## 快速开始

### 环境准备

```bash
# 1. 编译工作空间
cd ~/GTdrone_ws
colcon build --packages-select ros2_px4_swarm_manager

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

# 3. 启动 PX4 SITL 仿真（如果使用仿真）
# 在另一个终端执行:
cd ~/PX4-Autopilot
make px4_sitl_default gz_x500
```

### 运行 Demo

```bash
# 方式 1: 直接运行 Python 脚本
cd ~/GTdrone_ws
source install/setup.bash
python3 src/ros2_px4_swarm_manager/ros2_px4_swarm_manager/demo/01_single_takeoff_land.py

# 方式 2: 使用 ros2 run（如果已安装）
ros2 run ros2_px4_swarm_manager 01_single_takeoff_land.py
```

---

## Demo 列表

### Demo 01: 单机起飞与降落
**文件**: `01_single_takeoff_land.py`

最简单的无人机控制示例，演示基本的起飞和降落流程。

**功能**:
- 单机起飞到指定高度
- 悬停一段时间
- 降落

**关键 API**:
```python
self.takeoff(uav_id, height)   # 起飞
self.sleep(seconds)            # 等待
self.land(uav_id)              # 降落
```

---

### Demo 02: 单机正方形飞行
**文件**: `02_single_square.py`

演示 ENU 坐标系下的位置控制，无人机将飞行一个正方形航线。

**功能**:
- 起飞到指定高度
- 按 ENU 坐标飞行正方形（北→东→南→西）
- 检查闭合精度
- 降落

**关键 API**:
```python
self.goto_enu(uav_id, x, y, z)  # 飞往 ENU 坐标
```

**坐标系说明**:
- ENU: East-North-Up (东-北-上)
- X 轴指向东，Y 轴指向北，Z 轴指向上

---

### Demo 03: 多机同时起飞与降落
**文件**: `03_multi_takeoff_land_together.py`

演示多架无人机同时起飞和同时降落。

**功能**:
- 3 架无人机同时起飞
- 同时悬停
- 同时降落

**关键 API**:
```python
# 非阻塞起飞，立即返回
self.controller.takeoff(uav_id, height, blocking=False)
```

**阻塞 vs 非阻塞**:
- `blocking=True`: 等待起飞完成才返回（适合依次控制）
- `blocking=False`: 立即返回，不等待（适合同时控制）

---

### Demo 04: 多机依次起飞与降落
**文件**: `04_multi_takeoff_land_sequence.py`

演示多架无人机按顺序依次起飞和降落。

**功能**:
- 3 架无人机依次起飞（等前一架起飞完成再起飞下一架）
- 全体悬停
- 依次降落

**关键 API**:
```python
# 阻塞起飞，等待完成
self.takeoff(uav_id, height, blocking=True)
```

---

### Demo 05: 多机圆形编队旋转
**文件**: `05_multi_circle_formation.py`

演示多架无人机组成圆形编队并整体旋转。

**功能**:
- 3 架无人机同时起飞
- 组成圆形编队
- 整体旋转多次
- 编队整体前飞
- 清除编队并降落

**关键 API**:
```python
from ros2_px4_swarm_manager import FormationController

# 创建编队控制器
formation = FormationController(self.controller)

# 设置圆形编队
formation.set_formation_circle(uav_ids, radius)

# 旋转编队
formation.rotate_formation(angle_degrees)

# 移动编队
formation.move_formation(delta_x, delta_y, delta_z)

# 清除编队
formation.clear_formation()
```

---

## API 参考

### SimpleSwarmDemo 基类

所有 Demo 都继承自 `SimpleSwarmDemo`，它提供了简化的无人机控制接口。

#### 初始化

```python
class MyDemo(SimpleSwarmDemo):
    def __init__(self):
        super().__init__(
            uav_ids=[1, 2, 3],      # 控制的无人机 ID 列表
            refresh_rate=0.5         # 监控面板刷新频率（秒）
        )
```

#### 飞行控制方法

| 方法 | 参数 | 说明 | 返回值 |
|------|------|------|--------|
| `takeoff(uav_id, height, blocking)` | ID, 高度(m), 是否阻塞 | 起飞 | bool |
| `land(uav_id)` | ID | 降落 | - |
| `hover(uav_id)` | ID | 悬停 | - |
| `goto_enu(uav_id, x, y, z)` | ID, 东, 北, 上 | ENU 坐标飞行 | - |
| `goto_ned(uav_id, x, y, z)` | ID, 北, 东, 下 | NED 坐标飞行 | - |
| `sleep(seconds)` | 秒数 | 等待 | - |
| `log(message)` | 字符串 | 添加日志 | - |

#### 访问控制器

```python
# 通过 self.controller 访问底层控制器
status = self.controller.get_uav_status(uav_id)
```

---

### PX4SwarmController 核心控制器

#### 状态查询

```python
# 获取单个无人机状态
status = controller.get_uav_status(uav_id)
print(status.local_x)       # X 位置（NED 坐标）
print(status.local_y)       # Y 位置（NED 坐标）
print(status.local_z)       # Z 位置（NED 坐标，负值为高度）
print(status.yaw)           # 偏航角
print(status.armed)         # 是否解锁
print(status.flight_mode)   # 飞行模式
print(status.battery_voltage)  # 电池电压

# 获取所有无人机状态
all_data = controller.get_all_uav_data()
```

#### 高级控制

```python
# 返航
controller.return_home(uav_id)

# 紧急停止
controller.emergency_stop(uav_id)

# 重启飞控
controller.reboot(uav_id)

# 上锁
controller.disarm(uav_id)
```

---

### FormationController 编队控制器

#### 编队类型

```python
# 一字形编队
formation.set_formation_line(
    uav_ids=[1, 2, 3],
    spacing=3.0,           # 间距（米）
    direction='x'          # 方向: 'x' 或 'y'
)

# 圆形编队
formation.set_formation_circle(
    uav_ids=[1, 2, 3],
    radius=3.0             # 半径（米）
)

# 正方形编队
formation.set_formation_square(
    uav_ids=[1, 2, 3, 4],
    side_length=4.0        # 边长（米）
)

# 三角形编队
formation.set_formation_triangle(
    uav_ids=[1, 2, 3],
    side_length=4.0        # 边长（米）
)
```

#### 编队变换

```python
# 旋转编队
formation.rotate_formation(angle_degrees)

# 移动编队
formation.move_formation(delta_x, delta_y, delta_z)

# 清除编队
formation.clear_formation()
```

---

## 坐标系说明

### ENU (East-North-Up)
- **X**: 东 (+东, -西)
- **Y**: 北 (+北, -南)
- **Z**: 上 (+上, -下)

### NED (North-East-Down) - PX4 内部使用
- **X**: 北 (+北, -南)
- **Y**: 东 (+东, -西)
- **Z**: 下 (+下, -上，高度为负值)

### 坐标转换

```python
from ros2_px4_swarm_manager import ned_to_enu, enu_to_ned

# ENU 转 NED
ned_x, ned_y, ned_z = enu_to_ned(enu_x, enu_y, enu_z)

# NED 转 ENU
enu_x, enu_y, enu_z = ned_to_enu(ned_x, ned_y, ned_z)
```

---

## 自定义 Demo

创建自己的 Demo 非常简单：

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


class MyCustomDemo(SimpleSwarmDemo):
    def __init__(self):
        super().__init__(uav_ids=[1])  # 控制 UAV 1
        
    def run(self):
        # 在这里实现你的任务逻辑
        
        # 1. 起飞
        self.takeoff(1, 3.0)
        self.sleep(5)
        
        # 2. 飞行到指定位置
        self.goto_enu(1, 2.0, 0.0, 3.0)
        self.sleep(5)
        
        # 3. 降落
        self.land(1)


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

---

## 故障排除

### 问题 1: 无人机未连接
**症状**: 监控面板显示"未连接"

**解决**:
1. 检查 PX4 是否已启动
2. 检查话题命名空间是否正确（默认 `/uav1/fmu/out/vehicle_status`）
3. 检查 ROS2 网络配置

### 问题 2: 无法进入 Offboard 模式
**症状**: 模式一直显示"Unknown"或"Position"

**解决**:
1. 在 PX4 终端执行 `safety off` 解除安全开关
2. 检查 PX4 参数：`COM_OBL_ACT = 0`, `NAV_RCL_ACT = 0`

### 问题 3: 起飞失败
**症状**: `takeoff()` 返回 False

**解决**:
1. 检查无人机是否已连接
2. 检查 PX4 参数设置
3. 查看日志输出了解具体失败原因

---

## 更多示例

查看 `ros2_px4_swarm_manager` 包中的其他代码：
- `main_window_refactored.py` - GUI 界面
- `uav_monitor.py` - 终端监控
- `formation_controller.py` - 编队控制算法

---

**版本**: 2.0.0
**作者**: chai
**许可证**: Apache-2.0
