精简仓库结构并增强任务链路：归档双目可选后端、新增LOS判据与自定义返航
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
This commit is contained in:
keita
@@ -1,8 +1,12 @@
|
||||
"""state_machine_test.py —— 任务级状态机单元测试。
|
||||
|
||||
覆盖：完整状态链 WAIT→ARM_TAKEOFF→GOTO→TRACK→APPROACH→RTL、各状态超时、
|
||||
GOTO→TRACK 的锁定/视觉守卫、TRACK 丢目标退回 GOTO、紧急/返航全局终止、
|
||||
RTL 终态与 reset。
|
||||
覆盖（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
|
||||
@@ -19,6 +23,25 @@ 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
|
||||
@@ -32,6 +55,13 @@ def test_reset_returns_to_wait():
|
||||
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)
|
||||
@@ -60,12 +90,11 @@ 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 -------------------------------------------------------------------
|
||||
# --- GOTO：只按「到参考点半径」退出（不看锁定）-------------------------------
|
||||
def test_goto_far_away_stays():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
@@ -76,45 +105,31 @@ def test_goto_far_away_stays():
|
||||
assert sm.state is TaskState.GOTO
|
||||
|
||||
|
||||
def test_goto_within_radius_and_locked_goes_track():
|
||||
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.TRACK
|
||||
assert tr.occurred and tr.to_state is TaskState.SEARCH
|
||||
assert "参考点" in tr.reason
|
||||
|
||||
|
||||
def test_goto_within_radius_but_unlocked_stays():
|
||||
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 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
|
||||
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=10.0), 3000)
|
||||
tr = sm.update(_inp(target_injected=False, distance_to_target_m=100.0), 3000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
@@ -123,87 +138,113 @@ def test_goto_timeout_goes_rtl():
|
||||
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)
|
||||
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)
|
||||
# --- 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
|
||||
# 居中稳定满 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
|
||||
assert sm.state is TaskState.SEARCH
|
||||
|
||||
|
||||
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)
|
||||
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=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)
|
||||
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
|
||||
|
||||
|
||||
# --- APPROACH ---------------------------------------------------------------
|
||||
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(center_hold_s=0.0, approach_hover_s=10.0)
|
||||
cfg = TaskStateConfig(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)
|
||||
sm.force_state(TaskState.APPROACH, 1000)
|
||||
tr = sm.update(_inp(), 1000 + 5000)
|
||||
assert not tr.occurred
|
||||
# 满 10s → RTL
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 10_000)
|
||||
tr = sm.update(_inp(), 1000 + 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)
|
||||
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
|
||||
|
||||
|
||||
@@ -218,7 +259,6 @@ 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
|
||||
@@ -231,3 +271,11 @@ def test_transition_dataclass_defaults():
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user