swarm_api 使用

适用对象：在 SwarmCore 上开发单机/集群控制程序的用户前置：能跑通单机仿真的 demo 配套速查：docs/SWARM_API_REFERENCE.md（每个方法的签名/参数/异常/副作用，本文是理解与教学）

swarm_api 把 PX4 Offboard 控制的全部工程细节（QoS、ENU↔NED 转换、20Hz 设定点流、命令重发、EKF reset 补偿）封装成 Python 库。仿真与真机同一套 API，控制代码零改动。

from swarm_api import Drone, Swarm, Strategy, DroneError, SwarmError

1. 一分钟上手

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()            # 释放资源（必须调用）

多机只是把单数动作换成复数动作：

from swarm_api import Swarm

swarm = Swarm(num_drones=3)     # 自动发现在线飞机
swarm.takeoff(1.5)              # 全群同时起飞
swarm.goto_all([(0,2,1.5), (2,2,1.5), (4,2,1.5)])  # 各机飞各自目标
swarm.land()
swarm.shutdown()

2. 坐标系约定（最易错，先读）

统一 ENU：x=东，y=北，z=上；yaw 0=东、逆时针为正（rad）。框架不接受 NED，转换在框架内部完成。
本地系：所有 API 使用每架飞控各自的本地 ENU。仿真中原点在各自出生点；真机纯 UWB 下与锚点系对齐（当前约定北=锚点X、东=-锚点Y）。
多机共享几何（如同一圆心）需要原点偏置换算——现成写法见 demo_concentric_circle.py 的 measure_origin_offsets。
机体系位移用 move_body：yaw-only FLU（forward=机头前、left=左、up=上），不是 PX4 FRD。

3. Drone：单机原语

构造：Drone(namespace, sys_id=None)——namespace 如 "uav_1"；sys_id 缺省从命名空间尾数推断。

3.1 takeoff(alt=1.5, vz=0.5, tol=0.15, timeout=60.0, ...)

起飞到相对高度并悬停，返回时已悬停住。

高度基准 = 起飞前实测高度 + alt（免疫 EKF 高度原点偏差）；
水平速度恒 0 原地爬升（不追 EKF 位置漂移，杜绝"起飞后往前跑"），航向锚定起飞瞬间；
返回条件：高度到位且近 1s 平均水平速度收敛——"返回即悬停"是经过实测加固的语义，不是"发完命令就返回"。

3.2 goto(x, y, z, yaw=None, tol=0.3, timeout=60.0)

飞到 ENU 点并停稳冻结在目标点上才返回。前置：已 takeoff。

核心语义（2026-10-07 起）：两段式停稳——先进 tol 半径，再保持目标设定点直到水平速度 ≤0.15 m/s；返回后设定点流继续冻结在目标点（position+local 位置闭环），物理漂移由位置环拉回。这就是为什么 WEB 连续指点飞行不再漂移：旧的 follow 悬停把目标每帧重锚到当前估计，物理漂移无回正力会持续累积。

yaw=None 保持当前航向；传值则飞行中转至该 ENU 航向。

3.3 move_body(forward, left=0.0, up=0.0, yaw=None, tol=0.3, timeout=60.0)

按调用瞬间的机体方向相对位移（阻塞）：

drone.move_body(1.0, 0.0, 0.0)   # 沿机头方向前进 1m（不管机头朝哪）

位移按调用瞬间的实际 yaw 快照旋转到本地 ENU（yaw-only FLU）；可选 yaw 只是到位后的最终 ENU 航向，不参与位移旋转。不依赖全局定位质量，真机相对位移的首选。

3.4 画圆三模式 + 离散 goto 对比

签名（以 smooth 为例）：circle_smooth(cx, cy, z, radius, speed, laps=1.0, start_angle=None, ccw=True, k=0.3, k_i=0.1, yaw_rate=0.0, timeout=120.0)

