keita
ded17d7995
- utils/task_runner: 6态状态机(WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL) + 编排器(快照采集/状态动作) + 双层日志(事件JSONL/遥测CSV) + 吊舱控制指令(0x07/0x0F/0x0E/0x08) - pod_bridge: ground_link 地面坐标UDP注入(超龄判无效,无激光替代表4); set_heading_servo_enabled() 运行时L1/L2切换(切换即reset清积分防输出跳变); pod_locked 提为公共门控; config 新增 ground_inject 段 - virtual_pod 模拟吊舱(无电机语义) + verify_px4 增量④仅飞控验证 + pod_sitl_test L1 SITL全流程(飞行编排不塞进 run_bridge, 保持纯offboard闭环入口) - 入口: task_runner_sitl(VirtualPod+虚拟目标, SITL端到端跑通约1m24s) / task_runner_real(真串口+UDP注入+吊舱控制) - 地面站 ground_station: 坐标注入 + 状态可视化(Flask) - S1标定回填(2026-10-09): beta_sign=+1 / alpha_sign=-1, 加标定回归测试守护 - 新增测试: task_runner x4 + ground_link/virtual_pod*/bridge/px4_gateway 增量; 全量 483 passed - 文档: 预定流程(需求) + 预定流程-操作指南(操作/待办) + L1方案补S1结论与实现状态
312 lines
9.3 KiB
Python
312 lines
9.3 KiB
Python
"""runner_test.py —— 任务编排器单元测试（mock px4/bridge/ground/drone，无硬件）。
|
||
|
||
覆盖：输入采集（注入坐标新鲜/超龄、起飞到位、锁定、居中累加）、
|
||
状态进入动作（ARM_TAKEOFF 触发起飞、GOTO 切 L2、TRACK 切 L1）。
|
||
"""
|
||
|
||
import asyncio
|
||
from dataclasses import replace
|
||
|
||
import pytest
|
||
|
||
from pod_bridge.config import BridgeConfig
|
||
from pod_bridge.ground_link import GroundTarget
|
||
from pod_bridge.px4_gateway import OwnState
|
||
from pod_bridge.protocol import Telemetry
|
||
from task_runner.runner import TaskRunner
|
||
from task_runner.state_machine import (TaskState, TaskStateConfig,
|
||
TaskStateMachine)
|
||
|
||
|
||
# --- mocks ------------------------------------------------------------------
|
||
class FakePx4:
|
||
def __init__(self, state=None):
|
||
self._state = state or OwnState()
|
||
self.telemetry_started = 0
|
||
self.offboard_started = False
|
||
self.brake_calls = []
|
||
|
||
def own_state(self):
|
||
return replace(self._state)
|
||
|
||
async def start_telemetry(self):
|
||
self.telemetry_started += 1
|
||
|
||
async def stop_telemetry(self):
|
||
pass
|
||
|
||
async def start_offboard(self):
|
||
self.offboard_started = True
|
||
return True
|
||
|
||
async def stop_offboard(self):
|
||
pass
|
||
|
||
async def send_velocity_heading(self, out, t=None):
|
||
return True
|
||
|
||
async def send_brake_hover(self, t=None):
|
||
self.brake_calls.append(t)
|
||
return True
|
||
|
||
|
||
class FakeBridge:
|
||
def __init__(self):
|
||
self.started = False
|
||
self.stopped = False
|
||
self.law = None # set_heading_servo_enabled 最后值
|
||
self.steps = 0
|
||
self._locked = False
|
||
self._telemetry = None
|
||
|
||
async def start(self):
|
||
self.started = True
|
||
return True
|
||
|
||
async def stop(self):
|
||
self.stopped = True
|
||
|
||
def set_heading_servo_enabled(self, enabled):
|
||
self.law = enabled
|
||
|
||
def pod_locked(self, now_ms):
|
||
return self._locked
|
||
|
||
async def step(self, now_ms):
|
||
self.steps += 1
|
||
|
||
@property
|
||
def latest_telemetry(self):
|
||
return self._telemetry
|
||
|
||
|
||
class FakeGround:
|
||
def __init__(self, target=None):
|
||
self._target = target
|
||
|
||
def latest(self):
|
||
return self._target or GroundTarget()
|
||
|
||
|
||
class FakeDrone:
|
||
def __init__(self):
|
||
self.armed = False
|
||
self.took_off = False
|
||
self.takeoff_alt = 0.0
|
||
self.rtl = False
|
||
self.landed = False
|
||
self.disarmed = False
|
||
|
||
class _Action:
|
||
def __init__(self, d):
|
||
self._d = d
|
||
|
||
async def arm(self):
|
||
self._d.armed = True
|
||
|
||
async def set_takeoff_altitude(self, a):
|
||
self._d.takeoff_alt = a
|
||
|
||
async def takeoff(self):
|
||
self._d.took_off = True
|
||
|
||
async def return_to_launch(self):
|
||
self._d.rtl = True
|
||
|
||
async def land(self):
|
||
self._d.landed = True
|
||
|
||
async def disarm(self):
|
||
self._d.disarmed = True
|
||
|
||
@property
|
||
def action(self):
|
||
return FakeDrone._Action(self)
|
||
|
||
|
||
def _state(now_ms, lat=0.0, lon=0.0, rel_alt=0.0):
|
||
return OwnState(latitude_deg=lat, longitude_deg=lon,
|
||
relative_altitude_m=rel_alt, 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 _make_runner(px4=None, bridge=None, ground=None, drone=None,
|
||
sm_cfg=None, vision_fn=None, pod_control=None):
|
||
cfg = BridgeConfig()
|
||
cfg.ground_inject.enabled = True
|
||
cfg.ground_inject.timeout_ms = 5000
|
||
return TaskRunner(
|
||
cfg, sm_cfg or TaskStateConfig(),
|
||
px4 or FakePx4(_state(1000)),
|
||
bridge or FakeBridge(),
|
||
TaskStateMachine(sm_cfg or TaskStateConfig()),
|
||
ground or FakeGround(),
|
||
drone or FakeDrone(),
|
||
vision_fn=vision_fn,
|
||
now_ms_fn=lambda: 1000,
|
||
pod_control=pod_control,
|
||
)
|
||
|
||
|
||
# --- 输入采集 ---------------------------------------------------------------
|
||
def test_collect_injected_fresh_sets_distance():
|
||
ground = FakeGround(GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
|
||
time_ms=1000))
|
||
px4 = FakePx4(_state(1000))
|
||
r = _make_runner(px4=px4, ground=ground)
|
||
|
||
inp = r._collect_inputs(1000)
|
||
assert inp.target_injected is True
|
||
assert inp.distance_to_target_m == pytest.approx(111.19, rel=1e-2) # ~111m
|
||
assert inp.takeoff_reached is False # rel_alt=0 < 5
|
||
|
||
|
||
def test_collect_injected_stale_is_invalid():
|
||
ground = FakeGround(GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
|
||
time_ms=1000))
|
||
r = _make_runner(ground=ground) # timeout=5000
|
||
|
||
inp = r._collect_inputs(7000) # 6000ms 差值 > 5000 → 超龄
|
||
assert inp.target_injected is False
|
||
assert inp.distance_to_target_m == float("inf")
|
||
|
||
|
||
def test_collect_takeoff_reached():
|
||
px4 = FakePx4(_state(1000, rel_alt=5.0))
|
||
sm_cfg = TaskStateConfig(takeoff_altitude_m=5.0)
|
||
r = _make_runner(px4=px4, sm_cfg=sm_cfg)
|
||
|
||
assert r._collect_inputs(1000).takeoff_reached is True
|
||
|
||
|
||
def test_collect_locked_from_bridge():
|
||
bridge = FakeBridge()
|
||
bridge._locked = True
|
||
r = _make_runner(bridge=bridge)
|
||
|
||
assert r._collect_inputs(1000).target_locked is True
|
||
|
||
|
||
def test_collect_vision_second_judgement():
|
||
r = _make_runner(vision_fn=lambda: True)
|
||
assert r._collect_inputs(1000).vision_has_target is True
|
||
|
||
r2 = _make_runner(vision_fn=lambda: False)
|
||
assert r2._collect_inputs(1000).vision_has_target is False
|
||
|
||
|
||
# --- 居中累加 ---------------------------------------------------------------
|
||
def test_centered_accumulates_duration():
|
||
bridge = FakeBridge()
|
||
bridge._telemetry = (Telemetry(off_target_x_deg=0.1, off_target_y_deg=0.1), 1000)
|
||
sm_cfg = TaskStateConfig(center_threshold_deg=1.0)
|
||
r = _make_runner(bridge=bridge, sm_cfg=sm_cfg)
|
||
|
||
assert r._update_centered(1000) == (True, 0.0) # 开始居中
|
||
assert r._update_centered(3000) == (True, 2.0) # 持续 2s
|
||
assert r._update_centered(3500) == (True, 2.5) # 持续 2.5s
|
||
|
||
|
||
def test_centered_resets_on_large_offset():
|
||
bridge = FakeBridge()
|
||
bridge._telemetry = (Telemetry(off_target_x_deg=0.1, off_target_y_deg=0.1), 1000)
|
||
r = _make_runner(bridge=bridge)
|
||
|
||
r._update_centered(1000)
|
||
bridge._telemetry = (Telemetry(off_target_x_deg=5.0, off_target_y_deg=0.1), 2000)
|
||
assert r._update_centered(2000) == (False, 0.0) # 脱靶量大 → 清零
|
||
|
||
|
||
def test_centered_no_telemetry_returns_false():
|
||
r = _make_runner() # bridge.latest_telemetry is None
|
||
assert r._update_centered(1000) == (False, 0.0)
|
||
|
||
|
||
# --- 状态进入动作 -----------------------------------------------------------
|
||
def test_on_enter_arm_takeoff_arms_and_takes_off():
|
||
drone = FakeDrone()
|
||
r = _make_runner(drone=drone, sm_cfg=TaskStateConfig(takeoff_altitude_m=5.0))
|
||
asyncio.run(r._on_enter(TaskState.ARM_TAKEOFF, 1000))
|
||
assert drone.armed is True
|
||
assert drone.took_off is True
|
||
assert drone.takeoff_alt == pytest.approx(5.0)
|
||
|
||
|
||
def test_on_enter_goto_switches_to_l2():
|
||
bridge = FakeBridge()
|
||
r = _make_runner(bridge=bridge)
|
||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||
assert bridge.started is True
|
||
assert bridge.law is False # L2 地理引导
|
||
|
||
|
||
def test_on_enter_track_switches_to_l1():
|
||
bridge = FakeBridge()
|
||
r = _make_runner(bridge=bridge)
|
||
asyncio.run(r._on_enter(TaskState.TRACK, 1000))
|
||
assert bridge.law is True # L1 航向伺服
|
||
|
||
|
||
def test_on_enter_rtl_stops_bridge_and_returns():
|
||
bridge = FakeBridge()
|
||
drone = FakeDrone()
|
||
r = _make_runner(bridge=bridge, drone=drone)
|
||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|
||
assert bridge.stopped is True
|
||
assert drone.rtl is True
|
||
|
||
|
||
# --- 持续动作 ---------------------------------------------------------------
|
||
def test_tick_goto_steps_bridge():
|
||
bridge = FakeBridge()
|
||
r = _make_runner(bridge=bridge)
|
||
asyncio.run(r._tick(1000))
|
||
# WAIT 状态不 step；切到 GOTO 才 step
|
||
r._sm._state = TaskState.GOTO
|
||
asyncio.run(r._tick(1000))
|
||
assert bridge.steps == 1
|
||
|
||
|
||
def test_tick_approach_brakes():
|
||
px4 = FakePx4(_state(1000))
|
||
r = _make_runner(px4=px4)
|
||
r._sm._state = TaskState.APPROACH
|
||
asyncio.run(r._tick(1000))
|
||
assert len(px4.brake_calls) == 1
|
||
|
||
|
||
# --- 吊舱控制集成（真机）----------------------------------------------------
|
||
class FakePodControl:
|
||
def __init__(self):
|
||
self.armed = False
|
||
self.stopped = False
|
||
|
||
def arm_detection(self):
|
||
self.armed = True
|
||
|
||
def stop_tracking(self):
|
||
self.stopped = True
|
||
|
||
|
||
def test_on_enter_goto_arms_detection():
|
||
pod_control = FakePodControl()
|
||
r = _make_runner(pod_control=pod_control)
|
||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||
assert pod_control.armed is True
|
||
assert pod_control.stopped is False
|
||
|
||
|
||
def test_on_enter_rtl_stops_tracking_before_teardown():
|
||
pod_control = FakePodControl()
|
||
r = _make_runner(pod_control=pod_control)
|
||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|
||
assert pod_control.stopped is True
|
||
|
||
|
||
def test_no_pod_control_is_safe():
|
||
"""未注入 pod_control（SITL）→ GOTO/RTL 进入动作不触发吊舱控制、不抛异常。"""
|
||
r = _make_runner(pod_control=None)
|
||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|