"""state_machine_test.py —— 任务级状态机单元测试。 覆盖（2026-10-15 按「到参考点 → 视觉搜索 → 缓慢靠近」重构后的契约）： * 完整状态链 WAIT→ARM_TAKEOFF→GOTO→SEARCH→TRACK→(超时)RTL * GOTO 只按「到参考点半径」退出（不看锁定）；SEARCH 才看锁定/视觉守卫 * 搜索超时 → RTL；追踪（靠近）超时 → RTL * TRACK 丢目标 → 回 SEARCH（不回 GOTO）；坐标同时失效 → RTL * TRACK **不**因「目标居中」退出（居中即应继续靠近） * 紧急/返航全局终止、RTL 终态与 reset、force_state 兜底 """ 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()) def _to_search(cfg=None, t0: int = 1000): """推进到 SEARCH 态：WAIT→ARM_TAKEOFF→GOTO→SEARCH。返回 (sm, now_ms)。""" sm = _sm(cfg) sm.update(_inp(target_injected=True), t0) sm.update(_inp(target_injected=True, takeoff_reached=True), t0 + 1000) tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0), t0 + 2000) assert tr.to_state is TaskState.SEARCH return sm, t0 + 2000 def _to_track(cfg=None, t0: int = 1000): """推进到 TRACK 态：WAIT→…→SEARCH→TRACK。返回 (sm, now_ms)。""" sm, t = _to_search(cfg, t0) tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0, target_locked=True), t + 1000) assert tr.to_state is TaskState.TRACK return sm, t + 1000 # --- 初始态与 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 def test_force_state_jumps_without_transition(): """force_state 供异常兜底直接落状态（不产生 TaskTransition）。""" sm = _sm() sm.force_state(TaskState.RTL, 5000) assert sm.state is TaskState.RTL # --- 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) 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_goes_search_even_if_locked(): """到参考点即转搜索——参考点是搜索起点，锁定与否不影响该转移。""" 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.SEARCH assert "参考点" in tr.reason def test_goto_within_radius_unlocked_still_goes_search(): 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 tr.occurred and tr.to_state is TaskState.SEARCH 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=100.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 # --- SEARCH：搜索发现目标才进 TRACK ----------------------------------------- def test_search_sees_target_goes_track(): sm, t = _to_search() tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0, target_locked=True), t + 1000) assert tr.occurred and tr.to_state is TaskState.TRACK assert "发现目标" in tr.reason def test_search_without_target_stays(): sm, t = _to_search() tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0, target_locked=False), t + 1000) assert not tr.occurred assert sm.state is TaskState.SEARCH def test_search_timeout_goes_rtl(): cfg = TaskStateConfig(search_timeout_s=20.0) sm, t = _to_search(cfg) tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0), t + 21_000) assert tr.occurred and tr.to_state is TaskState.RTL assert "搜索超时" in tr.reason def test_search_loses_coords_goes_rtl(): sm, t = _to_search() tr = sm.update(_inp(target_injected=False), t + 1000) assert tr.occurred and tr.to_state is TaskState.RTL # --- TRACK：缓慢靠近，只受超时/返航/紧急控制 --------------------------------- def test_track_centered_does_not_exit(): """目标居中**不再**触发退出：居中正是应当继续缓慢靠近的时刻。""" sm, t = _to_track() tr = sm.update(_inp(target_injected=True, target_locked=True, target_centered=True, centered_duration_s=99.0), t + 1000) assert not tr.occurred assert sm.state is TaskState.TRACK def test_track_holds_while_locked(): sm, t = _to_track() for dt in (1000, 2000, 3000): tr = sm.update(_inp(target_injected=True, target_locked=True), t + dt) assert not tr.occurred assert sm.state is TaskState.TRACK def test_track_timeout_goes_rtl(): cfg = TaskStateConfig(track_timeout_s=20.0) sm, t = _to_track(cfg) tr = sm.update(_inp(target_injected=True, target_locked=True), t + 21_000) assert tr.occurred and tr.to_state is TaskState.RTL assert "超时" in tr.reason def test_track_lost_with_coords_goes_back_to_search(): """丢目标回 SEARCH 重新扫视（不回 GOTO：飞机本就在参考点附近）。""" sm, t = _to_track() tr = sm.update(_inp(target_injected=True, target_locked=False), t + 1000) assert tr.occurred and tr.to_state is TaskState.SEARCH assert "重新搜索" in tr.reason def test_track_lost_without_coords_goes_rtl(): sm, t = _to_track() tr = sm.update(_inp(target_injected=False, target_locked=False), t + 1000) assert tr.occurred and tr.to_state is TaskState.RTL def test_reacquire_after_loss_returns_to_track(): """丢目标→SEARCH→重新锁定→TRACK 全链路可循环。""" sm, t = _to_track() tr = sm.update(_inp(target_injected=True, target_locked=False), t + 1000) assert tr.to_state is TaskState.SEARCH tr = sm.update(_inp(target_injected=True, target_locked=True), t + 2000) assert tr.to_state is TaskState.TRACK # --- APPROACH（保留态：真机可选收尾）---------------------------------------- def test_approach_hover_done_goes_rtl(): cfg = TaskStateConfig(approach_hover_s=10.0) sm = _sm(cfg) sm.force_state(TaskState.APPROACH, 1000) tr = sm.update(_inp(), 1000 + 5000) assert not tr.occurred tr = sm.update(_inp(), 1000 + 10_000) assert tr.occurred and tr.to_state is TaskState.RTL # --- 全局终止（紧急 / 返航）------------------------------------------------- def test_emergency_from_search_goes_rtl(): sm, t = _to_search() tr = sm.update(_inp(emergency=True), t + 1000) assert tr.occurred and tr.to_state is TaskState.RTL def test_emergency_from_track_goes_rtl(): sm, t = _to_track() tr = sm.update(_inp(target_locked=True, emergency=True), t + 1000) 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 == "" # --- 配置默认值（搜索/靠近超时口径）------------------------------------------ def test_default_config_has_search_and_pursuit_timeouts(): cfg = TaskStateConfig() assert cfg.search_timeout_s == pytest.approx(20.0) assert cfg.track_timeout_s == pytest.approx(20.0) assert cfg.sweep_rate_dps > 0.0