# 多无人机Offboard控制程序

基于现有的单机offboard控制程序，扩展为支持多无人机同时起飞的控制系统。

## 文件说明

- `multi_uav_offboard_control.py` - 多无人机控制主程序
- `launch_multi_uav.sh` - 启动脚本
- `offboard_control.py` - 原始单机控制程序（参考）

## 功能特性

### 1. 多机话题支持
- 支持 `/uav1`, `/uav2`, `/uav3` 等话题前缀
- 每个无人机有独立的话题命名空间
- 话题格式：`/uav{id}/fmu/in/offboard_control_mode` 等

### 2. 同步起飞控制
- 所有无人机同时进入offboard模式
- 同时发送arm指令
- 同步起飞到指定高度

### 3. 原地起飞设计
- 所有无人机在当前位置垂直起飞
- 保持初始水平位置不变
- 只改变高度进行起飞

### 4. 可靠性改进
- 分阶段控制：offboard模式切换 → 解锁 → 起飞
- 重试机制：每个阶段都有最大尝试次数
- 通信优化：无人机间添加延迟避免拥塞
- 状态监控：实时统计无人机状态

## 使用方法

### 方法1：使用启动脚本
```bash
# 启动2架无人机（默认）
./launch_multi_uav.sh

# 启动3架无人机
./launch_multi_uav.sh 3

# 启动4架无人机
./launch_multi_uav.sh 4
```

### 方法2：直接运行Python程序
```bash
# 确保已source ROS 2环境和工作空间
source /opt/ros/humble/setup.bash
source install/setup.bash

# 运行程序
python3 src/px4_ros_com/src/examples/offboard_py/multi_uav_offboard_control.py [无人机数量]
```

## 程序架构

### UAVController类
- 管理单个无人机的所有操作
- 包含发布器、订阅器和控制逻辑
- 独立的状态跟踪
- 记录初始位置用于原地起飞

### MultiUAVOffboardControl类
- 管理所有无人机控制器
- 协调多机同步操作
- 提供统一的定时器回调

## 话题映射

对于每架无人机 `uav{i}`，话题映射如下：

| 功能 | 单机话题 | 多机话题 |
|------|----------|----------|
| Offboard控制模式 | `/fmu/in/offboard_control_mode` | `/uav{i}/fmu/in/offboard_control_mode` |
| 轨迹设定点 | `/fmu/in/trajectory_setpoint` | `/uav{i}/fmu/in/trajectory_setpoint` |
| 车辆命令 | `/fmu/in/vehicle_command` | `/uav{i}/fmu/in/vehicle_command` |
| 车辆位置 | `/fmu/out/vehicle_local_position_v1` | `/uav{i}/fmu/out/vehicle_local_position_v1` |
| 车辆状态 | `/fmu/out/vehicle_status_v1` | `/uav{i}/fmu/out/vehicle_status_v1` |

## 配置参数

- `takeoff_height`: 起飞高度（默认：-5.0米）
- 水平间隔：2米/架
- 定时器频率：10Hz

## 扩展建议

1. **编队飞行**: 在 `all_reached_target` 后添加编队控制逻辑
2. **任务协调**: 添加任务分配和协调机制
3. **故障处理**: 增加单机故障检测和处理
4. **通信优化**: 优化多机间的通信效率

## 注意事项

1. 确保PX4仿真环境支持多机配置
2. 每个无人机需要独立的系统ID
3. 在实际部署前进行充分的仿真测试
4. 注意网络带宽和延迟对多机控制的影响