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结论与实现状态
234 lines
9.0 KiB
Python
234 lines
9.0 KiB
Python
"""state_machine_test.py —— 任务级状态机单元测试。
|
|
|
|
覆盖：完整状态链 WAIT→ARM_TAKEOFF→GOTO→TRACK→APPROACH→RTL、各状态超时、
|
|
GOTO→TRACK 的锁定/视觉守卫、TRACK 丢目标退回 GOTO、紧急/返航全局终止、
|
|
RTL 终态与 reset。
|
|
"""
|
|
|
|
import pytest
|
|
|
|
from task_runner.state_machine import (TaskInputs, TaskState, TaskStateConfig,
|
|
TaskStateMachine, TaskTransition)
|
|
|
|
|
|
def _inp(**kw) -> TaskInputs:
|
|
return TaskInputs(**kw)
|
|
|
|
|
|
def _sm(cfg=None) -> TaskStateMachine:
|
|
return TaskStateMachine(cfg or TaskStateConfig())
|
|
|
|
|
|
# --- 初始态与 reset ---------------------------------------------------------
|
|
def test_initial_state_is_wait():
|
|
assert _sm().state is TaskState.WAIT
|
|
|
|
|
|
def test_reset_returns_to_wait():
|
|
sm = _sm()
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
assert sm.state is TaskState.ARM_TAKEOFF
|
|
sm.reset()
|
|
assert sm.state is TaskState.WAIT
|
|
|
|
|
|
# --- WAIT -------------------------------------------------------------------
|
|
def test_wait_without_coords_stays():
|
|
tr = _sm().update(_inp(), 1000)
|
|
assert not tr.occurred
|
|
assert tr.to_state is TaskState.WAIT
|
|
|
|
|
|
def test_wait_receives_coords_goes_arm_takeoff():
|
|
sm = _sm()
|
|
tr = sm.update(_inp(target_injected=True), 1000)
|
|
assert tr.occurred
|
|
assert tr.from_state is TaskState.WAIT
|
|
assert tr.to_state is TaskState.ARM_TAKEOFF
|
|
assert sm.state is TaskState.ARM_TAKEOFF
|
|
|
|
|
|
# --- ARM_TAKEOFF ------------------------------------------------------------
|
|
def test_arm_takeoff_reaches_altitude_goes_goto():
|
|
sm = _sm()
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
tr = sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
|
assert tr.occurred and tr.to_state is TaskState.GOTO
|
|
|
|
|
|
def test_arm_takeoff_timeout_goes_rtl():
|
|
cfg = TaskStateConfig(arm_takeoff_timeout_s=60.0)
|
|
sm = _sm(cfg)
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
# 60s 未到高度 → 超时返航
|
|
tr = sm.update(_inp(target_injected=True), 1000 + 61_000)
|
|
assert tr.occurred and tr.to_state is TaskState.RTL
|
|
|
|
|
|
# --- GOTO -------------------------------------------------------------------
|
|
def test_goto_far_away_stays():
|
|
sm = _sm()
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
|
tr = sm.update(_inp(target_injected=True, distance_to_target_m=100.0,
|
|
target_locked=True), 3000)
|
|
assert not tr.occurred
|
|
assert sm.state is TaskState.GOTO
|
|
|
|
|
|
def test_goto_within_radius_and_locked_goes_track():
|
|
sm = _sm()
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
|
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
|
target_locked=True), 3000)
|
|
assert tr.occurred and tr.to_state is TaskState.TRACK
|
|
|
|
|
|
def test_goto_within_radius_but_unlocked_stays():
|
|
sm = _sm()
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
|
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
|
target_locked=False), 3000)
|
|
assert not tr.occurred
|
|
assert sm.state is TaskState.GOTO
|
|
|
|
|
|
def test_goto_requires_vision_when_enabled():
|
|
cfg = TaskStateConfig(require_vision=True)
|
|
sm = _sm(cfg)
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
|
# 有锁定但无视觉第二判据 → 不切换
|
|
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
|
target_locked=True, vision_has_target=False), 3000)
|
|
assert not tr.occurred
|
|
# 补上视觉 → 切换
|
|
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
|
target_locked=True, vision_has_target=True), 4000)
|
|
assert tr.occurred and tr.to_state is TaskState.TRACK
|
|
|
|
|
|
def test_goto_loses_coords_goes_rtl():
|
|
sm = _sm()
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
|
tr = sm.update(_inp(target_injected=False, distance_to_target_m=10.0), 3000)
|
|
assert tr.occurred and tr.to_state is TaskState.RTL
|
|
|
|
|
|
def test_goto_timeout_goes_rtl():
|
|
cfg = TaskStateConfig(goto_timeout_s=120.0)
|
|
sm = _sm(cfg)
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
|
tr = sm.update(_inp(target_injected=True, distance_to_target_m=100.0), 2000 + 121_000)
|
|
assert tr.occurred and tr.to_state is TaskState.RTL
|
|
|
|
|
|
# --- TRACK ------------------------------------------------------------------
|
|
def test_track_centered_stable_goes_approach():
|
|
sm = _sm(TaskStateConfig(center_hold_s=3.0))
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
|
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
|
target_locked=True), 3000)
|
|
assert sm.state is TaskState.TRACK
|
|
# 居中但未满时长 → 停留
|
|
tr = sm.update(_inp(target_injected=True, target_locked=True,
|
|
target_centered=True, centered_duration_s=1.0), 4000)
|
|
assert not tr.occurred
|
|
# 居中稳定满 3s → APPROACH
|
|
tr = sm.update(_inp(target_injected=True, target_locked=True,
|
|
target_centered=True, centered_duration_s=3.0), 5000)
|
|
assert tr.occurred and tr.to_state is TaskState.APPROACH
|
|
|
|
|
|
def test_track_lost_with_coords_goes_back_to_goto():
|
|
sm = _sm()
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
|
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
|
target_locked=True), 3000)
|
|
tr = sm.update(_inp(target_injected=True, target_locked=False), 4000)
|
|
assert tr.occurred and tr.to_state is TaskState.GOTO
|
|
|
|
|
|
def test_track_lost_without_coords_goes_rtl():
|
|
sm = _sm()
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
|
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
|
target_locked=True), 3000)
|
|
tr = sm.update(_inp(target_injected=False, target_locked=False), 4000)
|
|
assert tr.occurred and tr.to_state is TaskState.RTL
|
|
|
|
|
|
def test_track_timeout_goes_rtl():
|
|
cfg = TaskStateConfig(track_timeout_s=120.0)
|
|
sm = _sm(cfg)
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
|
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
|
target_locked=True), 3000)
|
|
tr = sm.update(_inp(target_injected=True, target_locked=True), 3000 + 121_000)
|
|
assert tr.occurred and tr.to_state is TaskState.RTL
|
|
|
|
|
|
# --- APPROACH ---------------------------------------------------------------
|
|
def test_approach_hover_done_goes_rtl():
|
|
cfg = TaskStateConfig(center_hold_s=0.0, approach_hover_s=10.0)
|
|
sm = _sm(cfg)
|
|
sm.update(_inp(target_injected=True), 1000)
|
|
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
|
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
|
target_locked=True), 3000)
|
|
sm.update(_inp(target_injected=True, target_locked=True, target_centered=True,
|
|
centered_duration_s=0.0), 4000)
|
|
assert sm.state is TaskState.APPROACH
|
|
# 悬停未满 10s → 停留
|
|
tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 5000)
|
|
assert not tr.occurred
|
|
# 满 10s → RTL
|
|
tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 10_000)
|
|
assert tr.occurred and tr.to_state is TaskState.RTL
|
|
|
|
|
|
# --- 全局终止（紧急 / 返航）-------------------------------------------------
|
|
@pytest.mark.parametrize("state_kw", [
|
|
{}, # WAIT
|
|
{"target_injected": True}, # 会先转 ARM_TAKEOFF
|
|
])
|
|
def test_emergency_goes_rtl_from_any_state(state_kw):
|
|
sm = _sm()
|
|
sm.update(_inp(**state_kw), 1000)
|
|
tr = sm.update(_inp(emergency=True), 2000)
|
|
assert tr.occurred and tr.to_state is TaskState.RTL
|
|
|
|
|
|
def test_rtl_requested_goes_rtl():
|
|
sm = _sm()
|
|
tr = sm.update(_inp(rtl_requested=True), 1000)
|
|
assert tr.occurred and tr.to_state is TaskState.RTL
|
|
|
|
|
|
# --- RTL 终态 ---------------------------------------------------------------
|
|
def test_rtl_is_terminal():
|
|
sm = _sm()
|
|
sm.update(_inp(rtl_requested=True), 1000)
|
|
assert sm.state is TaskState.RTL
|
|
# 终态不再转移（连紧急/坐标也不动）
|
|
tr = sm.update(_inp(target_injected=True, emergency=True), 2000)
|
|
assert not tr.occurred
|
|
assert sm.state is TaskState.RTL
|
|
|
|
|
|
# --- 转移结果结构 -----------------------------------------------------------
|
|
def test_transition_dataclass_defaults():
|
|
tr = TaskTransition()
|
|
assert tr.from_state is TaskState.WAIT
|
|
assert tr.to_state is TaskState.WAIT
|
|
assert tr.occurred is False
|
|
assert tr.reason == ""
|