方式	方法	特点	适用
离散航点	`goto()` 循环	实现最简；逐点停顿、轨迹多边形	低速检验绝对精度
轨迹模式	`circle()`	20Hz 位置轨迹流 + 速度/加速度前馈；平滑匀速、相位滞后 ~0°；半径误差随速度增大（0.5m/s 时 +16%）	仿真 ≤0.4 m/s
速度模式	`circle_velocity()`	纯切向速度流；半径最准但无位置闭环，整圆漂移（2 圈 ~0.3m）	圈数少的场合
平滑模式	`circle_smooth()`	速度前馈（起步 1s ramp）+ 小增益径向/相位纠正 + 径向积分限幅	真机演示推荐（如 0.3 m/s）

真机已知物理地板：EKF 估计相对 UWB 真值弯曲 mean ~~0.06~~0.08m（瞬时 ~0.2m），画圆"肉眼毛边"属正常，调参无法消除（三个方向已证伪）。

3.5 其余原语

方法	阻塞	语义
`set_velocity(vx, vy, vz, yaw_rate=0.0)`	否	持续 ENU 速度飞行，直到 hover/goto/land；框架不做位置保护（围栏由 PX4 兜底）
`hover()`	否	零速度模式悬停（速度目标 [0,0,0]，物理原地不动；要"钉在点上"用 goto 的冻结目标点）
`arm(timeout=10.0)`	是	解锁电机（不切模式），命令重发直到确认
`disarm(timeout=10.0)`	是	上锁；空中被拒后半程升级强制停桨（kill）——飞行中调用必然坠落，仅作紧急手段
`rtl(timeout=15.0)`	模式确认即返回	返航（PX4 执行返航+降落全过程，不等落地）；内部先停设定点流
`land(timeout=60.0)`	是	原地降落，触地自动上锁后返回；内部先停设定点流（不停 PX4 会拒绝降落命令）
`shutdown()`	—	停线程毁节点，退出前必须调用

3.6 状态属性（只读，实时）

pos（本地 ENU 三元组，None=未连接）、yaw、armed、offboard、failsafe、nav_state、xy_valid、v_xy_valid、dead_reckoning、vx/vy/vz（ENU 速度）、ns、sys_id。

print(f"高度 {drone.pos[2]:.2f} m, offboard={drone.offboard}")

4. Swarm：多机并行

构造：Swarm(num_drones=None, namespaces=None, discovery_timeout=10.0)。自动发现规则：扫描发布 /<ns>/fmu/out/vehicle_local_position 的命名空间，自然序排列；num_drones 发现不足抛 SwarmError。

并行模型：多机动作 = 每机一个工作线程同时执行 + 屏障返回（最慢的一架决定总耗时）。异常隔离：某机失败 → 该机自动悬停，其余继续，全部结束后抛 SwarmError（e.errors 是 {命名空间: 异常}）。

容器接口：len(swarm)、swarm[0]、swarm["uav_2"]、swarm.drones、swarm.namespaces。

多机动作

方法	语义
`takeoff(alt=1.5, tol=0.15, timeout=60.0)`	全群同时起飞，各自以本机实测高度为基准
`goto_all(points, ...)`	`[(x,y,z), ...]` 逐机目标（推荐，真机防碰撞）；或单个 `(x,y,z)` 全群广播（仿真各自本地系安全；真机共享系会撞机）
`move_body_all(moves, yaw=None, ...)`	单 `(f,l,u)` 广播或逐机列表；各机按各自调用瞬间机头方向位移
`circle_all / circle_velocity_all / circle_smooth_all(circles, ...)`	6 元组 `(cx,cy,z,radius,speed,laps)` 广播或逐机列表
`set_velocity_all(velocities)`	非阻塞，广播或逐机
`hover() / emergency_stop()`	全群零速度悬停（急停当前等价 hover；PX4 失联保护仍是最后防线）
`land(timeout=60.0)`	全群同时降落，全部上锁后返回
`goto_formation(shape, spacing=2.0, z=1.5, center=(0,0), ...)`	编队：`line/column/triangle/grid`（每机在各自本地系）
`Swarm.formation_points(shape, n, spacing)`	静态方法，生成编队 XY 偏移

集群/单机混用示例：

swarm.takeoff(1.5)
swarm["uav_2"].goto(1, 1, 2.5)     # 单机微调
swarm.goto_all([(0,2,1.5), (2,2,1.5), (4,2,1.5)])
swarm.land()

