keita
477ad5f5de
- pod_bridge: 协议G版编解码/地理位置引导律/PX4网关/串口链路/闭环主循环 - 含三项安全修复: position_time_ms拆分、表2-2锁定门控、run()异常护栏 - 测试92条全绿; 附树莓派4B(Ubuntu 24.04)部署注意事项与最小依赖清单
359 lines
12 KiB
Python
359 lines
12 KiB
Python
"""bridge_test.py —— PodBridge 闭环主循环单元测试（FakePx4 + FakePod，无硬件）。
|
|
|
|
覆盖：单周期 step（有/无目标、位置无效、位置时间戳超龄）、姿态下发节流与帧内容、
|
|
吊舱帧入站缓存、锁定门控（表2-2 bit2 + 遥测超龄）、触发式控制指令编码、
|
|
start/stop 生命周期与停机刹车、run() 主循环异常护栏与故障恢复。
|
|
"""
|
|
|
|
import asyncio
|
|
import time
|
|
from dataclasses import replace
|
|
|
|
from pod_bridge.bridge import PodBridge
|
|
from pod_bridge.config import BridgeConfig
|
|
from pod_bridge.protocol import (ControlCode, ControlCommand, TargetGeo,
|
|
Telemetry, decode_attitude, decode_control)
|
|
from pod_bridge.px4_gateway import OwnState
|
|
|
|
|
|
class FakePx4:
|
|
def __init__(self, state=None):
|
|
self._state = state or OwnState()
|
|
self.telemetry_started = False
|
|
self.telemetry_stopped = False
|
|
self.offboard_started = False
|
|
self.offboard_stopped = False
|
|
self.offboard_start_ok = True
|
|
self.velocity_calls = []
|
|
self.brake_calls = []
|
|
|
|
def own_state(self):
|
|
return replace(self._state)
|
|
|
|
async def start_telemetry(self):
|
|
self.telemetry_started = True
|
|
|
|
async def stop_telemetry(self):
|
|
self.telemetry_stopped = True
|
|
|
|
async def start_offboard(self):
|
|
self.offboard_started = True
|
|
return self.offboard_start_ok
|
|
|
|
async def stop_offboard(self):
|
|
self.offboard_stopped = True
|
|
|
|
async def send_velocity_heading(self, out, time_boot_ms=None):
|
|
self.velocity_calls.append((out, time_boot_ms))
|
|
return True
|
|
|
|
async def send_brake_hover(self, time_boot_ms=None):
|
|
self.brake_calls.append(time_boot_ms)
|
|
return True
|
|
|
|
|
|
class FakePod:
|
|
def __init__(self):
|
|
self.inbox = []
|
|
self.sent = []
|
|
self.closed = False
|
|
|
|
def queue(self, *frames):
|
|
self.inbox.extend(frames)
|
|
|
|
def poll(self):
|
|
out, self.inbox = self.inbox, []
|
|
return out
|
|
|
|
def send(self, data):
|
|
self.sent.append(data)
|
|
|
|
def close(self):
|
|
self.closed = True
|
|
|
|
|
|
def _valid_state(now_ms, lat=0.0, lon=0.0, yaw_rad=0.0, gs=0.0):
|
|
return OwnState(latitude_deg=lat, longitude_deg=lon, yaw_rad=yaw_rad,
|
|
ground_speed_mps=gs, fix_type=3, num_satellites=12,
|
|
has_position=True, has_attitude=True, position_valid=True,
|
|
time_ms=now_ms, position_time_ms=now_ms)
|
|
|
|
|
|
def _locked_telemetry():
|
|
"""状态字节1 bit2=1(锁定)的遥测帧。"""
|
|
return Telemetry(status1=0b00000100)
|
|
|
|
|
|
def _bridge(px4, pod, cfg=None):
|
|
return PodBridge(cfg or BridgeConfig(), px4, pod, now_ms_fn=lambda: 0)
|
|
|
|
|
|
# --- step：有目标 → 速度指令 ------------------------------------------------
|
|
def test_step_with_target_commands_velocity():
|
|
px4 = FakePx4(_valid_state(1000))
|
|
pod = FakePod()
|
|
pod.queue(TargetGeo(latitude_deg=0.001, longitude_deg=0.0), # 正北 ~111m
|
|
_locked_telemetry())
|
|
b = _bridge(px4, pod)
|
|
|
|
out = asyncio.run(b.step(1000))
|
|
assert out.mode.name == "VELOCITY_HEADING"
|
|
assert len(px4.velocity_calls) == 1
|
|
assert px4.brake_calls == []
|
|
|
|
|
|
def test_step_without_target_brakes():
|
|
px4 = FakePx4(_valid_state(1000))
|
|
pod = FakePod()
|
|
b = _bridge(px4, pod)
|
|
|
|
out = asyncio.run(b.step(1000))
|
|
assert out.mode.name == "BRAKE_HOVER"
|
|
assert len(px4.brake_calls) == 1
|
|
assert px4.velocity_calls == []
|
|
|
|
|
|
def test_step_invalid_position_brakes_even_with_target():
|
|
state = replace(_valid_state(1000), position_valid=False, fix_type=0)
|
|
px4 = FakePx4(state)
|
|
pod = FakePod()
|
|
pod.queue(TargetGeo(latitude_deg=0.001))
|
|
b = _bridge(px4, pod)
|
|
|
|
out = asyncio.run(b.step(1000))
|
|
assert out.mode.name == "BRAKE_HOVER"
|
|
|
|
|
|
# --- 姿态下发：首拍即发 + 节流 + 帧内容 ------------------------------------
|
|
def test_attitude_downlinked_on_first_step_with_own_state():
|
|
px4 = FakePx4(_valid_state(1000, lat=31.5, lon=121.5, yaw_rad=1.57, gs=4.0))
|
|
pod = FakePod()
|
|
b = _bridge(px4, pod)
|
|
|
|
asyncio.run(b.step(1000))
|
|
assert len(pod.sent) == 1
|
|
att = decode_attitude(pod.sent[0])
|
|
assert abs(att.latitude_deg - 31.5) < 1e-6
|
|
assert abs(att.longitude_deg - 121.5) < 1e-6
|
|
assert abs(att.yaw_deg - 89.95) < 0.02 # 1.57 rad ≈ 89.95°
|
|
assert att.ground_speed_mps == 4.0
|
|
assert att.year >= 2026 and 1 <= att.month <= 12
|
|
|
|
|
|
def test_attitude_downlink_throttled():
|
|
px4 = FakePx4(_valid_state(1000))
|
|
pod = FakePod()
|
|
cfg = BridgeConfig()
|
|
cfg.link.attitude_downlink_hz = 20.0 # 周期 50ms
|
|
b = _bridge(px4, pod, cfg)
|
|
|
|
async def scenario():
|
|
await b.step(1000) # 发（首拍）
|
|
await b.step(1010) # 10ms < 50ms，跳过
|
|
await b.step(1040) # 40ms < 50ms，跳过
|
|
await b.step(1060) # 60ms ≥ 50ms，发
|
|
asyncio.run(scenario())
|
|
assert len(pod.sent) == 2
|
|
|
|
|
|
# --- 吊舱帧入站缓存 --------------------------------------------------------
|
|
def test_target_frame_updates_latest_and_flips_to_velocity():
|
|
px4 = FakePx4(_valid_state(1000))
|
|
pod = FakePod()
|
|
b = _bridge(px4, pod)
|
|
|
|
asyncio.run(b.step(1000)) # 无目标 → 刹车
|
|
assert b.latest_target is None
|
|
pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
|
|
asyncio.run(b.step(1100)) # 收到目标且锁定 → 速度
|
|
assert b.latest_target is not None
|
|
assert len(px4.velocity_calls) == 1
|
|
|
|
|
|
def test_telemetry_frame_cached():
|
|
px4 = FakePx4(_valid_state(1000))
|
|
pod = FakePod()
|
|
pod.queue(Telemetry(laser_range_m=88.8))
|
|
b = _bridge(px4, pod)
|
|
asyncio.run(b.step(1000))
|
|
assert b.latest_telemetry is not None
|
|
assert b.latest_telemetry[0].laser_range_m == 88.8
|
|
|
|
|
|
# --- 触发式控制指令 --------------------------------------------------------
|
|
def test_send_control_encodes_frame():
|
|
px4 = FakePx4(_valid_state(1000))
|
|
pod = FakePod()
|
|
b = _bridge(px4, pod)
|
|
b.send_control(ControlCommand(code=ControlCode.CENTER))
|
|
assert len(pod.sent) == 1
|
|
assert decode_control(pod.sent[0]).code == ControlCode.CENTER
|
|
|
|
|
|
# --- 生命周期 --------------------------------------------------------------
|
|
def test_start_wires_telemetry_and_offboard():
|
|
px4 = FakePx4(_valid_state(0))
|
|
b = _bridge(px4, FakePod())
|
|
assert asyncio.run(b.start()) is True
|
|
assert px4.telemetry_started and px4.offboard_started
|
|
|
|
|
|
def test_start_returns_false_when_offboard_fails():
|
|
px4 = FakePx4(_valid_state(0))
|
|
px4.offboard_start_ok = False
|
|
b = _bridge(px4, FakePod())
|
|
assert asyncio.run(b.start()) is False
|
|
|
|
|
|
def test_stop_brakes_and_tears_down():
|
|
px4 = FakePx4(_valid_state(0))
|
|
pod = FakePod()
|
|
b = _bridge(px4, pod)
|
|
|
|
async def scenario():
|
|
await b.start()
|
|
await b.stop()
|
|
asyncio.run(scenario())
|
|
assert len(px4.brake_calls) >= 1 # 停机前刹车
|
|
assert px4.offboard_stopped and px4.telemetry_stopped
|
|
assert pod.closed
|
|
|
|
|
|
|
|
# --- 缺陷①回归:position 时间戳超龄 -------------------------------------------
|
|
def test_step_brakes_when_position_timestamp_stale():
|
|
"""time_ms 被姿态流刷得很新,但 position_time_ms 超龄 → 必须刹车。"""
|
|
state = replace(_valid_state(1000), time_ms=100000, position_time_ms=1000)
|
|
px4 = FakePx4(state)
|
|
pod = FakePod()
|
|
pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
|
|
b = _bridge(px4, pod)
|
|
|
|
out = asyncio.run(b.step(100000)) # position 已 99s 未更新
|
|
assert out.mode.name == "BRAKE_HOVER"
|
|
assert px4.velocity_calls == []
|
|
|
|
|
|
# --- 缺陷②:锁定门控 -----------------------------------------------------------
|
|
def test_target_without_lock_bit_brakes():
|
|
"""吊舱在搜索状态(bit2=0),即使收到表4 坐标也必须刹车。"""
|
|
px4 = FakePx4(_valid_state(1000))
|
|
pod = FakePod()
|
|
pod.queue(TargetGeo(latitude_deg=0.001), Telemetry(status1=0)) # 未锁定
|
|
b = _bridge(px4, pod)
|
|
|
|
out = asyncio.run(b.step(1000))
|
|
assert out.mode.name == "BRAKE_HOVER"
|
|
assert px4.velocity_calls == []
|
|
|
|
|
|
def test_target_without_any_telemetry_brakes():
|
|
"""只收到表4、从未收到表2-2 → 锁定状态未知,按未锁定处理。"""
|
|
px4 = FakePx4(_valid_state(1000))
|
|
pod = FakePod()
|
|
pod.queue(TargetGeo(latitude_deg=0.001))
|
|
b = _bridge(px4, pod)
|
|
|
|
out = asyncio.run(b.step(1000))
|
|
assert out.mode.name == "BRAKE_HOVER"
|
|
|
|
|
|
def test_target_with_stale_telemetry_brakes():
|
|
"""锁定遥测超龄(> link.telemetry_timeout_ms)→ 目标视为无效。"""
|
|
px4 = FakePx4(_valid_state(1000))
|
|
pod = FakePod()
|
|
cfg = BridgeConfig()
|
|
cfg.link.telemetry_timeout_ms = 500
|
|
b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0)
|
|
|
|
async def scenario():
|
|
pod.queue(_locked_telemetry(), TargetGeo(latitude_deg=0.001))
|
|
await b.step(1000) # 遥测新鲜,但此刻若无目标则刹车
|
|
pod.queue(TargetGeo(latitude_deg=0.001))
|
|
out1 = await b.step(1200) # 遥测 200ms 前,仍新鲜 → 速度
|
|
out2 = await b.step(1600) # 遥测 600ms 前,超龄 → 刹车
|
|
return out1, out2
|
|
|
|
out1, out2 = asyncio.run(scenario())
|
|
assert out1.mode.name == "VELOCITY_HEADING"
|
|
assert out2.mode.name == "BRAKE_HOVER"
|
|
|
|
|
|
def test_lock_lost_midflight_falls_back_to_brake():
|
|
"""先锁定引导,随后遥测变为搜索状态 → 立即回落刹车。"""
|
|
px4 = FakePx4(_valid_state(1000))
|
|
pod = FakePod()
|
|
b = _bridge(px4, pod)
|
|
|
|
async def scenario():
|
|
pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
|
|
out1 = await b.step(1000)
|
|
pod.queue(Telemetry(status1=0)) # 丢失锁定
|
|
out2 = await b.step(1100)
|
|
return out1, out2
|
|
|
|
out1, out2 = asyncio.run(scenario())
|
|
assert out1.mode.name == "VELOCITY_HEADING"
|
|
assert out2.mode.name == "BRAKE_HOVER"
|
|
|
|
|
|
# --- 缺陷③:run() 异常护栏 -----------------------------------------------------
|
|
class FlakyPod(FakePod):
|
|
"""前 fail_times 次 poll 抛串口异常,之后恢复正常。"""
|
|
|
|
def __init__(self, fail_times):
|
|
super().__init__()
|
|
self.fail_times = fail_times
|
|
|
|
def poll(self):
|
|
if self.fail_times > 0:
|
|
self.fail_times -= 1
|
|
raise OSError("serial device disconnected")
|
|
return super().poll()
|
|
|
|
|
|
def _run_bridge(px4, pod, hz=200.0):
|
|
cfg = BridgeConfig()
|
|
cfg.guidance.frequency_hz = hz # 加快节拍,缩短测试时间
|
|
return PodBridge(cfg, px4, pod) # 用真实单调时钟
|
|
|
|
|
|
def test_run_survives_poll_exceptions_and_keeps_heartbeat():
|
|
"""串口持续异常:任务不死、每周期补发刹车保心跳、stop_event 正常退出。"""
|
|
now = int(time.monotonic() * 1000)
|
|
px4 = FakePx4(_valid_state(now))
|
|
pod = FlakyPod(fail_times=10**9) # 永远失败
|
|
b = _run_bridge(px4, pod)
|
|
|
|
async def scenario():
|
|
stop = asyncio.Event()
|
|
task = asyncio.ensure_future(b.run(stop))
|
|
await asyncio.sleep(0.2)
|
|
stop.set()
|
|
await asyncio.wait_for(task, timeout=2.0)
|
|
|
|
asyncio.run(scenario())
|
|
assert len(px4.brake_calls) > 0 # 异常期间持续补发刹车
|
|
assert pod.fail_times > 0 # 循环确实在反复尝试(未提前退出)
|
|
|
|
|
|
def test_run_recovers_after_transient_failure():
|
|
"""串口短暂异常后恢复:异常期刹车,恢复后照常引导。"""
|
|
now = int(time.monotonic() * 1000)
|
|
px4 = FakePx4(_valid_state(now))
|
|
pod = FlakyPod(fail_times=3)
|
|
pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
|
|
b = _run_bridge(px4, pod)
|
|
|
|
async def scenario():
|
|
stop = asyncio.Event()
|
|
task = asyncio.ensure_future(b.run(stop))
|
|
await asyncio.sleep(0.3)
|
|
stop.set()
|
|
await asyncio.wait_for(task, timeout=2.0)
|
|
|
|
asyncio.run(scenario())
|
|
assert len(px4.brake_calls) >= 3 # 3 次异常各补一发刹车
|
|
assert len(px4.velocity_calls) >= 1 # 恢复后进入正常引导
|
|
assert b._step_failures == 0 # 计数已复位
|