单机仿真与控制
适用对象：第一次使用 SwarmCore 的用户 前置条件：已按仓库根 README.md 完成安装（
./install.sh） 对应版本：2026-10（swarm_api 当前版 + gz_que 默认机型）
学完本文你将能够：启动 N 机仿真、用三种方式控制飞机（demo 脚本 / swarm_api / Web 地面站）、看懂坐标系与常见故障。
1. 仿真栈架构
┌────────────────────────────────────────────────────────┐
│ 你的控制端（三选一） │
│ demo_*.py 脚本 / swarm_api 自己的程序 / Web 地面站 │
└──────────────────────┬─────────────────────────────────┘
│ ROS 2 话题 /<uav_N>/fmu/*
┌──────────────────────┴─────────────────────────────────┐
│ MicroXRCEAgent (udp4:8888，全实例共用) │
└───────┬──────────────┬──────────────┬──────────────────┘
│ uXRCE-DDS │ │
┌────┴────┐ ┌────┴────┐ ┌────┴────┐
│ PX4 实例0│ │ PX4 实例1│ │ PX4 实例2│ ← 每机独立 SITL 实例
│ uav_1 │ │ uav_2 │ │ uav_3 │ 独立 EKF/参数/围栏
└────┬────┘ └────┬────┘ └────┬────┘
└──────────────┼──────────────┘
Gazebo Garden（物理仿真，机型 gz_que 雀）
要点：
- 每架飞机一个独立 PX4 实例，ROS 2 命名空间
/uav_<N>/fmu/*（N 从 1 开始），出生点 y 轴间隔 2m。 - 命名空间尾数 = MAV_SYS_ID = UXRCE_DDS_KEY（uav_2 → sysid 2）。这个规则仿真与真机完全一致。
- 默认机型是自主研发的"雀"（gz_que，2004 电机/90mm 桨/0.48kg，动力已按电机台架数据标定）。
2. 启动与停止
每次新开终端先加载环境：
source /opt/ros/humble/setup.bash
source ~/0c00_ws/swarm_ws/install/setup.bash
启动仿真：
~/0c00_ws/swarm_ws/src/bringup/scripts/start_swarm_sim.sh [机数=3] [HEADLESS=1] [机型=gz_que]
# 常用：
start_swarm_sim.sh 1 1 # 单机无头（调试首选，省 CPU）
start_swarm_sim.sh 3 1 # 三机无头（集群开发）
start_swarm_sim.sh 3 0 # 三机带 Gazebo 画面（演示）
停止：
~/0c00_ws/swarm_ws/src/bringup/scripts/stop_swarm_sim.sh
启动后等 30~40 秒再控制——EKF（状态估计）需要时间收敛，脚本会在启动后约 4 秒自动热重启 EKF 重新锁定高度原点。起飞太早的典型症状是"解锁超时"。
2.1 环境变量（启动脚本前缀传入）
| 变量 | 默认 | 含义 |
|---|---|---|
GF_ACT | 3 | 飞控电子围栏动作：0=无 1=警告 2=悬停 3=返航 5=降落（围栏固定 10m×6m） |
RTL_ALT | 0 | 返航爬升高度 RTL_RETURN_ALT；0=按当前高度返航不爬升 |
HGT_REF | 0 | EKF 高度参考：0=气压计（推荐，启动即压住 z）1=GPS 2=测距 3=视觉 |
EKF_RESTART | 1 | 启动后自动热重启 EKF2 锁定高度原点（治"出生在 1 米"），0=关闭 |
EKF_RESTART_DELAY | 4 | 启动后多少秒重启 EKF2 |
示例：GF_ACT=2 RTL_ALT=10 start_swarm_sim.sh 1 1
2.2 机型参数
第 3 个参数指定机型（默认 gz_que）。启动前脚本会预检 airframe 文件与模型目录，不存在会直接报错并列出可用机型，比翻 PX4 日志友好。接入自有机型的方法：放一个 gz 模型目录 + 一个 airframe 文件即可，详见 start_swarm_sim.sh 头部注释。
3. 三种控制方式
铁律：不要用
ros2 topic pub直接向/fmu/in/*发指令。 QoS 配置、Offboard 设定点流、ENU/NED 坐标转换全是坑，指令会被静默丢弃或让飞机乱飞。控制飞机只走下面三条路。
3.1 方式一：demo 脚本（验证环境、学习参考）
python3 ~/0c00_ws/swarm_ws/src/bringup/scripts/demo_takeoff_hover_land.py
15 个 demo 全部输出 DEMO_RESULT <json> 机器可解析结果（最后一行），退出码 0=PASS / 1=FAIL——验证环境是否正常：起仿真 → 跑 demo → 看结果。仿真未启动时 demo 会在 10~30 秒内 FAIL 退出，不会挂死。
完整 demo 导览见第 5 节。
3.2 方式二：swarm_api（写自己的控制程序，推荐）
from swarm_api import Drone
drone = Drone("uav_1") # 连接一架飞机
drone.takeoff(1.5) # 起飞到 1.5m（阻塞，返回时已悬停住）
drone.goto(0, 2, 1.5) # 向北飞 2m（阻塞，飞到才返回）
drone.land() # 降落，自动上锁后返回
drone.shutdown()
QoS、坐标转换、Offboard 设定点流、命令重发全部封装在框架内。完整教程见 swarm_api 使用。
3.3 方式三：Web 地面站（可视化操作）
~/0c00_ws/swarm_ws/src/ground_station/scripts/start_ground_station.sh
# 浏览器打开 http://localhost:8080（局域网用 http://<主机IP>:8080）
功能：状态卡片、3D 轨迹、按钮控制（解锁/起飞/返航/降落/上锁）、指点飞行、坐标飞行（绝对/机体相对）、电子围栏、话题录制与数据分析。页面显示采用起飞系（X=前=上电机头方向，Y=左，Z=上）。
页面行为异常先 Ctrl+F5 强制刷新——浏览器缓存是本项目头号"假故障"来源。
4. 关键概念（踩坑前必读）
4.1 Offboard 两条铁律
PX4 的 Offboard 模式允许外部计算机直接给飞控发设定点：
- 必须先连续发一段设定点，飞控才允许切入 Offboard；
- 设定点必须持续发送（≥2Hz），断流即判定"Offboard 失联"触发保护。swarm_api 用后台 20Hz 线程维持此流，你的程序崩溃/Ctrl+C 后飞控会自动接管降落——这是安全特性。
4.2 坐标系
- 用户面（swarm_api、demo、地面站输入）统一 ENU：x=东，y=北，z=上；yaw 0=东、逆时针为正。
- PX4 内部是 NED，转换由 swarm_api 完成，不要自己转。
- 每架飞机的本地坐标系原点在各自出生点/上电点，互不共享。多机飞行时同一个
(x,y,z)对每架飞机是不同的物理位置（demo 正利用这一点让多机各画各的正方形）。
4.3 高度基准
起飞高度是相对量：目标高度 = 起飞前实测高度 + 想爬的高度。免疫 EKF 高度原点偏差，仿真/真机/GPS/UWB 行为一致。
4.4 QoS 与命令目标（绕过框架直读话题时需要）
- 发给飞控的话题（
fmu/in/*）必须 reliable；订阅飞控话题（fmu/out/*）用 best_effort。 VehicleCommand.target_system必须等于该机 MAV_SYS_ID，否则命令被忽略。
5. demo 全景（15 个）
除 demo_square.py / demo_square_enu.py 是裸 rclpy 教学版外，其余 13 个全部基于 swarm_api。真机运行改脚本内的 NS（命名空间）即可。
入门与基线
| 脚本 | 内容 | 场景 |
|---|---|---|
demo_square_enu.py | 起飞→顺时针 2m 正方形→回原点→降落（ENU，裸 rclpy 教学版，~150 行） | 新手第一个 demo |
demo_square.py | 同上，NED 版（对照学习坐标系） | 教学 |
demo_single_drone.py | 单机正方形（swarm_api 版，对比裸写省多少代码） | 框架入门 |
demo_takeoff_hover_land.py | 最简闭环：起飞 1m→悬停 3s→降落 | 环境自检基线 |
demo_swarm_takeoff_hover_land.py | 多机最简闭环（默认 2 机） | 多机链路自检 |
demo_swarm_square.py | 三机同时画正方形（Swarm 类） | 集群入门 |
定位与控制精度
| 脚本 | 内容 |
|---|---|
demo_square_goto.py | goto 沿逆时针 1m 正方形（6 段+回中心，tol=0.12），检验绝对定位精度 |
demo_body_square.py | move_body 机体系正方形：前→左→后→右各 1m 回起点，不转航向 |
画圆四模式（对比学习控制方式的最佳素材）
| 脚本 | 方式 | 特点 |
|---|---|---|
demo_circle_position.py | 离散 goto 逐点 | 实现最简；逐点停顿，轨迹是 24 边形 |
demo_circle_trajectory.py | circle() 位置轨迹流+前馈 | 平滑匀速、相位滞后 ~0°；半径误差随速度增大 |
demo_circle_velocity.py | circle_velocity() 纯切向速度流 | 半径最准但无位置闭环，会整体漂移 |
demo_circle_smooth.py | circle_smooth() 速度前馈+小增益纠正 | 真机演示推荐（如 demo_circle_smooth.py 0.3） |
多机协同
| 脚本 | 内容 |
|---|---|
demo_dual_circle.py | 双机对置 180° 同圆旋转（仿真专用，原点偏移写死 (0,2)） |
demo_dual_concentric_circle.py | 双机同心圆，自动测原点偏移，仿真/真机通用 |
demo_concentric_circle.py | N 机均布同心圆（相位均布、逐架入圆防交叉） |
DEMO_RESULT 输出格式
DEMO_RESULT {"result":"PASS","waypoints":5,"duration_s":42.1} # 退出码 0
DEMO_RESULT {"result":"FAIL","stage":"takeoff","error":"..."} # 退出码 1
6. 常见问题
Q1：起飞报"进入 Offboard/解锁超时"？
仿真刚启动 EKF 未收敛，等 30~40 秒再跑。仍不行看 swarm_ws/logs/sim_*/px4_uav_1.log 里 preflight 失败原因。
Q2：demo 报"只发现 0 架飞机"或连接超时 FAIL？
仿真没启动或没启动完。用 ros2 topic list | grep vehicle_local_position 确认飞机在线。
Q3：飞机往反方向飞 / 高度反了？ 坐标传成了 NED。swarm_api 只接受 ENU（x=东，y=北，z=上）。
Q4：goto 到位后飞机缓慢漂走？
2026-10-07 已根治（goto 到位后冻结目标点，物理漂移由位置环拉回）。若仍见到，先确认改动后执行过 colcon build --packages-select swarm_api——安装是拷贝而非 symlink，不重新 build 跑的还是旧代码。
Q5：仿真日志把磁盘写满？
当前版本只保留最近 5 次仿真日志（swarm_ws/logs/sim_*）。rosbag 请在地面站"记录"页按需勾选话题。
Q6：起飞后高度显示不对 / "出生在 1 米"？
2026-07 起已通过 EKF 热重启根治（启动后 z≈0.03）。见到说明 EKF_RESTART=0 被关了，或起飞太早（EKF 重启后需 ~10s 收敛）。
Q7：想换机型 / 加相机？
机型用第 3 参数；相机/点云桥接用 start_sensor_bridge.sh（需 ros-humble-ros-gzgarden-bridge，雀当前无相机，属视觉机型预留）。
7. 下一步
- 写自己的控制程序 → swarm_api 使用
- 接入真机 → 真机控制（先仿真跑通同流程，这是强制流程）
本文档采用 CC BY-NC 4.0 许可。