5. Strategy：可复用算法插件

把算法写成 Strategy 子类，起飞/异常处理/降落/收尾全部托管——平台基线算法的组织方式，便于不同算法在相同环境公平对比：

from swarm_api import Strategy

class TriangleDemo(Strategy):
    name = "triangle_demo"
    def setup(self, swarm):      # 可选：准备（订阅传感器、加载参数）
        pass
    def run(self, swarm):        # 必须实现：算法主体
        swarm.takeoff(1.5)
        swarm.goto_formation("triangle", spacing=2.0, z=1.5)
    def teardown(self, swarm):   # 可选：清理（保存数据、生成报告）
        pass

if __name__ == "__main__":
    TriangleDemo().main(num_drones=3)

main() 托管的生命周期：发现飞机 → setup → run →（异常则全群悬停）→ 自动降落 → teardown → 释放资源。Ctrl+C 安全触发降落。

6. 错误处理

from swarm_api import Swarm, SwarmError

swarm = Swarm(num_drones=3)
try:
    swarm.takeoff(1.5)
    swarm.goto_all([(0,2,1.5), (2,2,1.5), (4,2,1.5)])
except SwarmError as e:
    for ns, err in e.errors.items():
        print(f"{ns} 失败: {err}")
    swarm.hover()        # 先稳住全群
finally:
    swarm.land()         # 无论如何安全降落
    swarm.shutdown()

要点：

所有失败抛 DroneError（单机）/ SwarmError（多机汇总），没有静默失败；
超时不等于坠机——飞机仍在目标点悬停（设定点流在工作），捕获后可继续下指令；
飞行中飞控 failsafe 接管时，goto 在 2 秒内检测 Offboard 丢失立即抛错（不傻等超时）；
部分失败语义：SwarmError 时失败机已悬停，其余机已完成动作——异常处理要意识到这一点。

7. 工程建议

验证环境用 DEMO_RESULT：起仿真 → 跑 demo_square_goto.py → 看最后一行 DEMO_RESULT {"result":"PASS"} 与退出码。
改了 swarm_api 必须重新 build：cd ~/0c00_ws/swarm_ws && colcon build --packages-select swarm_api——安装是拷贝不是 symlink，不 build 跑的是旧代码（demo 脚本直接跑 src 不受影响）。
tol 不要小于定位噪声：仿真 GPS 约 0.1~~0.3m，真机 UWB 实测到位 0.09~~0.18m；tol 太小只会在目标附近反复微调。
长距离 goto 自己估时：飞行速度由 PX4 参数决定（默认约 1~2 m/s），飞 20m 至少给 30s timeout。
真机共享几何用原点偏置换算：现成实现 demo_concentric_circle.py（measure_origin_offsets 中位数采样）。
算法与框架分离：你的规划逻辑（纯计算）不要 import rclpy，可脱离仿真单元测试；swarm_api 自身带 593 行单元测试（swarm_ws/src/swarm_api/test/）可作参考。

8. 深入学习路径

swarm_ws/src/bringup/scripts/demo_single_drone.py —— 最小框架示例
本文 + docs/SWARM_API_REFERENCE.md —— 接口全貌
demo_circle_*.py 四模式对比 —— 控制方式的最佳教材
swarm_ws/src/swarm_api/swarm_api/drone.py —— 框架实现，注释完整，理解 Offboard 的最好的材料

本文档采用 CC BY-NC 4.0 许可。

1. 一分钟上手​

2. 坐标系约定（最易错，先读）​

3. Drone：单机原语​

3.1 takeoff(alt=1.5, vz=0.5, tol=0.15, timeout=60.0, ...)​

3.2 goto(x, y, z, yaw=None, tol=0.3, timeout=60.0)​

3.3 move_body(forward, left=0.0, up=0.0, yaw=None, tol=0.3, timeout=60.0)​

3.4 画圆三模式 + 离散 goto 对比​

3.5 其余原语​

3.6 状态属性（只读，实时）​

4. Swarm：多机并行​

多机动作​

5. Strategy：可复用算法插件​

6. 错误处理​

7. 工程建议​

8. 深入学习路径​