精简仓库结构并增强任务链路：归档双目可选后端、新增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
@@ -0,0 +1,299 @@
|
||||
"""app_test.py —— 地面站 Flask 端点测试（test_client + 真实 UDP 回环监听）。
|
||||
|
||||
覆盖：POST /api/inject 合法/非法（UDP 实收断言）、局部坐标注入换算与拒绝、
|
||||
POST /api/command 白名单校验与 UDP 下发、POST /api/continuous 服务端持续注入启停、
|
||||
GET /api/link 链路目标读写、GET /api/state 空日志目录兜底与 continuous 标志。
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
import socket
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
_ROOT = Path(__file__).resolve().parents[3] # utils/tests/ground_station → 仓库根
|
||||
if str(_ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(_ROOT))
|
||||
|
||||
from ground_station.app import create_app # noqa: E402
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def udp_listener():
|
||||
"""真实 UDP 回环监听：返回 (port, recv_one)。"""
|
||||
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
s.bind(("127.0.0.1", 0))
|
||||
s.settimeout(1.5)
|
||||
|
||||
def recv_one():
|
||||
data, _addr = s.recvfrom(2048)
|
||||
return json.loads(data.decode("utf-8"))
|
||||
|
||||
yield s.getsockname()[1], recv_one
|
||||
s.close()
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def client(tmp_path, udp_listener):
|
||||
port, _recv = udp_listener
|
||||
app = create_app(udp_port=port, log_dir=str(tmp_path),
|
||||
inject_interval_s=0.05) # 测试用短周期
|
||||
app.config["TESTING"] = True
|
||||
return app.test_client()
|
||||
|
||||
|
||||
# --- /api/inject ------------------------------------------------------------
|
||||
def test_inject_ok_sends_udp(client, udp_listener):
|
||||
_port, recv_one = udp_listener
|
||||
r = client.post("/api/inject", json={"lat": 1.5, "lon": 2.5, "alt": 10})
|
||||
assert r.get_json()["ok"] is True
|
||||
assert recv_one() == {"lat": 1.5, "lon": 2.5, "alt": 10.0}
|
||||
|
||||
|
||||
def test_inject_missing_lat_400(client):
|
||||
r = client.post("/api/inject", json={"lon": 2.5})
|
||||
assert r.status_code == 400
|
||||
assert r.get_json()["ok"] is False
|
||||
|
||||
|
||||
def test_inject_non_numeric_400(client):
|
||||
r = client.post("/api/inject", json={"lat": "x", "lon": 2.5})
|
||||
assert r.status_code == 400
|
||||
|
||||
|
||||
# --- 局部坐标注入 -------------------------------------------------------------
|
||||
_TEL_HEADER = ("t_s,state,pos_valid,fix_type,num_satellites,latitude_deg,"
|
||||
"longitude_deg,relative_altitude_m,yaw_deg,target_injected,"
|
||||
"distance_to_target_m,target_locked,off_target_x_deg,"
|
||||
"off_target_y_deg,mode,yaw_rate_dps,vx_body_mps,vz_mps\n")
|
||||
|
||||
|
||||
def _write_telemetry(log_dir, lat=47.0, lon=8.0, pos_valid="True"):
|
||||
with open(Path(log_dir) / "task_telemetry.csv", "w", encoding="utf-8") as f:
|
||||
f.write(_TEL_HEADER)
|
||||
f.write(f"1.0,wait,{pos_valid},3,10,{lat},{lon},0,0,False,inf,False,"
|
||||
"0,0,,0,0,0\n")
|
||||
|
||||
|
||||
def test_inject_local_converts_from_vehicle_position(client, udp_listener, tmp_path):
|
||||
_write_telemetry(tmp_path, lat=47.0, lon=8.0)
|
||||
_port, recv_one = udp_listener
|
||||
r = client.post("/api/inject", json={"north_m": 30, "east_m": 15, "alt": 0})
|
||||
j = r.get_json()
|
||||
assert j["ok"] is True
|
||||
exp_lat = 47.0 + 30 / 111320.0
|
||||
exp_lon = 8.0 + 15 / (111320.0 * math.cos(math.radians(47.0)))
|
||||
msg = recv_one()
|
||||
assert msg["lat"] == pytest.approx(exp_lat, abs=1e-9)
|
||||
assert msg["lon"] == pytest.approx(exp_lon, abs=1e-9)
|
||||
assert j["local"]["north_m"] == 30 and j["local"]["east_m"] == 15
|
||||
assert j["local"]["origin_lat"] == pytest.approx(47.0)
|
||||
|
||||
|
||||
def test_inject_local_rejected_without_telemetry(client):
|
||||
r = client.post("/api/inject", json={"north_m": 30, "east_m": 15})
|
||||
assert r.status_code == 400
|
||||
assert "无法换算" in r.get_json()["error"]
|
||||
|
||||
|
||||
def test_inject_local_rejected_when_pos_invalid(client, tmp_path):
|
||||
_write_telemetry(tmp_path, pos_valid="False")
|
||||
r = client.post("/api/inject", json={"north_m": 30, "east_m": 15})
|
||||
assert r.status_code == 400
|
||||
|
||||
|
||||
def test_inject_local_bad_numbers_400(client, tmp_path):
|
||||
_write_telemetry(tmp_path)
|
||||
assert client.post("/api/inject",
|
||||
json={"north_m": "x", "east_m": 15}).status_code == 400
|
||||
|
||||
|
||||
def test_inject_local_home_origin_uses_session_start(client, udp_listener, tmp_path):
|
||||
# 同一段：起始有效定位 47.0/8.0（≈起飞点），之后移动到 47.5/8.5
|
||||
with open(tmp_path / "task_telemetry.csv", "w", encoding="utf-8") as f:
|
||||
f.write(_TEL_HEADER)
|
||||
f.write("1.0,wait,True,3,10,47.0,8.0,0,0,False,inf,False,0,0,,0,0,0\n")
|
||||
f.write("60.0,goto,True,3,10,47.5,8.5,0,0,False,inf,False,0,0,,0,0,0\n")
|
||||
_port, recv_one = udp_listener
|
||||
|
||||
j = client.post("/api/inject",
|
||||
json={"north_m": 0, "east_m": 0, "origin": "home"}).get_json()
|
||||
assert j["ok"] is True and j["local"]["origin"] == "home"
|
||||
assert j["local"]["origin_lat"] == pytest.approx(47.0) # 段首而非最后行
|
||||
assert j["local"]["origin_lon"] == pytest.approx(8.0)
|
||||
recv_one()
|
||||
|
||||
j2 = client.post("/api/inject",
|
||||
json={"north_m": 0, "east_m": 0}).get_json() # current
|
||||
assert j2["local"]["origin"] == "current"
|
||||
assert j2["local"]["origin_lat"] == pytest.approx(47.5) # 当前位置
|
||||
recv_one()
|
||||
|
||||
|
||||
def test_inject_local_home_origin_skips_invalid_rows(client, tmp_path):
|
||||
with open(tmp_path / "task_telemetry.csv", "w", encoding="utf-8") as f:
|
||||
f.write(_TEL_HEADER)
|
||||
f.write("0.5,wait,False,0,0,0.0,0.0,0,0,False,inf,False,0,0,,0,0,0\n")
|
||||
f.write("1.0,wait,True,3,10,47.0,8.0,0,0,False,inf,False,0,0,,0,0,0\n")
|
||||
j = client.post("/api/inject",
|
||||
json={"north_m": 30, "east_m": 15,
|
||||
"origin": "home"}).get_json()
|
||||
assert j["ok"] is True
|
||||
assert j["local"]["origin_lat"] == pytest.approx(47.0) # 取首个有效定位
|
||||
|
||||
|
||||
def test_inject_local_bad_origin_400(client, tmp_path):
|
||||
_write_telemetry(tmp_path)
|
||||
r = client.post("/api/inject",
|
||||
json={"north_m": 1, "east_m": 1, "origin": "mars"})
|
||||
assert r.status_code == 400
|
||||
|
||||
|
||||
def test_inject_local_home_origin_rejected_without_telemetry(client):
|
||||
r = client.post("/api/inject",
|
||||
json={"north_m": 30, "east_m": 15, "origin": "home"})
|
||||
assert r.status_code == 400
|
||||
assert "起飞点" in r.get_json()["error"]
|
||||
|
||||
|
||||
# --- /api/command -----------------------------------------------------------
|
||||
def test_command_whitelist_ok_sends_udp(client, udp_listener):
|
||||
_port, recv_one = udp_listener
|
||||
r = client.post("/api/command", json={"cmd": "rtl"})
|
||||
assert r.get_json() == {"ok": True, "sent": {"cmd": "rtl"}}
|
||||
assert recv_one() == {"cmd": "rtl"}
|
||||
|
||||
|
||||
def test_command_all_whitelist_entries_accepted(client, udp_listener):
|
||||
_port, recv_one = udp_listener
|
||||
for cmd in ("rtl", "emergency", "pod_detect_on", "pod_auto_lock",
|
||||
"pod_stop_track", "pod_detect_off", "pod_center"):
|
||||
r = client.post("/api/command", json={"cmd": cmd})
|
||||
assert r.get_json()["ok"] is True
|
||||
assert recv_one() == {"cmd": cmd}
|
||||
|
||||
|
||||
def test_command_unknown_rejected_no_udp(client, udp_listener):
|
||||
_port, recv_one = udp_listener
|
||||
r = client.post("/api/command", json={"cmd": "reboot"})
|
||||
assert r.status_code == 400
|
||||
assert r.get_json()["ok"] is False
|
||||
with pytest.raises(socket.timeout):
|
||||
recv_one() # 未下发任何 UDP → 监听超时即证明
|
||||
|
||||
|
||||
def test_command_non_string_rejected(client):
|
||||
r = client.post("/api/command", json={"cmd": 1})
|
||||
assert r.status_code == 400
|
||||
|
||||
|
||||
# --- /api/continuous --------------------------------------------------------
|
||||
def test_continuous_requires_prior_inject(client):
|
||||
r = client.post("/api/continuous", json={"on": True})
|
||||
assert r.status_code == 400
|
||||
assert r.get_json()["ok"] is False
|
||||
|
||||
|
||||
def test_continuous_non_bool_rejected(client):
|
||||
r = client.post("/api/continuous", json={"on": "yes"})
|
||||
assert r.status_code == 400
|
||||
|
||||
|
||||
def test_continuous_sends_repeatedly_and_stops(client, udp_listener):
|
||||
_port, recv_one = udp_listener
|
||||
client.post("/api/inject", json={"lat": 1.0, "lon": 2.0})
|
||||
assert recv_one() == {"lat": 1.0, "lon": 2.0, "alt": 0.0} # 手动单发
|
||||
|
||||
r = client.post("/api/continuous", json={"on": True})
|
||||
assert r.get_json() == {"ok": True, "continuous": True}
|
||||
assert recv_one() == {"lat": 1.0, "lon": 2.0, "alt": 0.0} # 服务端自动重发
|
||||
assert recv_one() == {"lat": 1.0, "lon": 2.0, "alt": 0.0}
|
||||
|
||||
r = client.post("/api/continuous", json={"on": False})
|
||||
assert r.get_json() == {"ok": True, "continuous": False}
|
||||
time.sleep(0.2)
|
||||
while True: # 排空在途包
|
||||
try:
|
||||
recv_one()
|
||||
except socket.timeout:
|
||||
break
|
||||
with pytest.raises(socket.timeout):
|
||||
recv_one() # 停止后不再发
|
||||
|
||||
|
||||
def test_continuous_uses_latest_inject(client, udp_listener):
|
||||
_port, recv_one = udp_listener
|
||||
client.post("/api/inject", json={"lat": 1.0, "lon": 2.0})
|
||||
recv_one()
|
||||
client.post("/api/inject", json={"lat": 3.0, "lon": 4.0}) # 中途改坐标
|
||||
recv_one()
|
||||
|
||||
client.post("/api/continuous", json={"on": True})
|
||||
assert recv_one() == {"lat": 3.0, "lon": 4.0, "alt": 0.0}
|
||||
client.post("/api/continuous", json={"on": False})
|
||||
|
||||
|
||||
# --- /api/link --------------------------------------------------------------
|
||||
def test_link_get_default_and_options(client, udp_listener):
|
||||
port, _recv = udp_listener
|
||||
j = client.get("/api/link").get_json()
|
||||
assert j["host"] == "127.0.0.1"
|
||||
assert j["port"] == port
|
||||
assert any(o["host"] == "127.0.0.1" for o in j["options"])
|
||||
|
||||
|
||||
def test_link_set_bad_host_or_port_400(client):
|
||||
assert client.post("/api/link", json={"host": " "}).status_code == 400
|
||||
assert client.post("/api/link",
|
||||
json={"host": "1.2.3.4", "port": "x"}).status_code == 400
|
||||
assert client.post("/api/link",
|
||||
json={"host": "1.2.3.4", "port": 70000}).status_code == 400
|
||||
|
||||
|
||||
def test_link_switch_redirects_inject_and_command(client, udp_listener):
|
||||
_port_a, recv_a = udp_listener
|
||||
s_b = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
s_b.bind(("127.0.0.1", 0))
|
||||
s_b.settimeout(1.5)
|
||||
port_b = s_b.getsockname()[1]
|
||||
try:
|
||||
r = client.post("/api/link", json={"host": "127.0.0.1", "port": port_b})
|
||||
assert r.get_json() == {"ok": True, "host": "127.0.0.1", "port": port_b}
|
||||
|
||||
client.post("/api/inject", json={"lat": 1.0, "lon": 2.0})
|
||||
data, _addr = s_b.recvfrom(2048)
|
||||
assert json.loads(data.decode("utf-8")) == {"lat": 1.0, "lon": 2.0,
|
||||
"alt": 0.0}
|
||||
client.post("/api/command", json={"cmd": "rtl"})
|
||||
data, _addr = s_b.recvfrom(2048)
|
||||
assert json.loads(data.decode("utf-8")) == {"cmd": "rtl"}
|
||||
|
||||
with pytest.raises(socket.timeout):
|
||||
recv_a() # 旧目标不再收到任何包
|
||||
finally:
|
||||
s_b.close()
|
||||
|
||||
|
||||
# --- /api/state -------------------------------------------------------------
|
||||
def test_state_empty_log_dir(client):
|
||||
j = client.get("/api/state").get_json()
|
||||
assert j["state"] == "unknown"
|
||||
assert j["telemetry"] is None
|
||||
assert j["continuous"] is False
|
||||
|
||||
|
||||
def test_video_disabled_by_default(client):
|
||||
assert client.get("/api/state").get_json()["video"] == {
|
||||
"enabled": False, "online": False}
|
||||
assert client.get("/video_feed").status_code == 503
|
||||
|
||||
|
||||
def test_state_reports_continuous(client):
|
||||
client.post("/api/inject", json={"lat": 1.0, "lon": 2.0})
|
||||
client.post("/api/continuous", json={"on": True})
|
||||
assert client.get("/api/state").get_json()["continuous"] is True
|
||||
client.post("/api/continuous", json={"on": False})
|
||||
assert client.get("/api/state").get_json()["continuous"] is False
|
||||
@@ -0,0 +1,138 @@
|
||||
"""video_source_test.py —— RTSP→MJPEG 视频源单元测试（注入假采集器，不依赖真流）。
|
||||
|
||||
覆盖：拉帧→JPEG 编码缓存、宽度限制（等比缩放）、断线重连（重新调用工厂）、
|
||||
打开失败退避重试、online 判定、MJPEG 分块格式。
|
||||
"""
|
||||
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
_ROOT = Path(__file__).resolve().parents[3] # utils/tests/ground_station → 仓库根
|
||||
if str(_ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(_ROOT))
|
||||
|
||||
from ground_station.video_source import RtspVideoSource # noqa: E402
|
||||
|
||||
|
||||
class FakeCapture:
|
||||
"""假采集器：可注入读失败次数；帧为纯色小图。"""
|
||||
|
||||
def __init__(self, fail_reads=0, height=60, width=120):
|
||||
self.fail_reads = fail_reads
|
||||
self._frame = np.full((height, width, 3), 128, dtype=np.uint8)
|
||||
self.released = False
|
||||
|
||||
def isOpened(self):
|
||||
return True
|
||||
|
||||
def read(self):
|
||||
if self.fail_reads > 0:
|
||||
self.fail_reads -= 1
|
||||
return False, None
|
||||
return True, self._frame
|
||||
|
||||
def release(self):
|
||||
self.released = True
|
||||
|
||||
|
||||
def _wait(pred, timeout_s=4.0, step=0.02):
|
||||
t0 = time.monotonic()
|
||||
while time.monotonic() - t0 < timeout_s:
|
||||
if pred():
|
||||
return True
|
||||
time.sleep(step)
|
||||
return False
|
||||
|
||||
|
||||
def _decode(jpeg):
|
||||
import cv2
|
||||
return cv2.imdecode(np.frombuffer(jpeg, dtype=np.uint8), cv2.IMREAD_COLOR)
|
||||
|
||||
|
||||
def test_frames_encoded_and_online():
|
||||
src = RtspVideoSource("rtsp://fake", width=120, fps=50,
|
||||
capture_factory=lambda: FakeCapture())
|
||||
assert src.online is False # 尚无帧 → 离线
|
||||
src.start()
|
||||
try:
|
||||
assert _wait(lambda: src.latest()[1] is not None)
|
||||
seq, jpeg = src.latest()
|
||||
assert seq >= 1
|
||||
assert jpeg[:2] == b"\xff\xd8" # JPEG SOI 头
|
||||
assert _decode(jpeg).shape[1] == 120 # 宽度不超限
|
||||
assert src.online is True
|
||||
finally:
|
||||
src.stop()
|
||||
|
||||
|
||||
def test_downscale_to_max_width():
|
||||
src = RtspVideoSource("rtsp://fake", width=60, fps=50,
|
||||
capture_factory=lambda: FakeCapture(height=60, width=120))
|
||||
src.start()
|
||||
try:
|
||||
assert _wait(lambda: src.latest()[1] is not None)
|
||||
img = _decode(src.latest()[1])
|
||||
assert img.shape[1] == 60 and img.shape[0] == 30 # 等比缩到 60 宽
|
||||
finally:
|
||||
src.stop()
|
||||
|
||||
|
||||
def test_reconnect_after_read_failure():
|
||||
opened = []
|
||||
|
||||
def factory():
|
||||
cap = FakeCapture(fail_reads=(10 ** 6 if not opened else 0))
|
||||
opened.append(cap)
|
||||
return cap
|
||||
|
||||
src = RtspVideoSource("rtsp://fake", width=120, fps=50,
|
||||
capture_factory=factory)
|
||||
src.start()
|
||||
try:
|
||||
assert _wait(lambda: src.latest()[1] is not None, timeout_s=5.0)
|
||||
assert len(opened) >= 2 # 首采集器读失败 → 重新打开
|
||||
assert opened[0].released is True
|
||||
finally:
|
||||
src.stop()
|
||||
|
||||
|
||||
def test_open_failure_keeps_retrying():
|
||||
calls = []
|
||||
|
||||
class ClosedCapture:
|
||||
def isOpened(self):
|
||||
return False
|
||||
|
||||
def release(self):
|
||||
pass
|
||||
|
||||
def factory():
|
||||
calls.append(1)
|
||||
return ClosedCapture()
|
||||
|
||||
src = RtspVideoSource("rtsp://fake", capture_factory=factory)
|
||||
src.start()
|
||||
try:
|
||||
assert _wait(lambda: len(calls) >= 2, timeout_s=4.0) # 1s 退避后重试
|
||||
assert src.latest()[1] is None
|
||||
finally:
|
||||
src.stop()
|
||||
|
||||
|
||||
def test_mjpeg_stream_chunks():
|
||||
src = RtspVideoSource("rtsp://fake", width=120, fps=50,
|
||||
capture_factory=lambda: FakeCapture())
|
||||
src.start()
|
||||
try:
|
||||
assert _wait(lambda: src.latest()[1] is not None)
|
||||
gen = src.mjpeg_stream()
|
||||
chunk = next(gen)
|
||||
assert chunk.startswith(b"--frame\r\nContent-Type: image/jpeg\r\n")
|
||||
assert b"Content-Length: " in chunk
|
||||
assert chunk.endswith(b"\r\n")
|
||||
gen.close()
|
||||
finally:
|
||||
src.stop()
|
||||
@@ -6,6 +6,7 @@ start/stop 生命周期与停机刹车、run() 主循环异常护栏与故障恢
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import math
|
||||
import time
|
||||
from dataclasses import replace
|
||||
|
||||
@@ -29,6 +30,7 @@ class FakePx4:
|
||||
self.offboard_start_ok = True
|
||||
self.velocity_calls = []
|
||||
self.brake_calls = []
|
||||
self.yaw_calls = []
|
||||
|
||||
def own_state(self):
|
||||
return replace(self._state)
|
||||
@@ -54,6 +56,10 @@ class FakePx4:
|
||||
self.brake_calls.append(time_boot_ms)
|
||||
return True
|
||||
|
||||
async def send_yaw_rate(self, yaw_rate_dps):
|
||||
self.yaw_calls.append(yaw_rate_dps)
|
||||
return True
|
||||
|
||||
|
||||
class FakePod:
|
||||
def __init__(self):
|
||||
@@ -159,6 +165,116 @@ def test_attitude_downlink_throttled():
|
||||
assert len(pod.sent) == 2
|
||||
|
||||
|
||||
def test_control_law_throttled_to_own_frequency():
|
||||
"""生效律按自身频率推进：L2 配置 10Hz，外部 20Hz 调用时控制律只跑 10 次/秒。
|
||||
|
||||
链路维持（表3 姿态下发）不受此节流影响——否则吊舱解算会断流。
|
||||
"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
cfg = BridgeConfig()
|
||||
cfg.heading_servo.enabled = False # L2 地理引导（10Hz）
|
||||
cfg.guidance.frequency_hz = 10.0
|
||||
cfg.link.attitude_downlink_hz = 20.0
|
||||
cfg.ground_inject.enabled = True
|
||||
ground = GroundTarget(latitude_deg=0.01, longitude_deg=0.0, time_ms=1000)
|
||||
b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 1000,
|
||||
ground_target_fn=lambda: ground)
|
||||
|
||||
# 统计控制律实际推进次数（包装 update，不改变行为）
|
||||
law_updates = {"n": 0}
|
||||
real_update = b._guidance.update
|
||||
|
||||
def counting_update(inp, now):
|
||||
law_updates["n"] += 1
|
||||
return real_update(inp, now)
|
||||
|
||||
b._guidance.update = counting_update
|
||||
|
||||
ticks = 10 # 20Hz 调用 10 拍 = 500ms
|
||||
|
||||
async def scenario():
|
||||
for i in range(ticks):
|
||||
await b.step(1000 + i * 50)
|
||||
asyncio.run(scenario())
|
||||
|
||||
# 10Hz 下 500ms 只应推进 5 拍控制律；其余 5 拍重发上一拍输出保心跳
|
||||
assert law_updates["n"] == 5
|
||||
assert len(px4.velocity_calls) == ticks # offboard 设定点每拍都在下发
|
||||
assert len(pod.sent) == ticks # 表3 姿态下发不受控制律节流影响
|
||||
|
||||
|
||||
def test_control_law_throttle_resets_on_law_switch():
|
||||
"""切律后立即出第一拍（不被上一拍的节流压住）。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
cfg = BridgeConfig()
|
||||
cfg.guidance.frequency_hz = 10.0
|
||||
b = _bridge(px4, pod, cfg)
|
||||
|
||||
async def scenario():
|
||||
await b.step(1000)
|
||||
b.set_heading_servo_enabled(True)
|
||||
await b.step(1010) # 切律后 10ms 即应推进
|
||||
asyncio.run(scenario())
|
||||
assert b._last_law_ms == 1010
|
||||
|
||||
|
||||
def test_stop_is_idempotent():
|
||||
"""重复 stop() 只真正停机一次（真机在环实测：RTL 与收尾各调一次）。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
b = _bridge(px4, pod)
|
||||
|
||||
async def scenario():
|
||||
await b.start()
|
||||
await b.stop()
|
||||
await b.stop() # 第二次应为 no-op
|
||||
asyncio.run(scenario())
|
||||
|
||||
assert pod.closed is True
|
||||
assert px4.offboard_stopped is True
|
||||
assert len(px4.brake_calls) == 1 # 刹车只发一次
|
||||
|
||||
|
||||
def test_start_after_stop_reenables_stop():
|
||||
"""停机后重新 start() 可再次生效（标志位复位）。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
b = _bridge(px4, pod)
|
||||
|
||||
async def scenario():
|
||||
await b.start()
|
||||
await b.stop()
|
||||
assert b._stopped is True
|
||||
await b.start()
|
||||
assert b._stopped is False
|
||||
await b.stop()
|
||||
asyncio.run(scenario())
|
||||
assert len(px4.brake_calls) == 2 # 两次停机各发一次刹车
|
||||
|
||||
|
||||
def test_landing_stop_keeps_telemetry_and_pod_open():
|
||||
"""降落前只退 offboard：遥测与串口保留，供落地判定与高度曲线。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
b = _bridge(px4, pod)
|
||||
|
||||
async def scenario():
|
||||
await b.start()
|
||||
await b.landing_stop()
|
||||
assert b._landing_stopped is True
|
||||
assert b._stopped is False # 未全停
|
||||
assert px4.offboard_stopped is True
|
||||
assert px4.telemetry_stopped is False # 遥测仍在
|
||||
assert pod.closed is False # 串口仍在
|
||||
await b.stop() # 收尾才全停
|
||||
asyncio.run(scenario())
|
||||
assert px4.telemetry_stopped is True
|
||||
assert pod.closed is True
|
||||
assert len(px4.brake_calls) == 2 # 两次停机各发一次刹车
|
||||
|
||||
|
||||
# --- 吊舱帧入站缓存 --------------------------------------------------------
|
||||
def test_target_frame_updates_latest_and_flips_to_velocity():
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
@@ -423,6 +539,12 @@ def _locked_off_target(x=0.0, y=0.0, frame_yaw=0.0, frame_pitch=0.0, laser=50.0)
|
||||
frame_yaw_deg=frame_yaw, frame_pitch_deg=frame_pitch)
|
||||
|
||||
|
||||
def _set_pose(px4, now_ms: int, yaw_deg: float):
|
||||
"""更新假飞控姿态与时间戳（保证控制律的姿态超龄判定通过）。"""
|
||||
px4._state = replace(px4._state, yaw_rad=math.radians(yaw_deg),
|
||||
time_ms=now_ms, position_time_ms=now_ms)
|
||||
|
||||
|
||||
def test_l1_off_target_beta_drives_yaw_rate():
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
@@ -503,26 +625,73 @@ def test_l1_no_telemetry_degrades_and_keeps_heartbeat():
|
||||
assert len(px4.brake_calls) == 1
|
||||
|
||||
|
||||
def test_l1_require_vision_blocks_without_source():
|
||||
# §2.3 第二判据：require_vision=True 但未接视觉源 → fail-safe 视为无目标 → 刹车
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
def test_l1_los_consistent_keeps_tracking():
|
||||
"""机头右转多少、脱靶量就左移多少（目标在世界系静止）→ 判一致，正常跟踪。"""
|
||||
clock = {"ms": 1_000_000}
|
||||
px4 = FakePx4(_valid_state(clock["ms"]))
|
||||
pod = FakePod()
|
||||
pod.queue(_locked_off_target(x=10.0))
|
||||
b = _bridge(px4, pod, _l1_config(require_vision=True))
|
||||
out = asyncio.run(b.step(1000))
|
||||
assert out.mode.name == "BRAKE_HOVER"
|
||||
assert px4.velocity_calls == []
|
||||
b = PodBridge(_l1_config(), px4, pod, now_ms_fn=lambda: clock["ms"])
|
||||
|
||||
async def scenario():
|
||||
for i in range(80): # 4s，20°/s 右转、脱靶量 −20°/s
|
||||
clock["ms"] += 50
|
||||
yaw = 20.0 * (i + 1) * 0.05
|
||||
beta = 10.0 - 20.0 * (i + 1) * 0.05
|
||||
_set_pose(px4, clock["ms"], yaw)
|
||||
pod.queue(_locked_off_target(x=beta))
|
||||
out = await b.step(clock["ms"])
|
||||
return out
|
||||
|
||||
def test_l1_require_vision_passes_with_source():
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
out = asyncio.run(scenario())
|
||||
assert b._los_consistent is True
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert b.pod_locked(clock["ms"]) is True
|
||||
|
||||
|
||||
def test_l1_los_frozen_angle_blocks_tracking():
|
||||
"""机头转了 60° 而脱靶量几乎不变 → 角度冻结/幽灵目标 → 判丢（刹车）。"""
|
||||
clock = {"ms": 1_000_000}
|
||||
px4 = FakePx4(_valid_state(clock["ms"]))
|
||||
pod = FakePod()
|
||||
pod.queue(_locked_off_target(x=10.0))
|
||||
b = PodBridge(_l1_config(require_vision=True), px4, pod,
|
||||
now_ms_fn=lambda: 0, vision_target_fn=lambda: True)
|
||||
out = asyncio.run(b.step(1000))
|
||||
b = PodBridge(_l1_config(), px4, pod, now_ms_fn=lambda: clock["ms"])
|
||||
|
||||
async def scenario():
|
||||
out = None
|
||||
for i in range(80): # 4s，右转 20°/s，脱靶量恒定 10°
|
||||
clock["ms"] += 50
|
||||
yaw = 20.0 * (i + 1) * 0.05
|
||||
_set_pose(px4, clock["ms"], yaw)
|
||||
pod.queue(_locked_off_target(x=10.0))
|
||||
out = await b.step(clock["ms"])
|
||||
return out
|
||||
|
||||
out = asyncio.run(scenario())
|
||||
assert b._los_consistent is False
|
||||
assert out.mode.name == "BRAKE_HOVER" # 控制律被喂 tracked=False
|
||||
assert b.pod_locked(clock["ms"]) is False # 状态机侧也判为丢目标
|
||||
|
||||
|
||||
def test_l1_los_disabled_falls_back_to_bit2():
|
||||
"""关掉一致性判据 → 退回"只信 bit2"（角度冻结也不再判丢）。"""
|
||||
clock = {"ms": 1_000_000}
|
||||
px4 = FakePx4(_valid_state(clock["ms"]))
|
||||
pod = FakePod()
|
||||
b = PodBridge(_l1_config(los_consistency_enabled=False), px4, pod,
|
||||
now_ms_fn=lambda: clock["ms"])
|
||||
|
||||
async def scenario():
|
||||
out = None
|
||||
for i in range(80):
|
||||
clock["ms"] += 50
|
||||
yaw = 20.0 * (i + 1) * 0.05
|
||||
_set_pose(px4, clock["ms"], yaw)
|
||||
pod.queue(_locked_off_target(x=10.0))
|
||||
out = await b.step(clock["ms"])
|
||||
return out
|
||||
|
||||
out = asyncio.run(scenario())
|
||||
assert b._los_consistent is True
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
|
||||
|
||||
|
||||
# --- 地面坐标注入（A2）-------------------------------------------------------
|
||||
|
||||
@@ -1,12 +1,14 @@
|
||||
"""ground_link_test.py —— 地面坐标注入通道单元测试。
|
||||
"""ground_link_test.py —— 地面站链路单元测试。
|
||||
|
||||
覆盖：parse_payload 解析（合法/缺字段/非法 JSON/非对象/类型错误）、
|
||||
is_valid 新鲜度判定（未注入/新鲜/超龄边界）。
|
||||
parse_message 双通道（坐标 + {"cmd"} 命令）、命令队列 drain、
|
||||
is_valid 新鲜度判定（未注入/新鲜/超龄边界）、run() 真 UDP 回环。
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from pod_bridge.ground_link import GroundInjectConfig, GroundLink, GroundTarget
|
||||
from pod_bridge.ground_link import (GroundCommand, GroundInjectConfig,
|
||||
GroundLink, GroundTarget)
|
||||
|
||||
|
||||
# --- parse_payload ----------------------------------------------------------
|
||||
@@ -92,3 +94,108 @@ def test_latest_returns_injected_target():
|
||||
_inject(link, 1000)
|
||||
assert link.latest().latitude_deg == pytest.approx(1.0)
|
||||
assert link.latest().time_ms == 1000
|
||||
|
||||
|
||||
# --- parse_message（坐标 + 命令双通道）---------------------------------------
|
||||
def test_parse_message_command():
|
||||
m = GroundLink.parse_message(b'{"cmd": "rtl"}')
|
||||
assert isinstance(m, GroundCommand)
|
||||
assert m.name == "rtl"
|
||||
|
||||
|
||||
def test_parse_message_command_normalized():
|
||||
m = GroundLink.parse_message(b'{"cmd": " RTL "}')
|
||||
assert isinstance(m, GroundCommand)
|
||||
assert m.name == "rtl"
|
||||
|
||||
|
||||
def test_parse_message_cmd_takes_precedence_over_coords():
|
||||
m = GroundLink.parse_message(b'{"cmd": "emergency", "lat": 1.0, "lon": 2.0}')
|
||||
assert isinstance(m, GroundCommand)
|
||||
assert m.name == "emergency"
|
||||
|
||||
|
||||
def test_parse_message_coords_still_work():
|
||||
m = GroundLink.parse_message(b'{"lat": 1.0, "lon": 2.0}')
|
||||
assert isinstance(m, GroundTarget)
|
||||
assert m.latitude_deg == pytest.approx(1.0)
|
||||
|
||||
|
||||
def test_parse_message_empty_cmd_falls_back_to_coords():
|
||||
m = GroundLink.parse_message(b'{"cmd": "", "lat": 1.0, "lon": 2.0}')
|
||||
assert isinstance(m, GroundTarget)
|
||||
|
||||
|
||||
def test_parse_message_non_string_cmd_falls_back_to_coords():
|
||||
m = GroundLink.parse_message(b'{"cmd": 5, "lat": 1.0, "lon": 2.0}')
|
||||
assert isinstance(m, GroundTarget)
|
||||
|
||||
|
||||
def test_parse_message_invalid_returns_none():
|
||||
assert GroundLink.parse_message(b"not json") is None
|
||||
|
||||
|
||||
# --- drain_commands ---------------------------------------------------------
|
||||
def test_drain_commands_empty():
|
||||
assert _link().drain_commands() == []
|
||||
|
||||
|
||||
def test_drain_commands_fifo_and_clear():
|
||||
link = _link()
|
||||
link._commands.append("rtl")
|
||||
link._commands.append("pod_center")
|
||||
assert link.drain_commands() == ["rtl", "pod_center"]
|
||||
assert link.drain_commands() == [] # 取后清空
|
||||
|
||||
|
||||
def test_commands_queue_drops_oldest_when_full():
|
||||
link = _link()
|
||||
for i in range(20):
|
||||
link._commands.append(f"c{i}")
|
||||
cmds = link.drain_commands()
|
||||
assert len(cmds) == 16 # maxlen=16
|
||||
assert cmds[0] == "c4" and cmds[-1] == "c19"
|
||||
|
||||
|
||||
# --- run() 真 UDP 回环（坐标 + 命令）-----------------------------------------
|
||||
def test_run_receives_coords_and_commands():
|
||||
import asyncio
|
||||
import socket as socket_mod
|
||||
|
||||
probe = socket_mod.socket(socket_mod.AF_INET, socket_mod.SOCK_DGRAM)
|
||||
probe.bind(("127.0.0.1", 0))
|
||||
port = probe.getsockname()[1]
|
||||
probe.close()
|
||||
|
||||
link = GroundLink(GroundInjectConfig(host="127.0.0.1", port=port),
|
||||
now_ms_fn=lambda: 1234)
|
||||
addr = ("127.0.0.1", port)
|
||||
|
||||
async def scenario():
|
||||
stop = asyncio.Event()
|
||||
task = asyncio.ensure_future(link.run(stop))
|
||||
udp = socket_mod.socket(socket_mod.AF_INET, socket_mod.SOCK_DGRAM)
|
||||
try:
|
||||
for _ in range(200): # 重发防 bind 竞态
|
||||
udp.sendto(b'{"lat": 1.5, "lon": 2.5}', addr)
|
||||
await asyncio.sleep(0.01)
|
||||
if link.latest().time_ms == 1234:
|
||||
break
|
||||
udp.sendto(b'{"cmd": "rtl"}', addr)
|
||||
for _ in range(100):
|
||||
if link._commands:
|
||||
break
|
||||
await asyncio.sleep(0.01)
|
||||
finally:
|
||||
udp.close()
|
||||
stop.set()
|
||||
task.cancel() # 收包循环阻塞在 sock_recvfrom，须 cancel 唤醒（同 SITL 入口）
|
||||
try:
|
||||
await task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
|
||||
asyncio.run(scenario())
|
||||
assert link.latest().latitude_deg == pytest.approx(1.5)
|
||||
assert link.latest().time_ms == 1234
|
||||
assert link.drain_commands() == ["rtl"]
|
||||
|
||||
@@ -0,0 +1,141 @@
|
||||
"""los_consistency_test.py —— 吊舱角度运动学一致性判据的纯逻辑测试。
|
||||
|
||||
背景：本吊舱无电机、相机刚性固连机体，机头转 Δψ 时真实目标脱靶量必变 ≈−Δψ。
|
||||
判据 `residual = Δβ + Δψ`：超限且激励足够 → 判不一致（角度冻结/幽灵目标）。
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from pod_bridge.los_consistency import (LosConsistencyConfig,
|
||||
LosConsistencyMonitor, wrap_deg_180)
|
||||
|
||||
|
||||
def _mon(cfg=None):
|
||||
return LosConsistencyMonitor(cfg or LosConsistencyConfig())
|
||||
|
||||
|
||||
def _drive(mon, n, dt_ms, yaw0=0.0, beta0=0.0, yaw_rate=0.0, beta_rate=0.0,
|
||||
t0=0):
|
||||
"""按固定角速度喂 n 拍，返回最后一次状态。"""
|
||||
status = None
|
||||
for i in range(n):
|
||||
t = t0 + (i + 1) * dt_ms
|
||||
yaw = yaw0 + yaw_rate * (i + 1) * dt_ms / 1000.0
|
||||
beta = beta0 + beta_rate * (i + 1) * dt_ms / 1000.0
|
||||
status = mon.update(t, yaw, beta)
|
||||
return status
|
||||
|
||||
|
||||
# --- 条件开关 ---------------------------------------------------------------
|
||||
def test_disabled_always_consistent():
|
||||
mon = _mon(LosConsistencyConfig(enabled=False))
|
||||
status = _drive(mon, 100, 50, yaw_rate=30.0, beta_rate=0.0)
|
||||
assert status.consistent is True
|
||||
assert status.excited is False
|
||||
|
||||
|
||||
def test_no_history_is_not_excited():
|
||||
mon = _mon()
|
||||
status = mon.update(1000, 0.0, 10.0)
|
||||
assert status.consistent is True
|
||||
assert status.excited is False
|
||||
|
||||
|
||||
def test_insufficient_span_is_not_excited():
|
||||
"""窗口未积累到 3/4 之前不做判断（避免一开始就误判丢目标）。"""
|
||||
mon = _mon(LosConsistencyConfig(window_ms=3000))
|
||||
status = _drive(mon, 20, 50, yaw_rate=40.0, beta_rate=0.0) # 仅 1s
|
||||
assert status.excited is False
|
||||
assert status.consistent is True
|
||||
|
||||
|
||||
# --- 一致：机头转、脱靶量反向跟随 -------------------------------------------
|
||||
def test_consistent_rotation_is_ok():
|
||||
"""右转 20°/s 且 β 以 −20°/s 变化 → 残差≈0。"""
|
||||
mon = _mon()
|
||||
status = _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=-20.0) # 4s
|
||||
assert status.excited is True
|
||||
assert status.consistent is True
|
||||
assert status.residual_deg == pytest.approx(0.0, abs=0.5)
|
||||
|
||||
|
||||
def test_consistent_left_rotation_is_ok():
|
||||
"""左转（负角速度）且 β 正向跟随 → 同样一致（符号对称）。"""
|
||||
mon = _mon()
|
||||
status = _drive(mon, 80, 50, yaw_rate=-20.0, beta_rate=20.0)
|
||||
assert status.excited is True
|
||||
assert status.consistent is True
|
||||
|
||||
|
||||
def test_target_motion_within_residual_tolerance_is_ok():
|
||||
"""目标自身在动（β 变化与 −Δψ 不完全相等）但残差在容差内 → 仍判一致。"""
|
||||
mon = _mon(LosConsistencyConfig(max_residual_deg=8.0))
|
||||
# 4s 内 Δψ=80°，β 只反向变 76° → 残差 4°（< 8°）
|
||||
status = _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=-19.0)
|
||||
assert status.excited is True
|
||||
assert status.consistent is True
|
||||
|
||||
|
||||
# --- 不一致：角度冻结/幽灵目标 ---------------------------------------------
|
||||
def test_frozen_beta_while_yawing_is_inconsistent():
|
||||
"""机头转 80° 而脱靶量纹丝不动 → 角度冻结 → 判不一致。
|
||||
|
||||
比较窗口为 3s（默认），故 4s 数据里只比较最后 3s：Δψ = 20°/s × 3s = 60°。
|
||||
"""
|
||||
mon = _mon()
|
||||
status = _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=0.0)
|
||||
assert status.excited is True
|
||||
assert status.consistent is False
|
||||
assert status.frozen is True
|
||||
assert status.residual_deg == pytest.approx(60.0, abs=2.0) # 窗口内 Δψ
|
||||
|
||||
|
||||
def test_small_yaw_change_does_not_judge_even_if_residual_big():
|
||||
"""转得不够多（< 激励门槛）时不判不一致 —— 避免误杀。"""
|
||||
mon = _mon(LosConsistencyConfig(min_yaw_change_deg=20.0))
|
||||
# 窗口内只转 8°，虽然 β 没动（残差 8°）但未达激励门槛
|
||||
status = _drive(mon, 80, 50, yaw_rate=2.0, beta_rate=0.0)
|
||||
assert status.excited is False
|
||||
assert status.consistent is True
|
||||
|
||||
|
||||
# --- 历史管理 ---------------------------------------------------------------
|
||||
def test_reset_clears_history():
|
||||
mon = _mon()
|
||||
_drive(mon, 80, 50, yaw_rate=20.0, beta_rate=0.0)
|
||||
assert mon._samples # 有历史
|
||||
mon.reset()
|
||||
assert not mon._samples
|
||||
status = mon.update(9999, 0.0, 0.0)
|
||||
assert status.excited is False
|
||||
|
||||
|
||||
def test_old_samples_dropped():
|
||||
"""超出历史保留期的样本被丢弃，不会永久污染判定。"""
|
||||
mon = _mon(LosConsistencyConfig(window_ms=1000, history_factor=2.0))
|
||||
_drive(mon, 40, 50, yaw_rate=50.0, beta_rate=0.0) # 2s
|
||||
# 再喂一拍"很久之后"，旧样本应被清掉
|
||||
status = mon.update(60_000, 0.0, 0.0)
|
||||
assert status.excited is False
|
||||
|
||||
|
||||
# --- 角度回绕 ---------------------------------------------------------------
|
||||
@pytest.mark.parametrize("deg,expect", [
|
||||
(0.0, 0.0), (179.0, 179.0), (181.0, -179.0),
|
||||
(-181.0, 179.0), (359.0, -1.0),
|
||||
])
|
||||
def test_wrap_deg_180(deg, expect):
|
||||
assert wrap_deg_180(deg) == pytest.approx(expect)
|
||||
|
||||
|
||||
def test_wrap_around_north_is_consistent():
|
||||
"""航向跨过 ±180 与 β 同步变化时，不应因回绕误判。"""
|
||||
mon = _mon()
|
||||
# 从 yaw=170 转到 190（即 -170），β 从 10 到 -10
|
||||
for i in range(80):
|
||||
t = (i + 1) * 50
|
||||
yaw = 170.0 + 20.0 * (i + 1) * 0.05
|
||||
beta = 10.0 - 20.0 * (i + 1) * 0.05
|
||||
status = mon.update(t, wrap_deg_180(yaw), beta)
|
||||
assert status.excited is True
|
||||
assert status.consistent is True
|
||||
@@ -43,11 +43,13 @@ class FakeOffboard:
|
||||
class FakeTelemetry:
|
||||
"""各遥测流：先吐预设消息，再挂起（由 stop_telemetry 取消）。"""
|
||||
|
||||
def __init__(self, position=None, attitude=None, gps=None, velocity=None):
|
||||
def __init__(self, position=None, attitude=None, gps=None, velocity=None,
|
||||
status=None):
|
||||
self._position = position or []
|
||||
self._attitude = attitude or []
|
||||
self._gps = gps or []
|
||||
self._velocity = velocity or []
|
||||
self._status = status or []
|
||||
|
||||
async def _stream(self, msgs):
|
||||
for m in msgs:
|
||||
@@ -66,6 +68,9 @@ class FakeTelemetry:
|
||||
def velocity_ned(self):
|
||||
return self._stream(self._velocity)
|
||||
|
||||
def status_text(self):
|
||||
return self._stream(self._status)
|
||||
|
||||
|
||||
class FakeDrone:
|
||||
def __init__(self, telemetry=None):
|
||||
@@ -197,6 +202,35 @@ def test_start_telemetry_populates_cache_then_stop():
|
||||
asyncio.run(scenario())
|
||||
|
||||
|
||||
# --- 飞控状态文本（STATUSTEXT）----------------------------------------------
|
||||
class Sev(Enum):
|
||||
INFO = 1
|
||||
WARNING = 2
|
||||
|
||||
|
||||
def test_update_fc_status_and_take_messages():
|
||||
gw = Px4Gateway(FakeDrone())
|
||||
gw.update_fc_status(SimpleNamespace(type=Sev.WARNING, text="Low battery"))
|
||||
gw.update_fc_status(SimpleNamespace(type="INFO", text="Takeoff detected"))
|
||||
assert gw.take_fc_messages() == [("WARNING", "Low battery"),
|
||||
("INFO", "Takeoff detected")]
|
||||
assert gw.take_fc_messages() == [] # 取后清空
|
||||
|
||||
|
||||
def test_status_text_subscription_captures_messages():
|
||||
telemetry = FakeTelemetry(status=[
|
||||
SimpleNamespace(type=Sev.WARNING, text="EKF variance")])
|
||||
gw = Px4Gateway(FakeDrone(telemetry))
|
||||
|
||||
async def scenario():
|
||||
await gw.start_telemetry()
|
||||
await asyncio.sleep(0.05)
|
||||
assert gw.take_fc_messages() == [("WARNING", "EKF variance")]
|
||||
await gw.stop_telemetry()
|
||||
|
||||
asyncio.run(scenario())
|
||||
|
||||
|
||||
def test_default_own_state_is_invalid():
|
||||
s = OwnState()
|
||||
assert s.has_position is False and s.position_valid is False
|
||||
|
||||
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,94 +0,0 @@
|
||||
"""StereoConfig 校验与 JSON 加载测试（规格 §9：配置外置、fail-fast）。"""
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import StereoConfig, load_from_json
|
||||
|
||||
CONFIG_PATH = Path(__file__).resolve().parents[2] / "config" / "stereo_ranger.json"
|
||||
|
||||
|
||||
def _config_dict() -> dict:
|
||||
return json.loads(CONFIG_PATH.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _write(tmp_path: Path, data: dict) -> str:
|
||||
path = tmp_path / "stereo.json"
|
||||
path.write_text(json.dumps(data, ensure_ascii=False), encoding="utf-8")
|
||||
return str(path)
|
||||
|
||||
|
||||
def test_defaults_are_valid_and_domains_are_consistent():
|
||||
cfg = StereoConfig()
|
||||
assert cfg.focal_baseline_px_m == pytest.approx(cfg.fx_px * cfg.baseline_m)
|
||||
assert cfg.z_min_m == pytest.approx(cfg.focal_baseline_px_m / cfg.disparity_max_px)
|
||||
assert cfg.z_max_m == pytest.approx(cfg.focal_baseline_px_m / cfg.disparity_min_px)
|
||||
assert cfg.z_error_budget_max_m == pytest.approx((cfg.max_delta_z_m * cfg.focal_baseline_px_m) ** 0.5)
|
||||
|
||||
|
||||
def test_load_from_json_matches_defaults():
|
||||
assert load_from_json(str(CONFIG_PATH)) == StereoConfig()
|
||||
|
||||
|
||||
def test_missing_field_rejected(tmp_path):
|
||||
data = _config_dict()
|
||||
data.pop("baseline_m")
|
||||
with pytest.raises(ValueError, match="缺少配置字段"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
def test_unknown_field_rejected(tmp_path):
|
||||
data = _config_dict()
|
||||
data["radar_fallback"] = True
|
||||
with pytest.raises(ValueError, match="未知配置字段"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"field,value",
|
||||
[
|
||||
("fx_px", "960"),
|
||||
("consistency_N_frames", 3.5),
|
||||
("smooth_alpha", True),
|
||||
],
|
||||
)
|
||||
def test_type_error_rejected(tmp_path, field, value):
|
||||
data = _config_dict()
|
||||
data[field] = value
|
||||
with pytest.raises(ValueError, match="配置类型非法"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"field,value",
|
||||
[
|
||||
("fx_px", 0.0),
|
||||
("fy_px", -1.0),
|
||||
("cx_px", -0.5),
|
||||
("cy_px", -0.5),
|
||||
("baseline_m", 0.0),
|
||||
("tau_y_px", 0.0),
|
||||
("tau_h_ratio", 0.0),
|
||||
("disparity_min_px", 0.0),
|
||||
("disparity_min_px", 60.0),
|
||||
("max_delta_z_m", 0.0),
|
||||
("consistency_N_frames", 0),
|
||||
("consistency_M_frames", 2),
|
||||
("consistency_jump_px", 0.0),
|
||||
("smooth_alpha", 0.0),
|
||||
("smooth_alpha", 1.5),
|
||||
("jump_reset_m", 0.0),
|
||||
("filter_reset_frames", 0),
|
||||
("isolation_gate_px", 0.0),
|
||||
],
|
||||
)
|
||||
def test_invalid_values_rejected(field, value):
|
||||
with pytest.raises(ValueError, match="配置非法"):
|
||||
StereoConfig(**{field: value})
|
||||
|
||||
|
||||
def test_boundary_values_accepted():
|
||||
cfg = StereoConfig(disparity_min_px=3.0, disparity_max_px=3.0 + 1e-9, smooth_alpha=1.0, consistency_M_frames=3)
|
||||
assert cfg.z_max_m > cfg.z_min_m
|
||||
@@ -1,80 +0,0 @@
|
||||
"""一致性监控测试（规格 §6.7/§11.4：N 帧降级、M 帧判 invalid、正常帧清零、每目标隔离）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import ConsistencyMonitor
|
||||
|
||||
|
||||
def _monitor() -> ConsistencyMonitor:
|
||||
return ConsistencyMonitor(jump_px=3.0, degrade_frames=3, invalid_frames=5)
|
||||
|
||||
|
||||
def test_first_frame_has_no_reference_jump():
|
||||
status = _monitor().advance("t", 10.0)
|
||||
assert status.jump_px is None
|
||||
assert status.abnormal is False
|
||||
assert status.streak == 0
|
||||
|
||||
|
||||
def test_normal_jump_resets_streak():
|
||||
monitor = _monitor()
|
||||
monitor.advance("t", 10.0)
|
||||
abnormal = monitor.advance("t", 16.0)
|
||||
assert abnormal.abnormal and abnormal.streak == 1
|
||||
normal = monitor.advance("t", 17.0)
|
||||
assert not normal.abnormal and normal.streak == 0
|
||||
|
||||
|
||||
def test_jump_at_threshold_is_normal():
|
||||
monitor = _monitor()
|
||||
monitor.advance("t", 10.0)
|
||||
assert not monitor.advance("t", 13.0).abnormal # |ΔD| = 3.0 不超门限
|
||||
|
||||
|
||||
def test_degrades_at_n_frames_and_invalidates_at_m_frames():
|
||||
monitor = _monitor()
|
||||
monitor.advance("t", 10.0)
|
||||
disparities = [14.0, 18.0, 22.0, 26.0, 30.0] # 每帧跳变 4 px > 3 px
|
||||
statuses = [monitor.advance("t", d) for d in disparities]
|
||||
assert [s.streak for s in statuses] == [1, 2, 3, 4, 5]
|
||||
assert [s.confidence_degraded for s in statuses] == [False, False, True, True, True]
|
||||
assert [s.invalid for s in statuses] == [False, False, False, False, True]
|
||||
|
||||
|
||||
def test_targets_are_isolated():
|
||||
monitor = _monitor()
|
||||
monitor.advance("a", 10.0)
|
||||
monitor.advance("b", 20.0)
|
||||
monitor.advance("a", 15.0) # a 跳变异常
|
||||
assert monitor.streak("a") == 1
|
||||
assert monitor.streak("b") == 0
|
||||
|
||||
|
||||
def test_drop_clears_only_target():
|
||||
monitor = _monitor()
|
||||
monitor.advance("a", 10.0)
|
||||
monitor.advance("a", 20.0)
|
||||
monitor.drop("a")
|
||||
assert monitor.streak("a") == 0
|
||||
assert monitor.advance("a", 40.0).jump_px is None # 状态已清空，重新起始
|
||||
|
||||
|
||||
def test_clear_resets_all():
|
||||
monitor = _monitor()
|
||||
monitor.advance("a", 10.0)
|
||||
monitor.advance("a", 20.0)
|
||||
monitor.clear()
|
||||
assert monitor.streak("a") == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"kwargs",
|
||||
[
|
||||
{"jump_px": 0.0, "degrade_frames": 3, "invalid_frames": 5},
|
||||
{"jump_px": 3.0, "degrade_frames": 0, "invalid_frames": 5},
|
||||
{"jump_px": 3.0, "degrade_frames": 5, "invalid_frames": 3},
|
||||
],
|
||||
)
|
||||
def test_invalid_ctor_args(kwargs):
|
||||
with pytest.raises(ValueError):
|
||||
ConsistencyMonitor(**kwargs)
|
||||
@@ -1,94 +0,0 @@
|
||||
"""时间滤波器测试（规格 §6.5/§11.4：低通收敛、跳变重置不拖尾、失效清理、每目标隔离）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import DistanceFilter
|
||||
|
||||
|
||||
def _filter() -> DistanceFilter:
|
||||
return DistanceFilter(alpha=0.3, jump_reset_m=5.0, reset_frames=5)
|
||||
|
||||
|
||||
def test_first_value_initializes():
|
||||
assert _filter().advance("t", 10.0) == pytest.approx(10.0)
|
||||
|
||||
|
||||
def test_low_pass_step_response():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
assert distance_filter.advance("t", 12.0) == pytest.approx(10.6)
|
||||
|
||||
|
||||
def test_low_pass_converges_to_measurement():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
value = 10.0
|
||||
for _ in range(40):
|
||||
value = distance_filter.advance("t", 12.0)
|
||||
assert value == pytest.approx(12.0, abs=1e-3)
|
||||
|
||||
|
||||
def test_alpha_one_is_identity():
|
||||
distance_filter = DistanceFilter(alpha=1.0, jump_reset_m=5.0, reset_frames=5)
|
||||
distance_filter.advance("t", 10.0)
|
||||
assert distance_filter.advance("t", 12.0) == pytest.approx(12.0)
|
||||
|
||||
|
||||
def test_jump_reset_without_tail():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
assert distance_filter.advance("t", 20.0) == pytest.approx(20.0) # |Δ| > 5 → 直接重置
|
||||
assert distance_filter.value("t") == pytest.approx(20.0)
|
||||
assert distance_filter.advance("t", 20.2) == pytest.approx(20.06) # 之后恢复低通
|
||||
|
||||
|
||||
def test_invalidate_keeps_state_before_threshold_and_clears_after():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
for _ in range(4):
|
||||
distance_filter.invalidate("t")
|
||||
assert distance_filter.value("t") == pytest.approx(10.0)
|
||||
assert distance_filter.invalid_streak("t") == 4
|
||||
distance_filter.invalidate("t")
|
||||
assert distance_filter.value("t") is None
|
||||
assert distance_filter.advance("t", 30.0) == pytest.approx(30.0) # 重新初始化而非平滑
|
||||
|
||||
|
||||
def test_valid_frame_resets_invalid_streak():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
distance_filter.invalidate("t")
|
||||
distance_filter.invalidate("t")
|
||||
distance_filter.advance("t", 10.5)
|
||||
assert distance_filter.invalid_streak("t") == 0
|
||||
|
||||
|
||||
def test_invalidate_without_state_is_noop():
|
||||
distance_filter = _filter()
|
||||
distance_filter.invalidate("t")
|
||||
assert distance_filter.value("t") is None
|
||||
|
||||
|
||||
def test_targets_are_isolated():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("a", 10.0)
|
||||
distance_filter.advance("b", 100.0)
|
||||
assert distance_filter.advance("a", 12.0) == pytest.approx(10.6)
|
||||
assert distance_filter.value("b") == pytest.approx(100.0)
|
||||
distance_filter.drop("a")
|
||||
assert distance_filter.value("a") is None
|
||||
assert distance_filter.value("b") == pytest.approx(100.0)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"kwargs",
|
||||
[
|
||||
{"alpha": 0.0, "jump_reset_m": 5.0, "reset_frames": 5},
|
||||
{"alpha": 1.5, "jump_reset_m": 5.0, "reset_frames": 5},
|
||||
{"alpha": 0.3, "jump_reset_m": 0.0, "reset_frames": 5},
|
||||
{"alpha": 0.3, "jump_reset_m": 5.0, "reset_frames": 0},
|
||||
],
|
||||
)
|
||||
def test_invalid_ctor_args(kwargs):
|
||||
with pytest.raises(ValueError):
|
||||
DistanceFilter(**kwargs)
|
||||
@@ -1,103 +0,0 @@
|
||||
"""测距计算与有效性判定测试（规格 §3.2/§6.4/§6.6/§7.1/§11.4）。"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import (
|
||||
Detection,
|
||||
StereoConfig,
|
||||
confidence_from_cost,
|
||||
evaluate_validity,
|
||||
forward_distance_m,
|
||||
range_error_abs_m,
|
||||
range_error_rel,
|
||||
solve_range,
|
||||
virtual_center_px,
|
||||
)
|
||||
|
||||
CFG = StereoConfig()
|
||||
|
||||
|
||||
def _boxes_for_point(x_m: float, y_m: float, z_m: float, width_px: float = 50.0, height_px: float = 40.0):
|
||||
"""由左目系 (X, Y, Z) 反推左右目框（同宽，边缘视差 = 中心视差 = F·B/Z）。"""
|
||||
d_px = CFG.focal_baseline_px_m / z_m
|
||||
x_l = CFG.cx_px + CFG.fx_px * x_m / z_m
|
||||
y_l = CFG.cy_px + CFG.fy_px * y_m / z_m
|
||||
left = Detection(x_l - width_px / 2, y_l - height_px / 2, x_l + width_px / 2, y_l + height_px / 2)
|
||||
right = Detection(
|
||||
x_l - width_px / 2 - d_px, y_l - height_px / 2, x_l + width_px / 2 - d_px, y_l + height_px / 2
|
||||
)
|
||||
return left, right, d_px
|
||||
|
||||
|
||||
def test_solve_range_recovers_ground_truth():
|
||||
# §11.4：已知 Z 反推 D 构造框对，断言 Z = F·B/D 回收
|
||||
left, right, d_px = _boxes_for_point(0.6, 0.3, 6.0)
|
||||
solution = solve_range(left, right, d_px, CFG)
|
||||
assert solution.z_m == pytest.approx(6.0)
|
||||
assert solution.x_m == pytest.approx(0.6)
|
||||
assert solution.y_m == pytest.approx(0.3)
|
||||
assert solution.r_m == pytest.approx(math.sqrt(0.6**2 + 0.3**2 + 6.0**2))
|
||||
|
||||
|
||||
def test_forward_distance_rejects_non_positive_disparity():
|
||||
with pytest.raises(ValueError):
|
||||
forward_distance_m(0.0, CFG)
|
||||
with pytest.raises(ValueError):
|
||||
forward_distance_m(-2.0, CFG)
|
||||
|
||||
|
||||
def test_virtual_center_equals_midline_camera_projection():
|
||||
# §6.6 恒等式：xL = cx + F(X_C + B/2)/Z，xR = cx + F(X_C − B/2)/Z ⇒ c_mid = cx + F·X_C/Z
|
||||
z_m = 5.0
|
||||
x_center_m = 0.57
|
||||
x_l = CFG.cx_px + CFG.fx_px * (x_center_m + CFG.baseline_m / 2.0) / z_m
|
||||
x_r = CFG.cx_px + CFG.fx_px * (x_center_m - CFG.baseline_m / 2.0) / z_m
|
||||
left = Detection(x_l - 30.0, 480.0, x_l + 30.0, 520.0)
|
||||
right = Detection(x_r - 30.0, 480.0, x_r + 30.0, 520.0)
|
||||
center_mid = virtual_center_px(left, right)
|
||||
assert center_mid[0] == pytest.approx(CFG.cx_px + CFG.fx_px * x_center_m / z_m)
|
||||
assert center_mid[1] == pytest.approx(500.0)
|
||||
|
||||
|
||||
def test_error_model_is_quadratic_in_z():
|
||||
assert range_error_abs_m(10.0, CFG) == pytest.approx(10.0**2 / CFG.focal_baseline_px_m)
|
||||
assert range_error_rel(10.0, CFG) == pytest.approx(10.0 / CFG.focal_baseline_px_m)
|
||||
assert range_error_abs_m(20.0, CFG) == pytest.approx(4.0 * range_error_abs_m(10.0, CFG))
|
||||
|
||||
|
||||
def test_validity_gate_1_out_of_domain():
|
||||
decision = evaluate_validity(CFG.disparity_min_px - 0.1, 30.0, CFG)
|
||||
assert not decision.in_domain and not decision.valid
|
||||
|
||||
|
||||
def test_validity_gate_2_error_budget_blocks_far_field():
|
||||
# 域内（D = 4.8 px → Z = 12 m）但 δZ = 2.5 m > 2 m，预算门拦截
|
||||
d_px = 4.8
|
||||
z_m = CFG.focal_baseline_px_m / d_px
|
||||
decision = evaluate_validity(d_px, z_m, CFG)
|
||||
assert decision.in_domain and not decision.budget_ok and not decision.valid
|
||||
|
||||
|
||||
def test_validity_gate_2_boundary_sides():
|
||||
z_limit = CFG.z_error_budget_max_m
|
||||
assert evaluate_validity(CFG.focal_baseline_px_m / z_limit, z_limit * 0.999, CFG).budget_ok
|
||||
assert not evaluate_validity(CFG.focal_baseline_px_m / z_limit, z_limit * 1.001, CFG).budget_ok
|
||||
|
||||
|
||||
def test_validity_requires_finite_z():
|
||||
assert not evaluate_validity(5.0, None, CFG).valid
|
||||
assert not evaluate_validity(5.0, float("inf"), CFG).valid
|
||||
|
||||
|
||||
def test_domain_boundaries_are_inclusive():
|
||||
assert evaluate_validity(CFG.disparity_min_px, 19.2, CFG).in_domain
|
||||
assert evaluate_validity(CFG.disparity_max_px, 1.0, CFG).in_domain
|
||||
|
||||
|
||||
def test_confidence_from_cost_linear_clamped():
|
||||
assert confidence_from_cost(0.0) == pytest.approx(1.0)
|
||||
assert confidence_from_cost(0.4) == pytest.approx(0.6)
|
||||
assert confidence_from_cost(1.0) == pytest.approx(0.0)
|
||||
assert confidence_from_cost(1.5) == pytest.approx(0.0)
|
||||
@@ -1,87 +0,0 @@
|
||||
"""匹配器测试（规格 §6.2/§11.4：硬约束各一例、贪心排序、一一配对、cost ≥ 1 拒绝）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import Detection, GreedyMatcher, SamplePoint, StereoConfig, sample_point
|
||||
|
||||
CFG = StereoConfig()
|
||||
MATCHER = GreedyMatcher()
|
||||
|
||||
|
||||
def _point(cx: float, cy: float, *, w: float = 60.0, h: float = 40.0, class_id: int = 0) -> SamplePoint:
|
||||
return sample_point(
|
||||
Detection(cx - w / 2.0, cy - h / 2.0, cx + w / 2.0, cy + h / 2.0, class_id=class_id)
|
||||
)
|
||||
|
||||
|
||||
def test_class_mismatch_rejected():
|
||||
assert MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 500.0, class_id=1)], CFG) == []
|
||||
|
||||
|
||||
def test_dy_beyond_tau_rejected():
|
||||
assert MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 502.1)], CFG) == []
|
||||
|
||||
|
||||
def test_dy_within_tau_accepted_with_cost():
|
||||
pairs = MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 501.9)], CFG)
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0].cost == pytest.approx(1.9 / CFG.tau_y_px)
|
||||
|
||||
|
||||
def test_height_ratio_beyond_tau_rejected():
|
||||
assert MATCHER.match([_point(1000.0, 500.0, h=100.0)], [_point(1000.0, 500.0, h=140.0)], CFG) == []
|
||||
|
||||
|
||||
def test_height_ratio_near_tau_accepted():
|
||||
pairs = MATCHER.match([_point(1000.0, 500.0, h=120.0)], [_point(1000.0, 500.0, h=159.0)], CFG)
|
||||
assert len(pairs) == 1
|
||||
|
||||
|
||||
def test_cost_at_one_rejected():
|
||||
# dy = tau_y ⇒ cost = 1.0，严格 cost < 1 才接受（§6.2）
|
||||
assert MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 502.0)], CFG) == []
|
||||
|
||||
|
||||
def test_combined_cost_at_one_rejected():
|
||||
# dy/tau = 0.5 与 Δh/(tau_h·h_max) = 0.5 相加恰为 1.0 → 拒绝
|
||||
assert MATCHER.match([_point(1000.0, 500.0, h=112.0)], [_point(1000.0, 501.0, h=128.0)], CFG) == []
|
||||
|
||||
|
||||
def test_greedy_pairs_cheapest_first_and_one_to_one():
|
||||
left = [_point(1000.0, 500.0), _point(1000.0, 501.0)]
|
||||
right = [_point(1000.0, 500.0), _point(1000.0, 501.0)]
|
||||
pairs = MATCHER.match(left, right, CFG)
|
||||
assert [(p.left_index, p.right_index) for p in pairs] == [(0, 0), (1, 1)]
|
||||
assert pairs[0].cost == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_one_left_claims_cheapest_right():
|
||||
pairs = MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 500.0), _point(1000.0, 500.5)], CFG)
|
||||
assert [(p.left_index, p.right_index) for p in pairs] == [(0, 0)]
|
||||
|
||||
|
||||
def test_two_lefts_cannot_share_one_right():
|
||||
left = [_point(1000.0, 500.0), _point(1000.0, 500.2)]
|
||||
right = [_point(1000.0, 500.0)]
|
||||
pairs = MATCHER.match(left, right, CFG)
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0].left_index == 0
|
||||
|
||||
|
||||
def test_unmatched_right_is_not_paired():
|
||||
pairs = MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 500.0), _point(1400.0, 500.0)], CFG)
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0].right_index == 0
|
||||
|
||||
|
||||
def test_non_finite_points_excluded():
|
||||
bad = sample_point(Detection(float("nan"), 500.0, 1060.0, 540.0))
|
||||
good = _point(1000.0, 500.0)
|
||||
assert MATCHER.match([bad], [good], CFG) == []
|
||||
assert MATCHER.match([good], [bad], CFG) == []
|
||||
|
||||
|
||||
def test_zero_height_box_rejected():
|
||||
left = sample_point(Detection(970.0, 500.0, 1030.0, 500.0))
|
||||
right = sample_point(Detection(970.0, 500.0, 1030.0, 500.0))
|
||||
assert MATCHER.match([left], [right], CFG) == []
|
||||
@@ -1,79 +0,0 @@
|
||||
"""视差取点器测试（规格 §6.1/§11.4：同侧边缘、框宽差隔离、越窗回退、边界含等号）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import Detection, DisparitySource, StereoConfig, sample_point, select_disparity
|
||||
|
||||
CFG = StereoConfig()
|
||||
|
||||
|
||||
def _box(x1: float, x2: float, *, y1: float = 500.0, y2: float = 540.0) -> Detection:
|
||||
return Detection(x1, y1, x2, y2)
|
||||
|
||||
|
||||
def _disparity(left: Detection, right: Detection):
|
||||
return select_disparity(sample_point(left), sample_point(right), CFG)
|
||||
|
||||
|
||||
def test_left_edge_used_as_primary_sampling():
|
||||
d = _disparity(_box(1000.0, 1060.0), _box(990.4, 1050.4))
|
||||
assert d.source is DisparitySource.LEFT_EDGE
|
||||
assert d.d_px == pytest.approx(9.6)
|
||||
assert d.in_domain and d.positive and not d.fallback_used
|
||||
|
||||
|
||||
def test_box_width_difference_does_not_enter_edge_disparity():
|
||||
# §11.4：构造两目不等宽框，断言同侧边缘视差相同（框宽差不进入视差）
|
||||
left = _box(1000.0, 1100.0) # 宽 100
|
||||
right_narrow = _box(994.0, 1024.0) # 宽 30，左边缘差同 6 px
|
||||
right_wide = _box(994.0, 1094.0) # 宽 100
|
||||
d_narrow = _disparity(left, right_narrow)
|
||||
d_wide = _disparity(left, right_wide)
|
||||
assert d_narrow.d_px == pytest.approx(6.0)
|
||||
assert d_wide.d_px == pytest.approx(6.0)
|
||||
|
||||
|
||||
def test_center_disparity_would_be_polluted_by_width_difference():
|
||||
# 对照：中心视差 41 px = 边缘视差 6 px + 框宽差的一半 35 px，证明主取点选择边缘的理由
|
||||
left = _box(1000.0, 1100.0) # 宽 100, center 1050
|
||||
right_narrow = _box(994.0, 1024.0) # 宽 30, center 1009
|
||||
assert (left.cx - right_narrow.cx) == pytest.approx(41.0)
|
||||
|
||||
|
||||
def test_center_fallback_when_edge_out_of_window():
|
||||
# 边缘视差 60 px 越窗（> 57.6），中心视差 40 px 在域内 → 回退中心并标记
|
||||
left = _box(1000.0, 1060.0) # center 1030
|
||||
right = _box(940.0, 1040.0) # center 990
|
||||
d = _disparity(left, right)
|
||||
assert d.fallback_used is True
|
||||
assert d.source is DisparitySource.CENTER_FALLBACK
|
||||
assert d.d_px == pytest.approx(40.0)
|
||||
assert d.in_domain and d.positive
|
||||
|
||||
|
||||
def test_both_out_of_window_keeps_edge_value_for_diagnostics():
|
||||
left = _box(1000.0, 1200.0) # center 1100
|
||||
right = _box(900.0, 1000.0) # center 950
|
||||
d = _disparity(left, right)
|
||||
assert d.d_px == pytest.approx(100.0) # 边缘值保留（诊断 + 违例告警）
|
||||
assert not d.in_domain and not d.fallback_used and d.positive
|
||||
|
||||
|
||||
def test_domain_boundaries_are_inclusive():
|
||||
assert _disparity(_box(1000.0, 1060.0), _box(997.0, 1057.0)).in_domain # D = 3.0 = 下限
|
||||
# 上限用二进制可精确表示的 57.5 构造：十进制字面量 57.6 无法精确表示，
|
||||
# 框坐标相减会落到 57.6 常量之上，边界断言被浮点舍入干扰
|
||||
exact_cfg = StereoConfig(disparity_max_px=57.5)
|
||||
left, right = _box(1000.0, 1060.0), _box(942.5, 1002.5)
|
||||
assert select_disparity(sample_point(left), sample_point(right), exact_cfg).in_domain # D = 57.5 = 上限
|
||||
|
||||
|
||||
def test_negative_disparity_flagged_as_violation():
|
||||
d = _disparity(_box(1000.0, 1060.0), _box(1010.0, 1070.0))
|
||||
assert d.d_px == pytest.approx(-10.0)
|
||||
assert not d.positive and not d.in_domain
|
||||
|
||||
|
||||
def test_non_finite_box_marked_unusable():
|
||||
assert sample_point(Detection(float("nan"), 0.0, 10.0, 10.0)).finite is False
|
||||
assert sample_point(Detection(0.0, 0.0, 10.0, 10.0)).finite is True
|
||||
@@ -1,292 +0,0 @@
|
||||
"""全流程测试（规格 §5/§11.4：单目标、多目标、单目退化、非同帧拒收、越界、跳变）。
|
||||
|
||||
合成框对驱动，无需真实图像：几何是解析可断言的（§11.4）。
|
||||
"""
|
||||
|
||||
import logging
|
||||
import math
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import (
|
||||
Degradation,
|
||||
Detection,
|
||||
DetectionFrame,
|
||||
Eye,
|
||||
MatchPair,
|
||||
StereoConfig,
|
||||
StereoRanger,
|
||||
)
|
||||
|
||||
CFG = StereoConfig()
|
||||
LOGGER_NAME = "stereo_ranger"
|
||||
|
||||
|
||||
def _boxes(
|
||||
x_center_m: float = 0.0,
|
||||
y_m: float = 0.0,
|
||||
z_m: float = 6.0,
|
||||
*,
|
||||
w_m: float = 0.5,
|
||||
h_m: float = 0.3,
|
||||
class_id: int = 0,
|
||||
track_id: Optional[int] = None,
|
||||
confidence: float = 0.95,
|
||||
) -> Tuple[Detection, Detection]:
|
||||
"""模组中心系 (X_C, Y, Z) 目标 → 左右目检测框（两目同宽，边缘视差 = 中心视差）。"""
|
||||
x_l = CFG.cx_px + CFG.fx_px * (x_center_m + CFG.baseline_m / 2.0) / z_m
|
||||
x_r = CFG.cx_px + CFG.fx_px * (x_center_m - CFG.baseline_m / 2.0) / z_m
|
||||
y = CFG.cy_px + CFG.fy_px * y_m / z_m
|
||||
w_px = CFG.fx_px * w_m / z_m
|
||||
h_px = CFG.fy_px * h_m / z_m
|
||||
|
||||
def _box(cx_px: float) -> Detection:
|
||||
return Detection(
|
||||
cx_px - w_px / 2.0,
|
||||
y - h_px / 2.0,
|
||||
cx_px + w_px / 2.0,
|
||||
y + h_px / 2.0,
|
||||
confidence=confidence,
|
||||
class_id=class_id,
|
||||
track_id=track_id,
|
||||
)
|
||||
|
||||
return _box(x_l), _box(x_r)
|
||||
|
||||
|
||||
def _pair_with_disparity(d_px: float, *, y_center: float = 500.0, class_id: int = 0) -> Tuple[Detection, Detection]:
|
||||
"""构造视差恰为 d_px 的等宽框对（d 取 0.5 的整数倍时浮点精确）。"""
|
||||
width, height = 60.0, 40.0
|
||||
left = Detection(1000.0 - width / 2, y_center - height / 2, 1000.0 + width / 2, y_center + height / 2, class_id=class_id)
|
||||
right = Detection(
|
||||
1000.0 - d_px - width / 2,
|
||||
y_center - height / 2,
|
||||
1000.0 - d_px + width / 2,
|
||||
y_center + height / 2,
|
||||
class_id=class_id,
|
||||
)
|
||||
return left, right
|
||||
|
||||
|
||||
def _frame(frame_id: int, detections: List[Detection]) -> DetectionFrame:
|
||||
return DetectionFrame(frame_id=frame_id, detections=detections)
|
||||
|
||||
|
||||
def test_single_target_complete_valid_record():
|
||||
ranger = StereoRanger()
|
||||
left, right = _boxes(x_center_m=0.0, z_m=6.0)
|
||||
records = ranger.update(_frame(1, [left]), _frame(1, [right]), now_ms=1000)
|
||||
|
||||
assert len(records) == 1
|
||||
record = records[0]
|
||||
assert record.degradation is Degradation.COMPLETE
|
||||
assert record.valid is True
|
||||
assert record.forward_distance_m == pytest.approx(6.0)
|
||||
assert record.slant_range_m == pytest.approx(math.hypot(CFG.baseline_m / 2.0, 6.0)) # R 含左目系 +B/2 横向偏移
|
||||
assert record.disparity_px == pytest.approx(CFG.focal_baseline_px_m / 6.0)
|
||||
assert record.center_mid_px == pytest.approx((CFG.cx_px, CFG.cy_px))
|
||||
assert record.reference_box == left.bbox
|
||||
assert record.reference_eye is Eye.LEFT
|
||||
assert record.confidence == pytest.approx(1.0)
|
||||
assert record.frame_id == 1 and record.timestamp_ms == 1000
|
||||
|
||||
|
||||
def test_filter_smooths_across_valid_frames():
|
||||
ranger = StereoRanger()
|
||||
left_1, right_1 = _boxes(z_m=6.0)
|
||||
left_2, right_2 = _boxes(z_m=6.5)
|
||||
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
||||
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
||||
assert records[0].valid is True
|
||||
assert records[0].forward_distance_m == pytest.approx(6.0 + 0.3 * (6.5 - 6.0))
|
||||
# 参考斜距与已发布 Z 同基准：按 z_out/z_raw 从本帧原始斜距精确缩放（R ≥ Z）
|
||||
raw_slant = math.hypot(CFG.baseline_m / 2.0, 6.5)
|
||||
assert records[0].slant_range_m == pytest.approx(raw_slant * 6.15 / 6.5)
|
||||
|
||||
|
||||
def test_jump_reset_skips_smoothing():
|
||||
ranger = StereoRanger()
|
||||
left_1, right_1 = _boxes(z_m=4.0)
|
||||
left_2, right_2 = _boxes(z_m=10.0) # 跳变 6 m > jump_reset_m = 5 m
|
||||
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
||||
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
||||
assert records[0].forward_distance_m == pytest.approx(10.0)
|
||||
|
||||
|
||||
def test_far_field_out_of_domain_still_matched():
|
||||
# 越界 = 有效性门（决策）：配对仍成立，c_mid（无偏方位）与 D 照常发布，无 Z
|
||||
ranger = StereoRanger()
|
||||
left, right = _boxes(x_center_m=0.6, z_m=25.0) # D ≈ 2.3 px < 3 px
|
||||
records = ranger.update(_frame(1, [left]), _frame(1, [right]))
|
||||
|
||||
record = records[0]
|
||||
assert record.degradation is Degradation.COMPLETE
|
||||
assert record.valid is False
|
||||
assert record.forward_distance_m is None
|
||||
assert record.disparity_px == pytest.approx(2.304)
|
||||
assert record.center_mid_px == pytest.approx((CFG.cx_px + CFG.fx_px * 0.6 / 25.0, CFG.cy_px))
|
||||
assert record.slant_range_m == pytest.approx(math.hypot(0.6 + CFG.baseline_m / 2.0, 25.0))
|
||||
|
||||
|
||||
def test_error_budget_gate_blocks_far_but_in_domain():
|
||||
ranger = StereoRanger()
|
||||
left, right = _boxes(z_m=12.0) # D = 4.8 px 域内，δZ = 2.5 m > 2 m
|
||||
record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
|
||||
assert record.disparity_px == pytest.approx(4.8)
|
||||
assert record.valid is False
|
||||
assert record.forward_distance_m is None
|
||||
assert record.slant_range_m == pytest.approx(math.hypot(CFG.baseline_m / 2.0, 12.0))
|
||||
|
||||
|
||||
def test_error_budget_gate_passes_just_inside():
|
||||
ranger = StereoRanger()
|
||||
left, right = _pair_with_disparity(5.8) # Z ≈ 9.93 m，δZ ≈ 1.71 m ≤ 2 m
|
||||
record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
|
||||
assert record.valid is True
|
||||
assert record.forward_distance_m == pytest.approx(CFG.focal_baseline_px_m / 5.8)
|
||||
|
||||
|
||||
def test_negative_disparity_violation(caplog):
|
||||
ranger = StereoRanger()
|
||||
left = Detection(1000.0, 500.0, 1060.0, 540.0)
|
||||
right = Detection(1010.0, 500.0, 1070.0, 540.0)
|
||||
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
||||
record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
|
||||
assert record.degradation is Degradation.COMPLETE
|
||||
assert record.valid is False
|
||||
assert record.forward_distance_m is None
|
||||
assert record.slant_range_m is None
|
||||
assert record.disparity_px == pytest.approx(-10.0)
|
||||
assert any("disparity_violation" in message for message in caplog.messages)
|
||||
|
||||
|
||||
def test_center_fallback_keeps_target_valid(caplog):
|
||||
ranger = StereoRanger()
|
||||
left = Detection(1000.0, 500.0, 1060.0, 540.0) # 宽 60, center 1030
|
||||
right = Detection(940.0, 500.0, 1040.0, 540.0) # 宽 100, center 990
|
||||
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
||||
record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
|
||||
assert record.disparity_px == pytest.approx(40.0) # 边缘 60 px 越窗 → 中心 40 px
|
||||
assert record.valid is True
|
||||
assert record.forward_distance_m == pytest.approx(CFG.focal_baseline_px_m / 40.0)
|
||||
assert any("center_fallback" in message for message in caplog.messages)
|
||||
|
||||
|
||||
def test_single_eye_degradation_left_and_right():
|
||||
ranger = StereoRanger()
|
||||
left, right = _boxes(x_center_m=0.3, z_m=4.0)
|
||||
|
||||
left_only = ranger.update(_frame(1, [left]), _frame(1, []))
|
||||
assert len(left_only) == 1
|
||||
record = left_only[0]
|
||||
assert record.degradation is Degradation.BEARING_ONLY
|
||||
assert record.valid is False
|
||||
assert record.center_mid_px == pytest.approx((left.cx, left.cy))
|
||||
assert record.forward_distance_m is None and record.disparity_px is None
|
||||
assert record.reference_eye is Eye.LEFT
|
||||
assert record.confidence == pytest.approx(left.confidence)
|
||||
|
||||
right_only = ranger.update(_frame(2, []), _frame(2, [right]))
|
||||
assert len(right_only) == 1
|
||||
assert right_only[0].reference_eye is Eye.RIGHT
|
||||
assert right_only[0].center_mid_px == pytest.approx((right.cx, right.cy))
|
||||
|
||||
|
||||
def test_records_order_left_first_then_unmatched_right():
|
||||
ranger = StereoRanger()
|
||||
left, right_matched = _boxes(z_m=6.0, class_id=0)
|
||||
_, right_extra = _boxes(z_m=6.0, class_id=0, y_m=0.0)
|
||||
right_extra = Detection(400.0, 500.0, 460.0, 540.0) # 远处无对应左框
|
||||
records = ranger.update(_frame(1, [left]), _frame(1, [right_matched, right_extra]))
|
||||
assert len(records) == 2
|
||||
assert records[0].degradation is Degradation.COMPLETE
|
||||
assert records[0].valid is True
|
||||
assert records[1].degradation is Degradation.BEARING_ONLY
|
||||
assert records[1].reference_eye is Eye.RIGHT
|
||||
|
||||
|
||||
def test_frame_id_mismatch_rejects_frame_and_throttles_warning(caplog):
|
||||
ranger = StereoRanger()
|
||||
left, right = _boxes()
|
||||
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
||||
assert ranger.update(_frame(1, [left]), _frame(2, [right])) == []
|
||||
assert ranger.update(_frame(3, [left]), _frame(4, [right])) == []
|
||||
warnings = [message for message in caplog.messages if "frame_mismatch" in message]
|
||||
assert len(warnings) == 1 # 第 1 次打印，第 2 次被节流
|
||||
assert "累计 1 次" in warnings[0]
|
||||
|
||||
|
||||
def test_empty_input_returns_empty_without_warning(caplog):
|
||||
ranger = StereoRanger()
|
||||
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
||||
assert ranger.update(_frame(1, []), _frame(1, [])) == []
|
||||
assert caplog.messages == []
|
||||
|
||||
|
||||
def test_non_finite_box_dropped_with_warning(caplog):
|
||||
ranger = StereoRanger()
|
||||
bad = Detection(float("nan"), 500.0, 1060.0, 540.0)
|
||||
good_left, good_right = _boxes(z_m=6.0)
|
||||
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
||||
records = ranger.update(_frame(1, [bad, good_left]), _frame(1, [good_right]))
|
||||
assert len(records) == 1
|
||||
assert records[0].valid is True
|
||||
assert any("invalid_box" in message for message in caplog.messages)
|
||||
|
||||
|
||||
def test_consistency_degrades_then_invalidates(caplog):
|
||||
ranger = StereoRanger()
|
||||
disparities = [9.5, 13.5, 17.5, 21.5, 25.5, 29.5] # 每帧跳变 4 px > 3 px
|
||||
records = []
|
||||
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
||||
for index, d_px in enumerate(disparities, start=1):
|
||||
left, right = _pair_with_disparity(d_px)
|
||||
records.append(ranger.update(_frame(index, [left]), _frame(index, [right]))[0])
|
||||
|
||||
assert [record.valid for record in records] == [True, True, True, True, True, False]
|
||||
assert records[3].confidence == pytest.approx(0.5) # 第 3 帧起置信度降级（N = 3）
|
||||
assert records[5].forward_distance_m is None
|
||||
assert records[5].confidence == pytest.approx(0.0)
|
||||
assert any("consistency_invalid" in message for message in caplog.messages)
|
||||
|
||||
|
||||
def test_track_id_keeps_state_across_large_pixel_move():
|
||||
ranger = StereoRanger()
|
||||
left_1, right_1 = _boxes(x_center_m=0.6, z_m=4.0, track_id=7)
|
||||
left_2, right_2 = _boxes(x_center_m=0.0, z_m=4.5, track_id=7) # c_mid 移动 144 px > 门限
|
||||
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
||||
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
||||
assert records[0].track_id == 7
|
||||
assert records[0].forward_distance_m == pytest.approx(4.0 + 0.3 * (4.5 - 4.0))
|
||||
|
||||
|
||||
def test_without_track_id_large_move_starts_new_state():
|
||||
ranger = StereoRanger()
|
||||
left_1, right_1 = _boxes(x_center_m=0.6, z_m=4.0)
|
||||
left_2, right_2 = _boxes(x_center_m=0.0, z_m=4.5) # 最近邻越门限 → 新目标状态
|
||||
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
||||
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
||||
assert records[0].forward_distance_m == pytest.approx(4.5) # 未平滑
|
||||
|
||||
|
||||
def test_custom_matcher_injection():
|
||||
class _NoMatchMatcher:
|
||||
def match(self, left, right, config) -> List[MatchPair]:
|
||||
return []
|
||||
|
||||
ranger = StereoRanger(matcher=_NoMatchMatcher())
|
||||
left, right = _boxes(z_m=6.0)
|
||||
records = ranger.update(_frame(1, [left]), _frame(1, [right]))
|
||||
assert len(records) == 2
|
||||
assert all(record.degradation is Degradation.BEARING_ONLY for record in records)
|
||||
|
||||
|
||||
def test_reset_clears_state():
|
||||
ranger = StereoRanger()
|
||||
left_1, right_1 = _boxes(z_m=6.0)
|
||||
left_2, right_2 = _boxes(z_m=6.5)
|
||||
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
||||
ranger.reset()
|
||||
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
||||
assert records[0].forward_distance_m == pytest.approx(6.5) # 未平滑（状态已清空）
|
||||
@@ -1,73 +0,0 @@
|
||||
"""目标隔离测试（规格 §4：track_id 优先 + 框位置最近邻、帧末老化清理）。"""
|
||||
|
||||
from stereo_ranger import StereoConfig, TargetStore
|
||||
|
||||
CFG = StereoConfig() # isolation_gate_px = 50，filter_reset_frames = 5
|
||||
|
||||
|
||||
def test_track_key_stable_across_position_move():
|
||||
store = TargetStore(CFG)
|
||||
key_1 = store.resolve(0, 7, (100.0, 100.0), 1)
|
||||
key_2 = store.resolve(0, 7, (400.0, 100.0), 2) # 位置大幅移动仍是同一目标
|
||||
assert key_1 == key_2
|
||||
assert store.count == 1
|
||||
|
||||
|
||||
def test_nearest_neighbor_keeps_identity_within_gate():
|
||||
store = TargetStore(CFG)
|
||||
key_1 = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
key_2 = store.resolve(0, None, (130.0, 140.0), 2) # 距离 50 ≤ 门限
|
||||
assert key_1 == key_2
|
||||
|
||||
|
||||
def test_beyond_gate_creates_new_key():
|
||||
store = TargetStore(CFG)
|
||||
key_1 = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
key_2 = store.resolve(0, None, (200.0, 100.0), 2) # 距离 100 > 门限
|
||||
assert key_1 != key_2
|
||||
assert store.count == 2
|
||||
|
||||
|
||||
def test_class_id_separates_states():
|
||||
store = TargetStore(CFG)
|
||||
assert store.resolve(0, None, (100.0, 100.0), 1) != store.resolve(1, None, (100.0, 100.0), 1)
|
||||
|
||||
|
||||
def test_two_boxes_same_frame_claim_distinct_states():
|
||||
store = TargetStore(CFG)
|
||||
key_1 = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
key_2 = store.resolve(0, None, (110.0, 100.0), 1) # 同帧第二框不得接管已占用状态
|
||||
assert key_1 != key_2
|
||||
assert store.count == 2
|
||||
|
||||
|
||||
def test_state_evicted_after_reset_frames_unseen():
|
||||
store = TargetStore(CFG)
|
||||
key = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
store.distance_filter.advance(key, 5.0)
|
||||
store.consistency.advance(key, 9.6)
|
||||
for frame in range(2, 7): # 第 2~6 帧：连续 5 帧缺席
|
||||
store.end_frame(frame)
|
||||
assert store.count == 0
|
||||
assert store.distance_filter.value(key) is None
|
||||
assert store.consistency.streak(key) == 0
|
||||
|
||||
|
||||
def test_present_but_invalid_keeps_position_state():
|
||||
store = TargetStore(CFG)
|
||||
key = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
store.distance_filter.advance(key, 5.0)
|
||||
for frame in range(2, 8):
|
||||
store.resolve(0, None, (100.0, 100.0), frame) # 每帧仍出现
|
||||
store.distance_filter.invalidate(key)
|
||||
assert store.count == 1 # 位置状态保留
|
||||
assert store.distance_filter.value(key) is None # 滤波状态已按门限清空
|
||||
|
||||
|
||||
def test_clear_resets_everything():
|
||||
store = TargetStore(CFG)
|
||||
key = store.resolve(0, 5, (100.0, 100.0), 1)
|
||||
store.distance_filter.advance(key, 5.0)
|
||||
store.clear()
|
||||
assert store.count == 0
|
||||
assert store.distance_filter.value(key) is None
|
||||
@@ -1,45 +0,0 @@
|
||||
"""告警节流测试（规格 §5：第 1 次 + 每满 100 次，带累计计数）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import WarnThrottle
|
||||
|
||||
|
||||
def test_first_and_every_hundred_emit():
|
||||
throttle = WarnThrottle() # 默认 every=100
|
||||
assert throttle.should_emit("a") is True
|
||||
for _ in range(98):
|
||||
assert throttle.should_emit("a") is False
|
||||
assert throttle.should_emit("a") is True # 第 100 次
|
||||
for _ in range(99):
|
||||
assert throttle.should_emit("a") is False
|
||||
assert throttle.should_emit("a") is True # 第 200 次
|
||||
assert throttle.count("a") == 200
|
||||
|
||||
|
||||
def test_categories_are_independent():
|
||||
throttle = WarnThrottle()
|
||||
assert throttle.should_emit("a") is True
|
||||
assert throttle.should_emit("b") is True
|
||||
assert throttle.count("a") == 1
|
||||
assert throttle.count("b") == 1
|
||||
assert throttle.count("never_seen") == 0
|
||||
|
||||
|
||||
def test_custom_interval():
|
||||
throttle = WarnThrottle(every=3)
|
||||
# 规格语义"第 1 次 + 计数每满 every" → 第 1/3/6 次打印
|
||||
assert [throttle.should_emit("a") for _ in range(6)] == [True, False, True, False, False, True]
|
||||
|
||||
|
||||
def test_clear_resets_counts():
|
||||
throttle = WarnThrottle()
|
||||
throttle.should_emit("a")
|
||||
throttle.clear()
|
||||
assert throttle.count("a") == 0
|
||||
assert throttle.should_emit("a") is True
|
||||
|
||||
|
||||
def test_invalid_interval():
|
||||
with pytest.raises(ValueError):
|
||||
WarnThrottle(every=0)
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,96 +0,0 @@
|
||||
"""StereoYoloConfig 校验与 JSON 加载测试（需求说明 §4：配置外置、fail-fast）。"""
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_yolo import StereoYoloConfig, load_from_json
|
||||
|
||||
CONFIG_PATH = Path(__file__).resolve().parents[2] / "config" / "stereo_yolo.json"
|
||||
|
||||
|
||||
def _config_dict() -> dict:
|
||||
return json.loads(CONFIG_PATH.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _write(tmp_path: Path, data: dict) -> str:
|
||||
path = tmp_path / "stereo_yolo.json"
|
||||
path.write_text(json.dumps(data, ensure_ascii=False), encoding="utf-8")
|
||||
return str(path)
|
||||
|
||||
|
||||
def test_load_from_json_matches_defaults():
|
||||
assert load_from_json(str(CONFIG_PATH)) == StereoYoloConfig()
|
||||
|
||||
|
||||
def test_whitelist_normalized_to_tuple():
|
||||
cfg = StereoYoloConfig(class_whitelist=[0, 2])
|
||||
assert cfg.class_whitelist == (0, 2)
|
||||
|
||||
|
||||
def test_missing_field_rejected(tmp_path):
|
||||
data = _config_dict()
|
||||
data.pop("imgsz")
|
||||
with pytest.raises(ValueError, match="缺少配置字段"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
def test_unknown_field_rejected(tmp_path):
|
||||
data = _config_dict()
|
||||
data["letterbox_scale"] = 0.5
|
||||
with pytest.raises(ValueError, match="未知配置字段"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"field,value",
|
||||
[
|
||||
("model_path", 123),
|
||||
("imgsz", "640"),
|
||||
("imgsz", True),
|
||||
("conf_threshold", "0.25"),
|
||||
("iou_threshold", None),
|
||||
("class_whitelist", 0),
|
||||
("class_whitelist", ["0"]),
|
||||
("device", 5),
|
||||
("concurrency", 1),
|
||||
],
|
||||
)
|
||||
def test_type_error_rejected(tmp_path, field, value):
|
||||
data = _config_dict()
|
||||
data[field] = value
|
||||
with pytest.raises(ValueError, match="配置类型非法"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"field,value",
|
||||
[
|
||||
("model_path", ""),
|
||||
("imgsz", 0),
|
||||
("imgsz", 600),
|
||||
("conf_threshold", -0.1),
|
||||
("conf_threshold", 1.5),
|
||||
("iou_threshold", 0.0),
|
||||
("class_whitelist", []),
|
||||
("class_whitelist", [-1]),
|
||||
("device", " "),
|
||||
("concurrency", "batch"),
|
||||
],
|
||||
)
|
||||
def test_invalid_values_rejected(field, value):
|
||||
with pytest.raises(ValueError, match="配置非法"):
|
||||
StereoYoloConfig(**{field: value})
|
||||
|
||||
|
||||
def test_boundary_values_accepted():
|
||||
cfg = StereoYoloConfig(
|
||||
imgsz=32,
|
||||
conf_threshold=0.0,
|
||||
iou_threshold=1.0,
|
||||
class_whitelist=(0,),
|
||||
device="cpu",
|
||||
concurrency="sequential",
|
||||
)
|
||||
assert cfg.imgsz == 32
|
||||
@@ -1,155 +0,0 @@
|
||||
"""StereoYoloFrontend 出口契约测试（需求说明 §2.2/§2.3/§2.5/§5）。
|
||||
|
||||
全部用注入的假检测器驱动：不需要模型权重、不需要图像素材、不需要硬件；
|
||||
真并行用双人屏障断言（串行实现会屏障超时）。
|
||||
"""
|
||||
|
||||
import logging
|
||||
import threading
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from stereo_ranger.types import Detection
|
||||
from stereo_yolo import StereoYoloConfig, StereoYoloFrontend
|
||||
|
||||
|
||||
class ScriptedDetector:
|
||||
"""按调用顺序返回脚本项：list[Detection] 正常返回；Exception 抛出。"""
|
||||
|
||||
def __init__(self, script):
|
||||
self._script = list(script)
|
||||
self.calls = 0
|
||||
|
||||
def detect(self, image):
|
||||
self.calls += 1
|
||||
if not self._script:
|
||||
raise AssertionError("假检测器脚本已耗尽（调用次数多于预期）")
|
||||
item = self._script.pop(0)
|
||||
if isinstance(item, Exception):
|
||||
raise item
|
||||
return list(item)
|
||||
|
||||
|
||||
class BarrierDetector:
|
||||
"""两次调用必须同时在途（屏障超时即证明串行或单线程执行）。"""
|
||||
|
||||
def __init__(self, barrier: threading.Barrier, detections):
|
||||
self._barrier = barrier
|
||||
self._detections = list(detections)
|
||||
|
||||
def detect(self, image):
|
||||
self._barrier.wait(timeout=2.0)
|
||||
return list(self._detections)
|
||||
|
||||
|
||||
def _img(fill=0, shape=(8, 8, 3)) -> np.ndarray:
|
||||
return np.full(shape, fill, dtype=np.uint8)
|
||||
|
||||
|
||||
def _det(x1=1.0, y1=2.0, x2=11.0, y2=12.0, class_id=0, confidence=0.9, track_id=None) -> Detection:
|
||||
return Detection(x1, y1, x2, y2, confidence=confidence, class_id=class_id, track_id=track_id)
|
||||
|
||||
|
||||
def _frontend(script, **cfg_kwargs) -> StereoYoloFrontend:
|
||||
kwargs = {"concurrency": "sequential"}
|
||||
kwargs.update(cfg_kwargs)
|
||||
return StereoYoloFrontend(StereoYoloConfig(**kwargs), detector=ScriptedDetector(script))
|
||||
|
||||
|
||||
def test_frames_share_frame_id_and_keep_boxes():
|
||||
left_box, right_box = _det(1.0, 2.0, 11.0, 12.0, confidence=0.77), _det(3.0, 4.0, 13.0, 14.0)
|
||||
frontend = _frontend([[left_box], [right_box]])
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=7)
|
||||
|
||||
assert left_frame.frame_id == right_frame.frame_id == 7
|
||||
assert left_frame.detections == (left_box,)
|
||||
assert right_frame.detections == (right_box,)
|
||||
assert left_frame.detections[0].confidence == pytest.approx(0.77)
|
||||
assert left_frame.detections[0].track_id is None # 无追踪器时留空（§2.1）
|
||||
assert frontend._detector.calls == 2
|
||||
|
||||
|
||||
def test_whitelist_filters_by_class():
|
||||
frontend = _frontend([[_det(class_id=3)], [_det(class_id=0)]], class_whitelist=(0,))
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=1)
|
||||
|
||||
assert left_frame.detections == ()
|
||||
assert len(right_frame.detections) == 1 and right_frame.detections[0].class_id == 0
|
||||
|
||||
|
||||
def test_no_whitelist_keeps_all_classes():
|
||||
frontend = _frontend([[_det(class_id=3)], [_det(class_id=5)]])
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=1)
|
||||
|
||||
assert len(left_frame.detections) == 1 and len(right_frame.detections) == 1
|
||||
|
||||
|
||||
def test_degenerate_boxes_dropped(caplog):
|
||||
nan_box = _det(x1=float("nan"))
|
||||
zero_width = _det(x1=5.0, x2=5.0)
|
||||
inverted = _det(x1=20.0, x2=10.0)
|
||||
good = _det()
|
||||
frontend = _frontend([[nan_box, zero_width, inverted, good], []])
|
||||
|
||||
with caplog.at_level(logging.WARNING, logger="stereo_yolo"):
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=2)
|
||||
|
||||
assert left_frame.detections == (good,)
|
||||
assert right_frame.detections == ()
|
||||
assert "丢弃退化检测框" in caplog.text
|
||||
assert "累计 1 次" in caplog.text
|
||||
|
||||
|
||||
def test_boundary_touching_box_kept():
|
||||
hairline = _det(x1=10.0, x2=10.0 + 1e-9)
|
||||
frontend = _frontend([[hairline], []])
|
||||
left_frame, _ = frontend.detect(_img(), _img(), frame_id=3)
|
||||
assert left_frame.detections == (hairline,)
|
||||
|
||||
|
||||
def test_one_eye_failure_isolated(caplog):
|
||||
good = _det()
|
||||
frontend = _frontend([[good], RuntimeError("boom")])
|
||||
|
||||
with caplog.at_level(logging.WARNING, logger="stereo_yolo"):
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=4)
|
||||
|
||||
assert left_frame.detections == (good,)
|
||||
assert right_frame.detections == ()
|
||||
assert left_frame.frame_id == right_frame.frame_id == 4
|
||||
assert "右目推理失败" in caplog.text
|
||||
|
||||
|
||||
def test_shape_mismatch_rejected(caplog):
|
||||
frontend = _frontend([]) # 脚本为空：若检测器被调用会抛断言
|
||||
|
||||
with caplog.at_level(logging.WARNING, logger="stereo_yolo"):
|
||||
left_frame, right_frame = frontend.detect(_img(shape=(8, 8, 3)), _img(shape=(8, 6, 3)), frame_id=5)
|
||||
|
||||
assert left_frame.detections == () and right_frame.detections == ()
|
||||
assert left_frame.frame_id == right_frame.frame_id == 5
|
||||
assert frontend._detector.calls == 0
|
||||
assert "左右图尺寸不一致" in caplog.text
|
||||
|
||||
|
||||
def test_parallel_eyes_run_concurrently():
|
||||
barrier = threading.Barrier(2)
|
||||
box = _det()
|
||||
frontend = StereoYoloFrontend(
|
||||
StereoYoloConfig(concurrency="parallel"),
|
||||
detector=BarrierDetector(barrier, [box]),
|
||||
)
|
||||
try:
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=6)
|
||||
finally:
|
||||
frontend.close()
|
||||
|
||||
assert left_frame.detections == (box,)
|
||||
assert right_frame.detections == (box,)
|
||||
|
||||
|
||||
def test_close_is_idempotent():
|
||||
frontend = StereoYoloFrontend(StereoYoloConfig(concurrency="parallel"), detector=ScriptedDetector([]))
|
||||
frontend.close()
|
||||
frontend.close()
|
||||
@@ -1,27 +0,0 @@
|
||||
"""split_sbs 裁切测试（需求说明 §2.3：sbs 单流形态）。"""
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from stereo_yolo import split_sbs
|
||||
|
||||
|
||||
def test_split_returns_left_and_right_halves():
|
||||
image = np.zeros((4, 10, 3), dtype=np.uint8)
|
||||
image[:, :5] = 1
|
||||
image[:, 5:] = 2
|
||||
|
||||
left, right = split_sbs(image)
|
||||
|
||||
assert left.shape == (4, 5, 3) and right.shape == (4, 5, 3)
|
||||
assert (left == 1).all() and (right == 2).all()
|
||||
|
||||
|
||||
def test_odd_width_rejected():
|
||||
with pytest.raises(ValueError, match="宽度须为偶数"):
|
||||
split_sbs(np.zeros((4, 9, 3), dtype=np.uint8))
|
||||
|
||||
|
||||
def test_non_image_rejected():
|
||||
with pytest.raises(ValueError, match="宽度须为偶数"):
|
||||
split_sbs(np.zeros((9,), dtype=np.uint8))
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
@@ -1,107 +0,0 @@
|
||||
"""kalman_box_filter_test.py —— KalmanBoxFilter 单元测试。
|
||||
|
||||
移植自 reference-c++/target_tracker/tests/kalman_box_filter_test.cpp。
|
||||
覆盖（视觉追踪技术路线 §4）：初始化状态、静止目标收敛、匀速目标速度收敛
|
||||
且预测超前一个 dt、未初始化 predict 返回 None。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from target_tracker.kalman_box_filter import BoxCXCYWH, KalmanBoxFilter
|
||||
|
||||
|
||||
def test_uninitialized_predict_returns_none():
|
||||
kf = KalmanBoxFilter()
|
||||
assert not kf.initialized
|
||||
assert kf.predict() is None
|
||||
|
||||
# 未初始化时 update 无副作用
|
||||
kf.update(BoxCXCYWH(100.0, 100.0, 20.0, 20.0))
|
||||
assert not kf.initialized
|
||||
assert kf.predict() is None
|
||||
for v in kf.state:
|
||||
assert v == 0.0
|
||||
|
||||
|
||||
def test_initiate_sets_state_correctly():
|
||||
kf = KalmanBoxFilter()
|
||||
kf.initiate(BoxCXCYWH(320.0, 240.0, 40.0, 30.0))
|
||||
assert kf.initialized
|
||||
|
||||
expected = [320.0, 240.0, 40.0, 30.0, 0.0, 0.0, 0.0, 0.0]
|
||||
state = kf.state
|
||||
for i, want in enumerate(expected):
|
||||
assert state[i] == want, f"index={i}"
|
||||
|
||||
# 初始化后 predict 返回状态前 4 维（速度为零 → 位置不变，§4.4）
|
||||
pred = kf.predict()
|
||||
assert pred is not None
|
||||
assert pred.cx == pytest.approx(320.0, abs=1e-9)
|
||||
assert pred.cy == pytest.approx(240.0, abs=1e-9)
|
||||
assert pred.w == pytest.approx(40.0, abs=1e-9)
|
||||
assert pred.h == pytest.approx(30.0, abs=1e-9)
|
||||
|
||||
|
||||
def test_static_target_converges_to_observation():
|
||||
kf = KalmanBoxFilter()
|
||||
z = BoxCXCYWH(100.0, 150.0, 40.0, 60.0)
|
||||
kf.initiate(BoxCXCYWH(104.0, 156.0, 42.0, 62.0)) # 带初始偏差，验证收敛能力
|
||||
|
||||
for _ in range(50):
|
||||
kf.predict()
|
||||
kf.update(z)
|
||||
|
||||
state = kf.state
|
||||
assert state[0] == pytest.approx(z.cx, abs=0.5) # cx
|
||||
assert state[1] == pytest.approx(z.cy, abs=0.5) # cy
|
||||
assert state[2] == pytest.approx(z.w, abs=0.5) # w
|
||||
assert state[3] == pytest.approx(z.h, abs=0.5) # h
|
||||
# 静止目标速度应收敛到约 0
|
||||
assert state[4] == pytest.approx(0.0, abs=0.1)
|
||||
assert state[5] == pytest.approx(0.0, abs=0.1)
|
||||
|
||||
|
||||
def test_constant_velocity_converges_and_predict_leads_one_dt():
|
||||
kf = KalmanBoxFilter()
|
||||
vx = 3.0 # 像素/帧
|
||||
vy = 2.0 # 像素/帧
|
||||
|
||||
# 目标从 (100, 100) 出发匀速运动，每帧观测一次（predict → update）
|
||||
z = BoxCXCYWH(100.0, 100.0, 30.0, 30.0)
|
||||
kf.initiate(z)
|
||||
for _ in range(60):
|
||||
kf.predict()
|
||||
z.cx += vx
|
||||
z.cy += vy
|
||||
kf.update(z)
|
||||
|
||||
state = kf.state
|
||||
# 速度收敛到真实速度（像素/帧）
|
||||
assert state[4] == pytest.approx(vx, abs=0.3), "vx 未收敛"
|
||||
assert state[5] == pytest.approx(vy, abs=0.3), "vy 未收敛"
|
||||
# 位置贴合当前观测
|
||||
assert state[0] == pytest.approx(z.cx, abs=1.5)
|
||||
assert state[1] == pytest.approx(z.cy, abs=1.5)
|
||||
|
||||
# 再预测一帧：应超前当前观测一个 dt（≈ 速度 × 1 帧）
|
||||
pred = kf.predict()
|
||||
assert pred is not None
|
||||
assert pred.cx == pytest.approx(z.cx + vx, abs=1.0), "预测未超前一个 dt"
|
||||
assert pred.cy == pytest.approx(z.cy + vy, abs=1.0), "预测未超前一个 dt"
|
||||
|
||||
|
||||
def test_small_target_noise_scaling_stable():
|
||||
# 动态噪声不退化：小尺寸目标多帧观测仍稳定收敛（§4.2 噪声与尺寸成比例）
|
||||
kf = KalmanBoxFilter()
|
||||
z = BoxCXCYWH(50.0, 60.0, 4.0, 4.0) # 小目标（4×4 像素）
|
||||
kf.initiate(z)
|
||||
for _ in range(30):
|
||||
kf.predict()
|
||||
kf.update(z)
|
||||
state = kf.state
|
||||
assert state[0] == pytest.approx(z.cx, abs=0.5)
|
||||
assert state[1] == pytest.approx(z.cy, abs=0.5)
|
||||
assert math.isfinite(state[2])
|
||||
assert math.isfinite(state[3])
|
||||
@@ -1,364 +0,0 @@
|
||||
"""target_tracker_test.py —— TargetTracker 单元测试。
|
||||
|
||||
移植自 reference-c++/target_tracker/tests/target_tracker_test.cpp。
|
||||
逐条覆盖 docs/视觉追踪技术路线.md §9.4 行为一致性验收用例（硬验收标准）：
|
||||
1. 空检测帧序列：Coast 持续 max_lost_frames 帧，第 max_lost_frames+1 帧判丢
|
||||
2. ID 严格递增不复用（含 Reset 后归零）
|
||||
3. 锁定不被抢占；锁定目标死亡后切换到新目标
|
||||
4. min_hits 回退：无已确认实测轨道时新轨道也可当选主目标
|
||||
5. cost=1.0 恰好不构成匹配（严格小于）
|
||||
6. 置信度优先：两轨道竞争同一检测，置信度高者获胜
|
||||
另含 NaN/Inf 防御与 §2.3/§6.3/§9.5 行为测试。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from target_tracker.target_tracker import (Detection, FrameShape, TargetTracker,
|
||||
TrackerConfig)
|
||||
|
||||
|
||||
def make_det(x1, y1, x2, y2, conf, cls=0):
|
||||
return Detection(x1, y1, x2, y2, conf, cls)
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 1（§9.4-1）：空检测帧序列
|
||||
# ============================================================================
|
||||
def test_empty_frames_coast_then_lost():
|
||||
tracker = TargetTracker() # 默认配置 max_lost_frames = 8
|
||||
assert tracker.get_config().max_lost_frames == 8
|
||||
|
||||
# 第 1 帧：给出一个检测，建立轨道并锁定为主目标
|
||||
r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.9)])
|
||||
assert r.tracked
|
||||
assert r.primary_id is not None
|
||||
assert not r.is_predicted # 当帧有实测
|
||||
assert r.lost_frames == 0
|
||||
assert r.n_active == 1
|
||||
assert r.box is not None
|
||||
assert r.raw is not None # 当帧关联到原始检测框
|
||||
|
||||
# 后续空检测帧：应持续 Coast 共 max_lost_frames 帧（is_predicted=True）
|
||||
for i in range(1, 9):
|
||||
r = tracker.update([])
|
||||
assert r.tracked, f"第 {i} 帧 Coast 期间不应判丢"
|
||||
assert r.is_predicted, f"第 {i} 帧应为纯预测输出"
|
||||
assert r.lost_frames == i
|
||||
assert r.primary_id is not None
|
||||
assert r.box is not None # Coast 期间继续输出预测框
|
||||
assert r.raw is None # Coast 时 raw 为空（§2.3）
|
||||
assert r.confidence == pytest.approx(0.9) # 继承最近实测置信度（§2.3）
|
||||
|
||||
# 第 max_lost_frames + 1 帧：轨道死亡回收 → tracked=False
|
||||
r = tracker.update([])
|
||||
assert not r.tracked
|
||||
assert r.primary_id is None
|
||||
assert r.box is None
|
||||
assert r.lost_frames == 0 # 丢失时 lost_frames=0（§2.3）
|
||||
assert r.n_active == 0
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 2（§9.4-2）：ID 单调性
|
||||
# ============================================================================
|
||||
def test_id_strictly_increasing_no_reuse():
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 1 # 缩短 Coast 窗口，加速轨道死亡
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# 目标 A 出现 → id 0
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9)])
|
||||
assert r.primary_id is not None
|
||||
id_a = r.primary_id
|
||||
assert id_a == 0
|
||||
|
||||
# 目标 A 消失至死亡（max_lost=1：第 2 帧 Coast、第 3 帧死亡回收）
|
||||
tracker.update([])
|
||||
r = tracker.update([])
|
||||
assert not r.tracked
|
||||
assert tracker.active_count() == 0
|
||||
|
||||
# 目标 B 出现（同一位置）：ID 不复用，严格递增 → id 1
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9)])
|
||||
assert r.primary_id is not None
|
||||
id_b = r.primary_id
|
||||
assert id_b == 1
|
||||
assert id_b > id_a
|
||||
|
||||
# 目标 C 同帧新建 → id 2（同帧多目标亦严格递增）
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9),
|
||||
make_det(400.0, 300.0, 430.0, 330.0, 0.8)])
|
||||
assert tracker.active_count() == 2
|
||||
max_id = max(tracker.active_tracks().keys())
|
||||
assert max_id == 2
|
||||
|
||||
# Reset 后计数器归零（§6.2）：新轨道重新从 id 0 开始
|
||||
tracker.reset()
|
||||
assert tracker.active_count() == 0
|
||||
assert not tracker.last_result().tracked
|
||||
r = tracker.update([make_det(50.0, 50.0, 70.0, 70.0, 0.9)])
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == 0
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 3（§9.4-3）：锁定不被抢占
|
||||
# ============================================================================
|
||||
def test_lock_not_stolen_and_switch_after_death():
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 3
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# 低置信度目标 A 建立并锁定
|
||||
r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.4)])
|
||||
assert r.primary_id is not None
|
||||
id_a = r.primary_id
|
||||
|
||||
# A 消失进入 Coast；期间出现更高置信度新目标 B（远离 A，不会被误关联）
|
||||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == id_a, "Coast 期间锁定不应被高置信度目标抢占"
|
||||
assert r.is_predicted
|
||||
assert tracker.active_count() == 2 # A（Coast）与 B 并存
|
||||
|
||||
# 继续 Coast，锁定依旧延续
|
||||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||||
assert r.primary_id == id_a
|
||||
assert r.is_predicted
|
||||
|
||||
# A 继续丢失直至死亡（max_lost=3：lost 达 4 帧时回收）
|
||||
for _ in range(2):
|
||||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||||
# A 已死亡删除 → 锁定解除，重新选举切换到 B（此时 B 已确认且实测）
|
||||
assert r.tracked
|
||||
assert r.primary_id != id_a, "锁定目标死亡后应切换到新目标"
|
||||
assert not r.is_predicted
|
||||
|
||||
# 后续 B 持续作为主目标
|
||||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id != id_a
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 4（§9.4-4）：min_hits 回退
|
||||
# ============================================================================
|
||||
def test_min_hits_fallback_allows_new_track():
|
||||
cfg = TrackerConfig()
|
||||
cfg.min_hits = 5 # 抬高确认门槛，保证测试期间轨道始终未确认
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# 首帧新建轨道 hits=1 < min_hits：候选池 confirmed 为空 → 退回 measured
|
||||
r = tracker.update([make_det(200.0, 150.0, 240.0, 190.0, 0.6)])
|
||||
assert r.tracked, "无已确认轨道时新轨道应能当选主目标"
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == 0
|
||||
assert not r.is_predicted
|
||||
|
||||
# 再跟几帧（hits 增长但仍未达 min_hits=5），依旧当选
|
||||
for _ in range(3):
|
||||
r = tracker.update([make_det(200.0, 150.0, 240.0, 190.0, 0.6)])
|
||||
assert r.tracked
|
||||
assert r.primary_id == 0
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 5（§9.4-5）：cost 边界（cost = 1.0 恰好不匹配）
|
||||
# ============================================================================
|
||||
def test_cost_exactly_one_does_not_match():
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 1
|
||||
cfg.max_association_dist = 200.0
|
||||
cfg.dist_weight = 1.0 # 纯距离项，IoU 项权重为 0 且两框零交集
|
||||
cfg.iou_weight = 0.0
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# 建立轨道：中心 (100,100)
|
||||
r = tracker.update([make_det(90.0, 90.0, 110.0, 110.0, 0.9)])
|
||||
assert r.primary_id is not None
|
||||
id_a = r.primary_id
|
||||
|
||||
# 下一帧检测中心 (100,300)：距离 = 200 = Dmax → cost = 1.0，恰好不匹配
|
||||
r = tracker.update([make_det(90.0, 290.0, 110.0, 310.0, 0.9)])
|
||||
# 未匹配 → 原轨道 Coast、检测新建轨道；两个目标并存
|
||||
assert tracker.active_count() == 2, "cost=1.0 不应匹配，应新建轨道"
|
||||
# 原轨道（锁定）进入 Coast：lost_frames=1
|
||||
assert r.tracked
|
||||
assert r.is_predicted
|
||||
assert r.primary_id == id_a
|
||||
assert r.lost_frames == 1
|
||||
|
||||
# 对照：距离略小于阈值（199px → cost=0.995 < 1.0）应匹配成功
|
||||
tracker2 = TargetTracker(cfg)
|
||||
r = tracker2.update([make_det(90.0, 90.0, 110.0, 110.0, 0.9)])
|
||||
id_b = r.primary_id
|
||||
r = tracker2.update([make_det(90.0, 289.0, 110.0, 309.0, 0.9)])
|
||||
assert tracker2.active_count() == 1, "cost<1.0 应匹配成功，不应新建轨道"
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == id_b
|
||||
assert not r.is_predicted # 实测命中
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 6（§9.4-6）：置信度优先
|
||||
# ============================================================================
|
||||
def test_higher_confidence_track_wins_detection():
|
||||
tracker = TargetTracker()
|
||||
|
||||
# 同帧建立两条轨道：A 置信度 0.5、B 置信度 0.9，相距 80px（不互相重叠）
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.5),
|
||||
make_det(180.0, 100.0, 200.0, 120.0, 0.9)])
|
||||
assert tracker.active_count() == 2
|
||||
|
||||
# 找出两条轨道 ID（按置信度区分）
|
||||
id_low = None
|
||||
id_high = None
|
||||
for track_id, track in tracker.active_tracks().items():
|
||||
if track.confidence > 0.8:
|
||||
id_high = track_id
|
||||
else:
|
||||
id_low = track_id
|
||||
assert id_low is not None
|
||||
assert id_high is not None
|
||||
|
||||
# 下一帧只给一个检测，放在两轨道预测中心之间（各距 40px，对称）：
|
||||
# 距离项对两者相同，贪婪按置信度降序 → 高置信度轨道 B 优先匹配
|
||||
r = tracker.update([make_det(140.0, 100.0, 160.0, 120.0, 0.7)])
|
||||
|
||||
tracks = tracker.active_tracks()
|
||||
assert len(tracks) == 2
|
||||
# 获胜者：命中（lost=0、hits=2、置信度更新）
|
||||
assert tracks[id_high].lost_count == 0, "高置信度轨道应获胜"
|
||||
assert tracks[id_high].hits == 2
|
||||
assert tracks[id_high].confidence == pytest.approx(0.7)
|
||||
# 失败者：未匹配进入 Coast（lost=1、hits 不变）
|
||||
assert tracks[id_low].lost_count == 1, "低置信度轨道应落选"
|
||||
assert tracks[id_low].hits == 1
|
||||
assert tracks[id_low].confidence == pytest.approx(0.5)
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# NaN/Inf 防御（评审修复项）
|
||||
# ============================================================================
|
||||
def test_non_finite_detections_are_discarded():
|
||||
tracker = TargetTracker()
|
||||
nan = math.nan
|
||||
inf = math.inf
|
||||
|
||||
# 第 1 帧：建立正常轨道
|
||||
r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.9)])
|
||||
assert r.tracked
|
||||
|
||||
# 第 2 帧：混入 NaN 坐标 / Inf 坐标 / NaN 置信度检测与一个正常检测：
|
||||
# 非有限检测全部被丢弃，仅正常检测参与关联，不新建轨道
|
||||
r = tracker.update([make_det(nan, 100.0, 140.0, 140.0, 0.9),
|
||||
make_det(100.0, -inf, 140.0, 140.0, 0.9),
|
||||
make_det(100.0, 100.0, nan, 140.0, 0.9),
|
||||
make_det(102.0, 102.0, 142.0, 142.0, nan),
|
||||
make_det(101.0, 101.0, 141.0, 141.0, 0.9)])
|
||||
assert tracker.active_count() == 1, "非有限检测不应新建轨道"
|
||||
assert r.tracked
|
||||
assert not r.is_predicted # 正常检测命中，实测输出
|
||||
# 主目标输出未被 NaN 污染
|
||||
assert math.isfinite(r.center[0])
|
||||
assert math.isfinite(r.center[1])
|
||||
assert r.box is not None
|
||||
assert math.isfinite(r.box.x1) and math.isfinite(r.box.y1)
|
||||
assert math.isfinite(r.box.x2) and math.isfinite(r.box.y2)
|
||||
assert math.isfinite(r.confidence)
|
||||
assert r.raw is not None # 实测帧 raw 有值
|
||||
|
||||
# 第 3 帧：仅含 NaN 检测 → 等价空帧，轨道 Coast 不崩溃
|
||||
r = tracker.update([make_det(nan, nan, nan, nan, nan)])
|
||||
assert r.tracked
|
||||
assert r.is_predicted
|
||||
assert r.lost_frames == 1
|
||||
assert r.raw is None # Coast 时 raw 为空（§2.3）
|
||||
assert math.isfinite(r.center[0])
|
||||
|
||||
# 持续 NaN 检测直至轨道死亡回收，全程不崩溃
|
||||
for _ in range(10):
|
||||
r = tracker.update([make_det(nan, 0.0, 10.0, 10.0, 0.9)])
|
||||
assert not r.tracked
|
||||
assert tracker.active_count() == 0
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 补充行为测试（§2.3 / §6.3 / §9.5）
|
||||
# ============================================================================
|
||||
|
||||
def test_lost_result_fields_are_empty():
|
||||
tracker = TargetTracker()
|
||||
r = tracker.update([]) # 空帧序列起点：无任何轨道
|
||||
assert not r.tracked
|
||||
assert r.primary_id is None
|
||||
assert r.box is None
|
||||
assert r.raw is None
|
||||
assert r.lost_frames == 0
|
||||
assert r.n_active == 0
|
||||
assert r.confidence == 0.0
|
||||
|
||||
# LastResult 返回同一快照（§9.5 跨线程读取入口）
|
||||
cached = tracker.last_result()
|
||||
assert not cached.tracked
|
||||
|
||||
|
||||
def test_coasting_fallback_chooses_nearest_to_frame_center():
|
||||
# Coast 兜底选举：无实测轨道时选离画面中心最近的 Coast 轨道（§6.3）。
|
||||
# 锁定延续优先：需先让锁定轨道死亡解除锁定，才会进入重新选举分支。
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 2
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# F1：角落目标（高置信度）先建立并被锁定
|
||||
tracker.update([make_det(10.0, 10.0, 30.0, 30.0, 0.95)])
|
||||
# F2：画面中心附近目标建立；角落目标丢失(1)
|
||||
tracker.update([make_det(300.0, 220.0, 340.0, 260.0, 0.9)])
|
||||
# F3：远处目标建立；角落丢失(2)、中心目标丢失(1)
|
||||
tracker.update([make_det(600.0, 450.0, 620.0, 470.0, 0.85)])
|
||||
# F4：全部丢失：角落丢失(3) > max_lost 死亡解除锁定；
|
||||
# 其余两条 Coast 存活 → 重新选举选离画面中心最近者
|
||||
r = tracker.update([], FrameShape(480, 640))
|
||||
assert r.tracked
|
||||
assert r.is_predicted
|
||||
assert r.center[0] == pytest.approx(320.0, abs=25.0) # 靠近画面中心 (320,240)
|
||||
assert r.center[1] == pytest.approx(240.0, abs=25.0)
|
||||
assert r.n_active == 2 # 角落轨道已回收
|
||||
|
||||
|
||||
def test_locked_primary_kept_while_measured():
|
||||
# 锁定延续优先于置信度选举（§6.3 ①）
|
||||
tracker = TargetTracker()
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.4)])
|
||||
assert r.primary_id is not None
|
||||
locked_id = r.primary_id
|
||||
|
||||
# 高置信度新目标出现并持续，锁定目标也持续实测 → primary_id 不变
|
||||
for i in range(5):
|
||||
r = tracker.update([make_det(100.0 + i, 100.0, 120.0 + i, 120.0, 0.4),
|
||||
make_det(400.0, 300.0, 430.0, 330.0, 0.95)])
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == locked_id
|
||||
assert not r.is_predicted
|
||||
|
||||
|
||||
def test_default_frame_center_is_320x240():
|
||||
# frame_shape 缺省按 640×480 画面中心 (320,240) 计算（§8.2 / §2.2）
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 2
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# F1：角落目标先建立并被锁定（高置信度）
|
||||
tracker.update([make_det(0.0, 0.0, 20.0, 20.0, 0.95)])
|
||||
# F2：左侧目标（中心 (280,240)，距默认中心 40px）建立
|
||||
tracker.update([make_det(270.0, 230.0, 290.0, 250.0, 0.9)])
|
||||
# F3：右侧目标（中心 (500,240)，距默认中心 180px）建立
|
||||
tracker.update([make_det(490.0, 230.0, 510.0, 250.0, 0.85)])
|
||||
# F4：全部丢失：锁定轨道死亡解除锁定，两条 Coast 轨道兜底选举
|
||||
r = tracker.update([])
|
||||
assert r.tracked
|
||||
assert r.is_predicted
|
||||
assert r.center[0] == pytest.approx(280.0, abs=30.0) # 左侧更近，当选
|
||||
assert r.center[1] == pytest.approx(240.0, abs=30.0)
|
||||
@@ -1,159 +0,0 @@
|
||||
"""tracklet_test.py —— Tracklet 与几何工具单元测试。
|
||||
|
||||
移植自 reference-c++/target_tracker/tests/tracklet_test.cpp。
|
||||
覆盖（视觉追踪技术路线 §3.2/§5.2/§6.1/§7）：框互转、IoU 边界、
|
||||
IsDead/IsCoasting 判定（严格大于）、Predict/Update/MarkMiss 计数语义。
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from target_tracker.tracklet import (BoxXYXY, BoxCXCYWH, Tracklet, compute_iou,
|
||||
to_center_size, to_xyxy)
|
||||
|
||||
|
||||
# ---- 框表示互转（§7） ----
|
||||
|
||||
def test_round_trip_preserves_values():
|
||||
box = BoxXYXY(10.0, 20.0, 50.0, 80.0)
|
||||
c = to_center_size(box)
|
||||
assert c.cx == 30.0 # (10+50)/2
|
||||
assert c.cy == 50.0 # (20+80)/2
|
||||
assert c.w == 40.0 # 50−10
|
||||
assert c.h == 60.0 # 80−20
|
||||
|
||||
back = to_xyxy(c)
|
||||
assert back.x1 == box.x1
|
||||
assert back.y1 == box.y1
|
||||
assert back.x2 == box.x2
|
||||
assert back.y2 == box.y2
|
||||
|
||||
|
||||
# ---- IoU 边界（§5.2） ----
|
||||
|
||||
def test_identical_boxes_return_one():
|
||||
a = BoxXYXY(10.0, 10.0, 30.0, 30.0)
|
||||
assert compute_iou(a, a) == pytest.approx(1.0, abs=1e-12) # 完全重合 = 1
|
||||
|
||||
|
||||
def test_disjoint_boxes_return_zero():
|
||||
a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
|
||||
b = BoxXYXY(20.0, 20.0, 30.0, 30.0)
|
||||
assert compute_iou(a, b) == 0.0 # 零交集
|
||||
|
||||
|
||||
def test_touching_edges_return_zero():
|
||||
# 仅边/角相接：交集宽高为 0 → 交集面积 ≤ 0 返回 0（§5.2）
|
||||
a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
|
||||
b = BoxXYXY(10.0, 0.0, 20.0, 10.0) # 右边缘相接
|
||||
assert compute_iou(a, b) == 0.0
|
||||
c = BoxXYXY(10.0, 10.0, 20.0, 20.0) # 角点相接
|
||||
assert compute_iou(a, c) == 0.0
|
||||
|
||||
|
||||
def test_empty_box_returns_zero():
|
||||
normal = BoxXYXY(0.0, 0.0, 10.0, 10.0)
|
||||
zero_area = BoxXYXY(5.0, 5.0, 5.0, 5.0) # 零面积
|
||||
inverted = BoxXYXY(10.0, 10.0, 0.0, 0.0) # 反向（负宽高）
|
||||
assert compute_iou(normal, zero_area) == 0.0
|
||||
assert compute_iou(zero_area, normal) == 0.0
|
||||
assert compute_iou(normal, inverted) == 0.0
|
||||
assert compute_iou(zero_area, zero_area) == 0.0
|
||||
|
||||
|
||||
def test_partial_overlap_is_correct():
|
||||
# A = [0,0,10,10]（面积 100），B = [5,5,15,15]（面积 100），交集 25
|
||||
a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
|
||||
b = BoxXYXY(5.0, 5.0, 15.0, 15.0)
|
||||
assert compute_iou(a, b) == pytest.approx(25.0 / 175.0, abs=1e-12)
|
||||
assert compute_iou(b, a) == pytest.approx(25.0 / 175.0, abs=1e-12) # 对称性
|
||||
|
||||
|
||||
# ---- 构造与派生状态（§3.2 / §6.1） ----
|
||||
|
||||
def test_construction_initial_fields():
|
||||
t = Tracklet(7, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 1, 8)
|
||||
|
||||
assert t.track_id == 7
|
||||
assert t.lost_count == 0
|
||||
assert t.max_lost == 8
|
||||
assert t.confidence == 0.9
|
||||
assert t.cls_id == 1
|
||||
assert t.age == 1 # 创建时为 1（§3.2）
|
||||
assert t.hits == 1 # 创建时为 1（§3.2）
|
||||
assert t.last_center == (100.0, 100.0)
|
||||
assert t.last_box is not None # 创建当帧即赋 last_box（§7 步骤 5）
|
||||
assert t.last_box.x1 == 90.0
|
||||
assert t.last_box.y1 == 90.0
|
||||
assert t.last_box.x2 == 110.0
|
||||
assert t.last_box.y2 == 110.0
|
||||
assert not t.is_dead
|
||||
assert not t.is_coasting
|
||||
|
||||
|
||||
def test_is_dead_uses_strictly_greater():
|
||||
# IsDead 使用严格大于：lost_count == max_lost 仍存活（§6.1 注记 2）
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
for _ in range(8):
|
||||
t.mark_miss()
|
||||
assert not t.is_dead, f"lost_count={t.lost_count} 未超过 max_lost 不应判死"
|
||||
assert t.lost_count == 8
|
||||
t.mark_miss() # lost_count = 9 > 8
|
||||
assert t.is_dead
|
||||
|
||||
|
||||
def test_is_coasting_when_lost_positive():
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
assert not t.is_coasting
|
||||
t.mark_miss()
|
||||
assert t.is_coasting # lost_count > 0 即 Coast（§3.2）
|
||||
|
||||
|
||||
# ---- 生命周期方法计数语义（§7） ----
|
||||
|
||||
def test_predict_advances_age_and_updates_last_state():
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
age_before = t.age
|
||||
t.predict() # 速度为零：位置不变，但 age+1、last_box 更新为预测框
|
||||
assert t.age == age_before + 1
|
||||
assert t.last_box is not None
|
||||
assert t.last_center[0] == pytest.approx(100.0, abs=1e-9)
|
||||
assert t.last_center[1] == pytest.approx(100.0, abs=1e-9)
|
||||
# hits / lost_count / confidence 不受预测影响
|
||||
assert t.hits == 1
|
||||
assert t.lost_count == 0
|
||||
assert t.confidence == 0.9
|
||||
|
||||
|
||||
def test_update_resets_lost_and_increments_hits():
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
t.mark_miss()
|
||||
t.mark_miss()
|
||||
assert t.lost_count == 2
|
||||
assert t.is_coasting
|
||||
|
||||
age_before = t.age
|
||||
box = BoxXYXY(150.0, 160.0, 170.0, 180.0)
|
||||
t.update(box, 0.75, 2)
|
||||
|
||||
assert t.lost_count == 0 # 匹配成功清零（§7）
|
||||
assert t.hits == 2 # hits+1
|
||||
assert t.age == age_before + 1 # age+1
|
||||
assert t.confidence == 0.75
|
||||
assert t.cls_id == 2
|
||||
assert t.last_center == (160.0, 170.0) # ((150+170)/2, (160+180)/2)
|
||||
assert t.last_box is not None
|
||||
assert t.last_box.x1 == box.x1
|
||||
assert t.last_raw_box is not None # 实测框同步记录
|
||||
assert t.last_raw_box.x2 == box.x2
|
||||
assert not t.is_coasting
|
||||
|
||||
|
||||
def test_mark_miss_increments_lost_and_age():
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
age_before = t.age
|
||||
t.mark_miss()
|
||||
assert t.lost_count == 1
|
||||
assert t.age == age_before + 1
|
||||
# MarkMiss 不影响 hits / confidence
|
||||
assert t.hits == 1
|
||||
assert t.confidence == 0.9
|
||||
@@ -1,7 +1,7 @@
|
||||
"""log_test.py —— 任务日志模块单元测试。
|
||||
|
||||
覆盖：open 建目录/写表头、事件 JSONL、遥测 CSV、telemetry_every_n 节流、
|
||||
disabled 时零副作用、close 收尾。
|
||||
disabled 时零副作用、close 收尾、简洁任务简报（task_brief.log）。
|
||||
"""
|
||||
|
||||
import csv
|
||||
@@ -121,3 +121,84 @@ def test_append_keeps_existing_rows(tmp_path):
|
||||
assert len(rows) == 2
|
||||
assert rows[0]["state"] == "wait"
|
||||
assert rows[1]["state"] == "goto"
|
||||
|
||||
|
||||
# --- 简洁任务简报（task_brief.log）-------------------------------------------
|
||||
def test_brief_log_lines_and_summary(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path))
|
||||
logger.open()
|
||||
logger.log_event(1000, "state", from_state="wait", to_state="wait",
|
||||
reason="任务启动")
|
||||
logger.log_event(2000, "state", from_state="wait", to_state="arm_takeoff",
|
||||
reason="收到地面坐标")
|
||||
logger.log_event(3000, "command", name="rtl")
|
||||
logger.log_event(4000, "fc_status", severity="WARNING", text="Low battery\t")
|
||||
logger.log_event(5000, "takeoff", altitude_m=5.0)
|
||||
logger.log_event(6000, "rtl")
|
||||
logger.log_telemetry(1000, TelemetryRecord(
|
||||
state="wait", relative_altitude_m=0.0, distance_to_target_m=33.5))
|
||||
logger.log_telemetry(1100, TelemetryRecord(
|
||||
state="arm_takeoff", relative_altitude_m=5.2, distance_to_target_m=20.0))
|
||||
logger.log_telemetry(1200, TelemetryRecord(
|
||||
state="arm_takeoff", relative_altitude_m=5.0,
|
||||
distance_to_target_m=float("inf")))
|
||||
logger.close()
|
||||
|
||||
text = (tmp_path / "task_brief.log").read_text(encoding="utf-8")
|
||||
assert "任务开始" in text
|
||||
assert "[00:00.0] 任务启动（当前: 待命）" in text
|
||||
assert "[00:01.0] 阶段: 待命 → 起飞（收到地面坐标）" in text
|
||||
assert "[00:02.0] 地面站命令: rtl" in text
|
||||
assert "[00:03.0] 飞控 WARNING: Low battery" in text # 尾随 tab 已 strip
|
||||
assert "[00:04.0] 起飞至 5.0 m" in text
|
||||
assert "[00:05.0] 开始返航" in text
|
||||
assert "本次运行汇总" in text
|
||||
assert "时长 0.2s" in text # 最后一行遥测的 t_s
|
||||
assert "最高 5.2m" in text
|
||||
assert "最近距目标 20.0m" in text # inf 不计入
|
||||
assert "事件 6 条" in text
|
||||
assert "状态链 待命→起飞" in text
|
||||
|
||||
|
||||
def test_brief_required_no_file_when_disabled_or_closed(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path, enabled=False))
|
||||
logger.open()
|
||||
logger.log_event(1000, "rtl")
|
||||
logger.close()
|
||||
assert not (tmp_path / "task_brief.log").exists()
|
||||
|
||||
|
||||
# --- 遥测落盘节流（2026-10-15：主循环 20Hz 不得每行同步 flush）----------------
|
||||
def test_telemetry_buffered_until_flush_interval(tmp_path):
|
||||
"""telemetry_flush_ms 内只写不 flush；close 兜底落盘，一行不丢。"""
|
||||
logger = TaskLogger(_cfg(tmp_path, telemetry_flush_ms=1000))
|
||||
logger.open()
|
||||
rec = TelemetryRecord(state="track")
|
||||
logger.log_telemetry(1000, rec) # 首行：落盘一次并打戳
|
||||
for i in range(1, 19): # 其后 19 行都在 1s 内 → 不再 flush
|
||||
logger.log_telemetry(1000 + i * 50, rec)
|
||||
logger.close()
|
||||
|
||||
with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
|
||||
rows = list(csv.DictReader(f))
|
||||
assert len(rows) == 19
|
||||
|
||||
|
||||
def test_telemetry_flush_zero_flushes_every_row(tmp_path):
|
||||
"""telemetry_flush_ms=0 保留旧的「每行立即 flush」行为（兼容/调试）。"""
|
||||
logger = TaskLogger(_cfg(tmp_path, telemetry_flush_ms=0))
|
||||
logger.open()
|
||||
logger.log_telemetry(1000, TelemetryRecord(state="goto"))
|
||||
assert logger._last_tel_flush_ms == 1000
|
||||
logger.close()
|
||||
|
||||
|
||||
def test_events_still_flushed_immediately(tmp_path):
|
||||
"""事件（阶段切换）仍即时落盘：地面站页面实时性不受遥测节流影响。"""
|
||||
logger = TaskLogger(_cfg(tmp_path, telemetry_flush_ms=10_000))
|
||||
logger.open()
|
||||
logger.log_event(1000, "state", from_state="goto", to_state="search")
|
||||
with open(tmp_path / "task_events.jsonl", encoding="utf-8") as f:
|
||||
lines = [json.loads(x) for x in f if x.strip()]
|
||||
assert len(lines) == 1
|
||||
logger.close()
|
||||
|
||||
@@ -1,7 +1,8 @@
|
||||
"""pod_control_test.py —— 吊舱目标识别/锁定控制器单元测试。
|
||||
|
||||
覆盖：arm_detection 发 0x07(识别,目标类型)+0x0F(自动锁定)、
|
||||
stop_tracking 发 0x0E(停止)+0x08(识别关)。
|
||||
stop_tracking 发 0x0E(停止)+0x08(识别关)、地面站手动命令
|
||||
stop_track(0x0E)/auto_lock(0x0F+0x10)/center(0x2B)。
|
||||
"""
|
||||
|
||||
from pod_bridge.protocol import ControlCode
|
||||
@@ -43,3 +44,31 @@ def test_stop_tracking_sends_stop_and_detect_off():
|
||||
assert len(bridge.sent) == 2
|
||||
assert bridge.sent[0].code == ControlCode.STOP_TRACK
|
||||
assert bridge.sent[1].code == ControlCode.TARGET_DETECT_OFF
|
||||
|
||||
|
||||
def test_stop_track_sends_only_stop():
|
||||
"""地面站「停跟踪」= 只发 0x0E（0x0F 常驻自动锁定仍生效，对齐按键 u）。"""
|
||||
bridge = FakeBridge()
|
||||
pc = PodController(bridge)
|
||||
pc.stop_track()
|
||||
|
||||
assert [c.code for c in bridge.sent] == [ControlCode.STOP_TRACK]
|
||||
|
||||
|
||||
def test_auto_lock_sends_autolock_and_detect_to_track():
|
||||
"""地面站「自动锁定」= 0x0F + 0x10（对齐按键 a）。"""
|
||||
bridge = FakeBridge()
|
||||
pc = PodController(bridge)
|
||||
pc.auto_lock()
|
||||
|
||||
assert [c.code for c in bridge.sent] == [ControlCode.AUTO_LOCK_ON_FOUND,
|
||||
ControlCode.DETECT_TO_TRACK]
|
||||
|
||||
|
||||
def test_center_sends_center():
|
||||
"""地面站「归中」= 0x2B（对齐按键 c）。"""
|
||||
bridge = FakeBridge()
|
||||
pc = PodController(bridge)
|
||||
pc.center()
|
||||
|
||||
assert [c.code for c in bridge.sent] == [ControlCode.CENTER]
|
||||
|
||||
@@ -0,0 +1,142 @@
|
||||
"""return_home_test.py —— 自定义返航（回起飞点 + 恢复初始航向）纯逻辑测试。
|
||||
|
||||
覆盖：转场速度/对准门控、到达判定、转场高度保持（含符号）、初始航向恢复、
|
||||
角度回绕、以及"不超过转场高度"的语义。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from task_runner.return_home import (HomingController, ReturnConfig,
|
||||
wrap_deg_180)
|
||||
|
||||
HOME_LAT, HOME_LON = 47.397971, 8.546164
|
||||
_DEG_LAT_M = 111320.0
|
||||
|
||||
|
||||
def _offset(lat0, lon0, north_m, east_m):
|
||||
return (lat0 + north_m / _DEG_LAT_M,
|
||||
lon0 + east_m / (_DEG_LAT_M * math.cos(math.radians(lat0))))
|
||||
|
||||
|
||||
def _ctrl(cfg=None, initial_yaw_deg=0.0, transit_alt_m=5.0):
|
||||
return HomingController(cfg or ReturnConfig(), HOME_LAT, HOME_LON,
|
||||
math.radians(initial_yaw_deg), transit_alt_m)
|
||||
|
||||
|
||||
# --- 角度回绕 ---------------------------------------------------------------
|
||||
@pytest.mark.parametrize("deg,expect", [
|
||||
(0.0, 0.0), (179.0, 179.0), (181.0, -179.0),
|
||||
(-181.0, 179.0), (360.0, 0.0), (-360.0, 0.0),
|
||||
])
|
||||
def test_wrap_deg_180(deg, expect):
|
||||
assert wrap_deg_180(deg) == pytest.approx(expect)
|
||||
|
||||
|
||||
# --- 转场：朝起飞点飞 -------------------------------------------------------
|
||||
def test_far_away_aligned_commands_full_speed():
|
||||
"""远在起飞点北侧、机头朝南（对准回家方向）→ 满速前进、几乎不转。"""
|
||||
cfg = ReturnConfig(speed_mps=2.0)
|
||||
c = _ctrl(cfg)
|
||||
# 在起飞点正北 50m，机头朝南（yaw=180°）→ 应朝南飞（北向速度为负）
|
||||
lat, lon = _offset(HOME_LAT, HOME_LON, 50.0, 0.0)
|
||||
out = c.update(lat, lon, math.radians(180.0), 5.0)
|
||||
assert out.arrived is False
|
||||
assert out.distance_m == pytest.approx(50.0, rel=5e-2)
|
||||
assert out.vx_mps == pytest.approx(2.0, rel=1e-2) # 对准 → 全速
|
||||
assert abs(out.yaw_rate_dps) < 1e-6
|
||||
|
||||
|
||||
def test_misaligned_gates_forward_speed():
|
||||
"""机头与回家方向垂直（90°）→ 前向速度被 cos(err) 门控为 0，只转向。"""
|
||||
c = _ctrl(ReturnConfig(speed_mps=2.0, yaw_rate_limit_dps=20.0))
|
||||
lat, lon = _offset(HOME_LAT, HOME_LON, 0.0, 50.0) # 起飞点正东 50m
|
||||
out = c.update(lat, lon, math.radians(0.0), 5.0) # 机头朝北
|
||||
assert out.vx_mps == pytest.approx(0.0, abs=1e-3) # cos(90°)=0（含地理近似误差）
|
||||
assert out.bearing_err_deg == pytest.approx(-90.0, abs=1e-2) # 回家方向在西
|
||||
assert out.yaw_rate_dps < 0.0 # 往左转（目标在西侧）
|
||||
|
||||
|
||||
def test_yaw_rate_limited():
|
||||
c = _ctrl(ReturnConfig(yaw_kp=5.0, yaw_rate_limit_dps=20.0))
|
||||
lat, lon = _offset(HOME_LAT, HOME_LON, 0.0, 50.0)
|
||||
out = c.update(lat, lon, math.radians(0.0), 5.0)
|
||||
assert out.yaw_rate_dps == pytest.approx(-20.0) # 被限幅（左转）
|
||||
|
||||
|
||||
def test_arrival_stops_forward_motion():
|
||||
"""进入到达半径 → arrived，且不再有前向速度。"""
|
||||
c = _ctrl(ReturnConfig(speed_mps=2.0, arrival_radius_m=3.0))
|
||||
lat, lon = _offset(HOME_LAT, HOME_LON, 1.0, 0.0) # 距起飞点 1m
|
||||
out = c.update(lat, lon, math.radians(180.0), 5.0)
|
||||
assert out.arrived is True
|
||||
assert out.vx_mps == 0.0
|
||||
|
||||
|
||||
def test_distance_measured_to_home():
|
||||
c = _ctrl()
|
||||
lat, lon = _offset(HOME_LAT, HOME_LON, 30.0, 40.0) # 3-4-5 三角形 → 50m
|
||||
out = c.update(lat, lon, 0.0, 5.0)
|
||||
assert out.distance_m == pytest.approx(50.0, rel=5e-2)
|
||||
|
||||
|
||||
# --- 转场高度保持（NED 下正）------------------------------------------------
|
||||
def test_alt_hold_climbs_when_below_transit():
|
||||
c = _ctrl(transit_alt_m=5.0)
|
||||
vz = c.alt_hold_vz(3.0) # 低于转场高度
|
||||
assert vz < 0.0 # NED 下正 → 负 = 爬升
|
||||
|
||||
|
||||
def test_alt_hold_descends_when_above_transit():
|
||||
c = _ctrl(transit_alt_m=5.0)
|
||||
vz = c.alt_hold_vz(7.0)
|
||||
assert vz > 0.0 # 正 = 下降
|
||||
|
||||
|
||||
def test_alt_hold_zero_at_target():
|
||||
assert _ctrl(transit_alt_m=5.0).alt_hold_vz(5.0) == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_alt_hold_rate_limited():
|
||||
cfg = ReturnConfig(alt_kp=10.0, alt_limit_mps=0.5)
|
||||
c = _ctrl(cfg, transit_alt_m=5.0)
|
||||
assert c.alt_hold_vz(0.0) == pytest.approx(-0.5) # 限幅
|
||||
assert c.alt_hold_vz(100.0) == pytest.approx(0.5)
|
||||
|
||||
|
||||
# --- 恢复初始航向 -----------------------------------------------------------
|
||||
def test_yaw_align_rate_and_direction():
|
||||
c = _ctrl(ReturnConfig(align_kp=1.0, align_rate_limit_dps=15.0),
|
||||
initial_yaw_deg=0.0)
|
||||
err, rate = c.yaw_align(math.radians(10.0)) # 当前偏右 10°
|
||||
assert err == pytest.approx(-10.0) # 需要往左转
|
||||
assert rate == pytest.approx(-10.0) # 负 = 左转
|
||||
|
||||
|
||||
def test_yaw_align_limited():
|
||||
c = _ctrl(ReturnConfig(align_kp=10.0, align_rate_limit_dps=15.0),
|
||||
initial_yaw_deg=0.0)
|
||||
_, rate = c.yaw_align(math.radians(90.0))
|
||||
assert rate == pytest.approx(-15.0)
|
||||
|
||||
|
||||
def test_yaw_aligned_within_tolerance():
|
||||
c = _ctrl(ReturnConfig(yaw_align_tolerance_deg=5.0), initial_yaw_deg=30.0)
|
||||
assert c.yaw_aligned(math.radians(33.0)) is True # 差 3° < 5°
|
||||
assert c.yaw_aligned(math.radians(40.0)) is False # 差 10°
|
||||
|
||||
|
||||
def test_yaw_align_wraps_around_north():
|
||||
"""初始航向 179°、当前 -179° → 误差 2°（不是 358°）。"""
|
||||
c = _ctrl(initial_yaw_deg=179.0)
|
||||
err, _ = c.yaw_align(math.radians(-179.0))
|
||||
assert abs(err) == pytest.approx(2.0, abs=1e-6)
|
||||
|
||||
|
||||
# --- 配置默认值 -------------------------------------------------------------
|
||||
def test_default_config_is_non_climbing_and_enabled():
|
||||
cfg = ReturnConfig()
|
||||
assert cfg.enabled is True
|
||||
assert cfg.alt_limit_mps > 0.0
|
||||
assert cfg.speed_mps > 0.0
|
||||
@@ -25,10 +25,17 @@ class FakePx4:
|
||||
self.telemetry_started = 0
|
||||
self.offboard_started = False
|
||||
self.brake_calls = []
|
||||
self.yaw_calls = [] # SEARCH 扫视下发记录
|
||||
self.velocity_calls = [] # send_velocity_heading 记录（返航转场用）
|
||||
self.fc_msgs = []
|
||||
|
||||
def own_state(self):
|
||||
return replace(self._state)
|
||||
|
||||
def take_fc_messages(self):
|
||||
msgs, self.fc_msgs = self.fc_msgs, []
|
||||
return msgs
|
||||
|
||||
async def start_telemetry(self):
|
||||
self.telemetry_started += 1
|
||||
|
||||
@@ -43,19 +50,26 @@ class FakePx4:
|
||||
pass
|
||||
|
||||
async def send_velocity_heading(self, out, t=None):
|
||||
self.velocity_calls.append(out)
|
||||
return True
|
||||
|
||||
async def send_brake_hover(self, t=None):
|
||||
self.brake_calls.append(t)
|
||||
return True
|
||||
|
||||
async def send_yaw_rate(self, yaw_rate_dps):
|
||||
self.yaw_calls.append(yaw_rate_dps)
|
||||
return True
|
||||
|
||||
|
||||
class FakeBridge:
|
||||
def __init__(self):
|
||||
self.started = False
|
||||
self.stopped = False
|
||||
self.landing_stopped = False # landing_stop()：只退 offboard、保留遥测
|
||||
self.law = None # set_heading_servo_enabled 最后值
|
||||
self.steps = 0
|
||||
self.search_steps = [] # SEARCH 态收到的扫视角速度
|
||||
self._locked = False
|
||||
self._telemetry = None
|
||||
|
||||
@@ -66,6 +80,9 @@ class FakeBridge:
|
||||
async def stop(self):
|
||||
self.stopped = True
|
||||
|
||||
async def landing_stop(self):
|
||||
self.landing_stopped = True
|
||||
|
||||
def set_heading_servo_enabled(self, enabled):
|
||||
self.law = enabled
|
||||
|
||||
@@ -75,6 +92,9 @@ class FakeBridge:
|
||||
async def step(self, now_ms):
|
||||
self.steps += 1
|
||||
|
||||
async def search_step(self, now_ms, yaw_rate_dps):
|
||||
self.search_steps.append(yaw_rate_dps)
|
||||
|
||||
@property
|
||||
def latest_telemetry(self):
|
||||
return self._telemetry
|
||||
@@ -83,10 +103,16 @@ class FakeBridge:
|
||||
class FakeGround:
|
||||
def __init__(self, target=None):
|
||||
self._target = target
|
||||
self.commands = [] # 地面站命令队列（供 drain_commands 取走）
|
||||
|
||||
def latest(self):
|
||||
return self._target or GroundTarget()
|
||||
|
||||
def drain_commands(self):
|
||||
cmds = list(self.commands)
|
||||
self.commands.clear()
|
||||
return cmds
|
||||
|
||||
|
||||
class FakeDrone:
|
||||
def __init__(self):
|
||||
@@ -96,6 +122,25 @@ class FakeDrone:
|
||||
self.rtl = False
|
||||
self.landed = False
|
||||
self.disarmed = False
|
||||
self.telemetry = FakeDrone._Telemetry()
|
||||
|
||||
class _Telemetry:
|
||||
"""模仿 MAVSDK `in_air()`：**异步生成器**，finalize 时取消底层流。
|
||||
|
||||
真实实现在 finally 里 `in_air_stream.cancel()`；「取一帧就 break」只是
|
||||
把生成器挂起，复用同一生成器即可避免反复订阅。
|
||||
"""
|
||||
|
||||
def __init__(self, frames=(False,)):
|
||||
self._frames = frames
|
||||
self.cancelled = False
|
||||
|
||||
async def in_air(self):
|
||||
try:
|
||||
for value in self._frames:
|
||||
yield value
|
||||
finally:
|
||||
self.cancelled = True
|
||||
|
||||
class _Action:
|
||||
def __init__(self, d):
|
||||
@@ -132,7 +177,8 @@ def _state(now_ms, lat=0.0, lon=0.0, rel_alt=0.0):
|
||||
|
||||
|
||||
def _make_runner(px4=None, bridge=None, ground=None, drone=None,
|
||||
sm_cfg=None, vision_fn=None, pod_control=None):
|
||||
sm_cfg=None, pod_control=None,
|
||||
land_instead_of_rtl=False, now_ms_fn=None):
|
||||
cfg = BridgeConfig()
|
||||
cfg.ground_inject.enabled = True
|
||||
cfg.ground_inject.timeout_ms = 5000
|
||||
@@ -143,9 +189,9 @@ def _make_runner(px4=None, bridge=None, ground=None, drone=None,
|
||||
TaskStateMachine(sm_cfg or TaskStateConfig()),
|
||||
ground or FakeGround(),
|
||||
drone or FakeDrone(),
|
||||
vision_fn=vision_fn,
|
||||
now_ms_fn=lambda: 1000,
|
||||
now_ms_fn=now_ms_fn or (lambda: 1000),
|
||||
pod_control=pod_control,
|
||||
land_instead_of_rtl=land_instead_of_rtl,
|
||||
)
|
||||
|
||||
|
||||
@@ -180,6 +226,15 @@ def test_collect_takeoff_reached():
|
||||
assert r._collect_inputs(1000).takeoff_reached is True
|
||||
|
||||
|
||||
def test_collect_takeoff_reached_tolerance():
|
||||
"""容差内（目标 5.0m，实际 4.6m ≥ 5.0-0.5）判到达；容差外（4.4m）判未到达。"""
|
||||
sm_cfg = TaskStateConfig(takeoff_altitude_m=5.0)
|
||||
px4_low = FakePx4(_state(1000, rel_alt=4.6))
|
||||
assert _make_runner(px4=px4_low, sm_cfg=sm_cfg)._collect_inputs(1000).takeoff_reached is True
|
||||
px4_below = FakePx4(_state(1000, rel_alt=4.4))
|
||||
assert _make_runner(px4=px4_below, sm_cfg=sm_cfg)._collect_inputs(1000).takeoff_reached is False
|
||||
|
||||
|
||||
def test_collect_locked_from_bridge():
|
||||
bridge = FakeBridge()
|
||||
bridge._locked = True
|
||||
@@ -188,14 +243,6 @@ def test_collect_locked_from_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()
|
||||
@@ -257,6 +304,15 @@ def test_on_enter_rtl_stops_bridge_and_returns():
|
||||
assert drone.rtl is True
|
||||
|
||||
|
||||
def test_land_instead_of_rtl_lands():
|
||||
"""--land 模式：收尾调 land 而不是 return_to_launch（真机就地回收）。"""
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(drone=drone, land_instead_of_rtl=True)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|
||||
assert drone.landed is True
|
||||
assert drone.rtl is False
|
||||
|
||||
|
||||
# --- 持续动作 ---------------------------------------------------------------
|
||||
def test_tick_goto_steps_bridge():
|
||||
bridge = FakeBridge()
|
||||
@@ -281,12 +337,24 @@ class FakePodControl:
|
||||
def __init__(self):
|
||||
self.armed = False
|
||||
self.stopped = False
|
||||
self.calls = []
|
||||
|
||||
def arm_detection(self):
|
||||
self.armed = True
|
||||
self.calls.append("arm_detection")
|
||||
|
||||
def stop_tracking(self):
|
||||
self.stopped = True
|
||||
self.calls.append("stop_tracking")
|
||||
|
||||
def stop_track(self):
|
||||
self.calls.append("stop_track")
|
||||
|
||||
def auto_lock(self):
|
||||
self.calls.append("auto_lock")
|
||||
|
||||
def center(self):
|
||||
self.calls.append("center")
|
||||
|
||||
|
||||
def test_on_enter_goto_arms_detection():
|
||||
@@ -309,3 +377,596 @@ def test_no_pod_control_is_safe():
|
||||
r = _make_runner(pod_control=None)
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|
||||
|
||||
|
||||
# --- 地面站命令分派 ---------------------------------------------------------
|
||||
def test_ground_command_rtl_sets_requested():
|
||||
ground = FakeGround()
|
||||
ground.commands.append("rtl")
|
||||
r = _make_runner(ground=ground)
|
||||
|
||||
r._apply_ground_commands()
|
||||
assert r._collect_inputs(1000).rtl_requested is True
|
||||
|
||||
|
||||
def test_ground_command_emergency_sets_flag():
|
||||
ground = FakeGround()
|
||||
ground.commands.append("emergency")
|
||||
r = _make_runner(ground=ground)
|
||||
|
||||
r._apply_ground_commands()
|
||||
assert r._collect_inputs(1000).emergency is True
|
||||
|
||||
|
||||
def test_ground_command_pod_dispatch():
|
||||
pod_control = FakePodControl()
|
||||
ground = FakeGround()
|
||||
ground.commands.extend(["pod_detect_on", "pod_auto_lock", "pod_stop_track",
|
||||
"pod_detect_off", "pod_center"])
|
||||
r = _make_runner(ground=ground, pod_control=pod_control)
|
||||
|
||||
r._apply_ground_commands()
|
||||
assert pod_control.calls == ["arm_detection", "auto_lock", "stop_track",
|
||||
"stop_tracking", "center"]
|
||||
|
||||
|
||||
def test_ground_command_pod_without_control_is_ignored():
|
||||
"""SITL（无 pod_control）→ 吊舱命令记警告忽略，不抛异常。"""
|
||||
ground = FakeGround()
|
||||
ground.commands.append("pod_center")
|
||||
r = _make_runner(ground=ground, pod_control=None)
|
||||
|
||||
r._apply_ground_commands() # 不抛异常即通过
|
||||
|
||||
|
||||
def test_ground_command_unknown_is_ignored():
|
||||
ground = FakeGround()
|
||||
ground.commands.append("unknown_cmd")
|
||||
r = _make_runner(ground=ground)
|
||||
|
||||
r._apply_ground_commands() # 不抛异常即通过
|
||||
|
||||
|
||||
def test_run_drains_ground_commands():
|
||||
"""run() 主循环每拍排空命令队列：RTL 命令 → 状态机转 RTL → 调 return_to_launch。"""
|
||||
ground = FakeGround()
|
||||
ground.commands.append("rtl")
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(ground=ground, drone=drone)
|
||||
|
||||
async def scenario():
|
||||
stop = asyncio.Event()
|
||||
task = asyncio.ensure_future(r.run(stop))
|
||||
try:
|
||||
for _ in range(100):
|
||||
if drone.rtl:
|
||||
break
|
||||
await asyncio.sleep(0.01)
|
||||
finally:
|
||||
stop.set()
|
||||
await task
|
||||
|
||||
asyncio.run(scenario())
|
||||
assert r.state is TaskState.RTL
|
||||
assert drone.rtl is True
|
||||
|
||||
|
||||
# --- 飞控状态文本落日志 -----------------------------------------------------
|
||||
class FakeLogger:
|
||||
def __init__(self):
|
||||
self.events = []
|
||||
|
||||
def open(self):
|
||||
pass
|
||||
|
||||
def close(self):
|
||||
pass
|
||||
|
||||
def log_event(self, now_ms, event_type, **fields):
|
||||
self.events.append((event_type, fields))
|
||||
|
||||
def log_telemetry(self, now_ms, rec):
|
||||
pass
|
||||
|
||||
|
||||
def test_fc_messages_logged_as_events():
|
||||
px4 = FakePx4(_state(1000))
|
||||
px4.fc_msgs = [("WARNING", "Low battery"), ("INFO", "Takeoff detected")]
|
||||
logger = FakeLogger()
|
||||
r = _make_runner(px4=px4)
|
||||
r._logger = logger
|
||||
|
||||
r._log_fc_messages()
|
||||
assert [e[0] for e in logger.events] == ["fc_status", "fc_status"]
|
||||
assert logger.events[0][1] == {"severity": "WARNING", "text": "Low battery"}
|
||||
assert px4.fc_msgs == [] # 已排空，不重复记录
|
||||
|
||||
|
||||
# --- SEARCH 搜索扫视（2026-10-15 新增）--------------------------------------
|
||||
def test_on_enter_search_disables_servo_and_resets_sweep():
|
||||
bridge = FakeBridge()
|
||||
r = _make_runner(bridge=bridge)
|
||||
r._sweep_dir = 9 # 脏值，应被重置
|
||||
asyncio.run(r._on_enter(TaskState.SEARCH, 1000))
|
||||
assert bridge.law is False # 搜索不用控制律（不由 L1/L2 出指令）
|
||||
assert r._sweep_dir == 0 # 每次进搜索从右向起扫
|
||||
assert r._sweep_since_ms == 1000
|
||||
|
||||
|
||||
def test_search_yaw_rate_sweeps_both_directions():
|
||||
"""扫视按 sweep_reverse_s 左右交替，幅值为配置角速度。"""
|
||||
sm_cfg = TaskStateConfig(sweep_rate_dps=15.0, sweep_reverse_s=4.0)
|
||||
r = _make_runner(sm_cfg=sm_cfg)
|
||||
assert r._search_yaw_rate(1000) == pytest.approx(15.0) # 先向右
|
||||
assert r._search_yaw_rate(4999) == pytest.approx(15.0) # 未到换向点
|
||||
assert r._search_yaw_rate(5000) == pytest.approx(-15.0) # 换向左
|
||||
assert r._search_yaw_rate(9000) == pytest.approx(15.0) # 再换回右
|
||||
|
||||
|
||||
def test_search_yaw_rate_zero_disables_sweep():
|
||||
sm_cfg = TaskStateConfig(sweep_rate_dps=0.0, sweep_reverse_s=4.0)
|
||||
r = _make_runner(sm_cfg=sm_cfg)
|
||||
assert r._search_yaw_rate(1000) == 0.0
|
||||
|
||||
|
||||
def test_search_yaw_rate_continuous_when_reverse_disabled():
|
||||
"""sweep_reverse_s=0 → 单向持续扫视（不换向）。"""
|
||||
sm_cfg = TaskStateConfig(sweep_rate_dps=10.0, sweep_reverse_s=0.0)
|
||||
r = _make_runner(sm_cfg=sm_cfg)
|
||||
assert r._search_yaw_rate(1000) == pytest.approx(10.0)
|
||||
assert r._search_yaw_rate(600_000) == pytest.approx(10.0)
|
||||
|
||||
|
||||
def test_tick_search_sends_sweep_step():
|
||||
bridge = FakeBridge()
|
||||
sm_cfg = TaskStateConfig(sweep_rate_dps=12.0, sweep_reverse_s=4.0)
|
||||
r = _make_runner(bridge=bridge, sm_cfg=sm_cfg)
|
||||
r._sm.force_state(TaskState.SEARCH, 1000)
|
||||
asyncio.run(r._tick(1000))
|
||||
assert bridge.search_steps == [pytest.approx(12.0)]
|
||||
assert bridge.steps == 0 # 搜索不走控制律 step
|
||||
|
||||
|
||||
def test_search_step_polls_pod_so_lock_can_be_fresh():
|
||||
"""SEARCH 必须 poll 吊舱：否则遥测超龄，搜到目标也切不进 TRACK。"""
|
||||
from pod_bridge.bridge import PodBridge
|
||||
from pod_bridge.protocol import Telemetry
|
||||
|
||||
class FakePod:
|
||||
def __init__(self):
|
||||
self.polls = 0
|
||||
|
||||
def poll(self):
|
||||
self.polls += 1
|
||||
return [Telemetry(status1=0b100, off_target_x_deg=1.0,
|
||||
off_target_y_deg=0.0)]
|
||||
|
||||
def send(self, data):
|
||||
pass
|
||||
|
||||
def close(self):
|
||||
pass
|
||||
|
||||
pod = FakePod()
|
||||
px4 = FakePx4(_state(1000))
|
||||
cfg = BridgeConfig()
|
||||
cfg.link.require_laser_valid = False # 本吊舱无激光（与部署配置一致）
|
||||
bridge = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 1000,
|
||||
ground_target_fn=lambda: None)
|
||||
asyncio.run(bridge.search_step(1000, -15.0))
|
||||
|
||||
assert pod.polls == 1
|
||||
assert bridge.pod_locked(1000) is True # 本拍锁定位已刷新
|
||||
assert px4.yaw_calls == [pytest.approx(-15.0)] # 扫视角速度已下发
|
||||
|
||||
|
||||
# --- 进入动作只执行一次（防重入导致的自激）----------------------------------
|
||||
class _CountingPodControl:
|
||||
def __init__(self):
|
||||
self.arm_calls = 0
|
||||
self.stop_calls = 0
|
||||
|
||||
def arm_detection(self):
|
||||
self.arm_calls += 1
|
||||
|
||||
def stop_tracking(self):
|
||||
self.stop_calls += 1
|
||||
|
||||
def stop_track(self):
|
||||
pass
|
||||
|
||||
def auto_lock(self):
|
||||
pass
|
||||
|
||||
def center(self):
|
||||
pass
|
||||
|
||||
|
||||
def test_pod_detection_armed_only_once_across_reentry():
|
||||
"""丢目标回 SEARCH 再进 TRACK 时，不得重发 0x07/0x0F（会打断正在进行的跟踪）。"""
|
||||
pod_control = _CountingPodControl()
|
||||
r = _make_runner(pod_control=pod_control)
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
asyncio.run(r._on_enter(TaskState.SEARCH, 2000))
|
||||
asyncio.run(r._on_enter(TaskState.TRACK, 3000))
|
||||
asyncio.run(r._on_enter(TaskState.SEARCH, 4000)) # 丢目标回搜索
|
||||
asyncio.run(r._on_enter(TaskState.TRACK, 5000)) # 再次锁定
|
||||
assert pod_control.arm_calls == 1
|
||||
|
||||
|
||||
def test_rtl_stops_pod_tracking_even_after_reentry():
|
||||
pod_control = _CountingPodControl()
|
||||
r = _make_runner(pod_control=pod_control)
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
assert pod_control.stop_calls == 1
|
||||
|
||||
|
||||
# --- 主循环异常兜底：必须返航，不能静默停摆 ---------------------------------
|
||||
def test_iteration_exception_triggers_rtl():
|
||||
"""主循环单周期异常 → 必须转紧急返航，不能只停机静默停摆。
|
||||
|
||||
异常注入在**每拍都跑**的输入采集路径（own_state），与当前状态无关。
|
||||
"""
|
||||
px4 = FakePx4(_state(1000))
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(px4=px4, drone=drone)
|
||||
calls = {"n": 0}
|
||||
|
||||
def boom():
|
||||
calls["n"] += 1
|
||||
raise RuntimeError("模拟瞬态异常（遥测/串口抖动）")
|
||||
|
||||
px4.own_state = boom
|
||||
|
||||
async def scenario():
|
||||
stop = asyncio.Event()
|
||||
task = asyncio.ensure_future(r.run(stop))
|
||||
for _ in range(300):
|
||||
if drone.rtl:
|
||||
break
|
||||
await asyncio.sleep(0.005)
|
||||
stop.set()
|
||||
task.cancel()
|
||||
try:
|
||||
await task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
|
||||
asyncio.run(scenario())
|
||||
assert calls["n"] >= 1
|
||||
assert drone.rtl is True # 异常兜底确实触发了返航
|
||||
assert r.state is TaskState.RTL
|
||||
|
||||
|
||||
def test_iteration_exception_records_event():
|
||||
"""异常兜底要在任务日志留痕，便于事后定位。"""
|
||||
px4 = FakePx4(_state(1000))
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(px4=px4, drone=drone)
|
||||
logger = FakeLogger()
|
||||
r._logger = logger
|
||||
|
||||
def boom():
|
||||
raise RuntimeError("串口拔线")
|
||||
|
||||
px4.own_state = boom
|
||||
|
||||
async def scenario():
|
||||
stop = asyncio.Event()
|
||||
task = asyncio.ensure_future(r.run(stop))
|
||||
for _ in range(300):
|
||||
if drone.rtl:
|
||||
break
|
||||
await asyncio.sleep(0.005)
|
||||
stop.set()
|
||||
task.cancel()
|
||||
try:
|
||||
await task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
|
||||
asyncio.run(scenario())
|
||||
types = [e[0] for e in logger.events]
|
||||
assert "iteration_error" in types
|
||||
assert "rtl" in types # 兜底返航事件也落盘
|
||||
|
||||
|
||||
def test_safe_abort_is_noop_when_already_rtl():
|
||||
"""已在 RTL 时兜底不重复触发返航（避免重复 RTL 指令）。"""
|
||||
bridge = FakeBridge()
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(bridge=bridge, drone=drone)
|
||||
r._sm.force_state(TaskState.RTL, 0)
|
||||
asyncio.run(r._safe_abort())
|
||||
assert drone.rtl is False
|
||||
assert bridge.stopped is False
|
||||
|
||||
|
||||
# --- in_air 遥测流复用（降低服务端订阅churn）--------------------------------
|
||||
def test_in_air_stream_reused_across_ticks():
|
||||
"""in_air 流只订阅一次：每拍新建会在服务端反复订阅/退订同一条流。
|
||||
|
||||
注意必须在**同一个事件循环**内连续调用（与生产一致：主循环是单 loop）；
|
||||
跨 loop 复用 async generator 会被 asyncio 拒绝，实现里也就丢弃重建。
|
||||
"""
|
||||
drone = FakeDrone()
|
||||
calls = {"n": 0}
|
||||
|
||||
class CountingTelemetry:
|
||||
def in_air(self):
|
||||
calls["n"] += 1
|
||||
return FakeDrone._Telemetry().in_air()
|
||||
|
||||
drone.telemetry = CountingTelemetry()
|
||||
r = _make_runner(drone=drone)
|
||||
|
||||
async def scenario():
|
||||
for _ in range(5):
|
||||
await r._check_landed()
|
||||
|
||||
asyncio.run(scenario())
|
||||
assert calls["n"] == 1 # 只订阅一次
|
||||
assert r.rtl_done is True # in_air=False → 判落地并 disarm
|
||||
|
||||
|
||||
def test_in_air_stream_rebuilt_when_broken():
|
||||
"""流建立/消费失败时丢弃缓存，下拍重建（不永久卡在坏流上）。"""
|
||||
drone = FakeDrone()
|
||||
calls = {"n": 0}
|
||||
|
||||
def broken():
|
||||
calls["n"] += 1
|
||||
raise RuntimeError("stream broken")
|
||||
|
||||
class FlakyTelemetry:
|
||||
def in_air(self):
|
||||
return broken()
|
||||
|
||||
drone.telemetry = FlakyTelemetry()
|
||||
r = _make_runner(drone=drone)
|
||||
|
||||
async def scenario():
|
||||
await r._check_landed()
|
||||
assert calls["n"] == 1
|
||||
assert r._in_air_stream is None # 已丢弃
|
||||
await r._check_landed()
|
||||
assert calls["n"] == 2 # 重建后再试
|
||||
|
||||
asyncio.run(scenario())
|
||||
|
||||
|
||||
# --- RTL 落地等待上限（真机在环发现：判不出落地会无限等）--------------------
|
||||
def test_rtl_timeout_ends_task_when_landing_unobservable():
|
||||
"""in_air 始终不报落地 → 超过 rtl_timeout_s 后结束任务，不无限等待。"""
|
||||
now = {"ms": 1_000_000}
|
||||
drone = FakeDrone()
|
||||
|
||||
class AlwaysAirborne:
|
||||
async def in_air(self):
|
||||
while True:
|
||||
yield True
|
||||
|
||||
drone.telemetry = AlwaysAirborne()
|
||||
sm_cfg = TaskStateConfig(rtl_timeout_s=5.0)
|
||||
r = _make_runner(drone=drone, sm_cfg=sm_cfg,
|
||||
now_ms_fn=lambda: now["ms"])
|
||||
r._rtl_entered_ms = now["ms"]
|
||||
r._rtl_phase = "land"
|
||||
|
||||
async def scenario():
|
||||
now["ms"] += 4_000 # 未到上限：不应结束
|
||||
await r._tick_rtl(now["ms"])
|
||||
assert r.rtl_done is False
|
||||
now["ms"] += 2_000 # 超过上限：结束任务
|
||||
await r._tick_rtl(now["ms"])
|
||||
assert r.rtl_done is True
|
||||
|
||||
asyncio.run(scenario())
|
||||
|
||||
|
||||
def test_rtl_timeout_not_triggered_after_landing():
|
||||
"""已判落地后不再触发超时逻辑（rtl_done 保持）。"""
|
||||
now = {"ms": 1_000_000}
|
||||
drone = FakeDrone() # 默认 in_air 首帧 False
|
||||
sm_cfg = TaskStateConfig(rtl_timeout_s=1.0)
|
||||
r = _make_runner(drone=drone, sm_cfg=sm_cfg,
|
||||
now_ms_fn=lambda: now["ms"])
|
||||
r._rtl_entered_ms = now["ms"]
|
||||
r._rtl_phase = "land"
|
||||
|
||||
async def scenario():
|
||||
await r._tick_rtl(now["ms"])
|
||||
assert r.rtl_done is True
|
||||
now["ms"] += 60_000 # 大幅超过上限
|
||||
await r._tick_rtl(now["ms"])
|
||||
assert r.rtl_done is True
|
||||
|
||||
asyncio.run(scenario())
|
||||
|
||||
|
||||
# --- 自定义返航：回起飞点 → 恢复初始航向 → 降落 -----------------------------
|
||||
def _armed_runner(px4=None, drone=None, return_cfg=None):
|
||||
"""走完 解锁→GOTO（记录起飞点 + 进入 offboard）的 runner。"""
|
||||
px4 = px4 or FakePx4(_state(1000, lat=0.0, lon=0.0))
|
||||
r = _make_runner(px4=px4, drone=drone or FakeDrone())
|
||||
if return_cfg is not None:
|
||||
r._return_cfg = return_cfg
|
||||
asyncio.run(r._on_enter(TaskState.ARM_TAKEOFF, 1000))
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
return r, px4
|
||||
|
||||
|
||||
def test_custom_return_selected_after_takeoff():
|
||||
"""已起飞 + 有起飞点 → 进入 RTL 走自定义返航，不调 PX4 RTL。"""
|
||||
drone = FakeDrone()
|
||||
r, _ = _armed_runner(drone=drone)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
assert r._rtl_phase == "return"
|
||||
assert drone.rtl is False # 不再交给 PX4 原生 RTL
|
||||
assert r._bridge_started is True
|
||||
|
||||
|
||||
def test_custom_return_records_home_and_initial_yaw():
|
||||
r, px4 = _armed_runner()
|
||||
assert r._home_lat == pytest.approx(0.0)
|
||||
assert r._home_lon == pytest.approx(0.0)
|
||||
assert r._initial_yaw_rad == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_return_step_commands_velocity_toward_home():
|
||||
px4 = FakePx4(_state(1000, lat=0.0, lon=0.0))
|
||||
r, _ = _armed_runner(px4=px4)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
|
||||
# 飞到起飞点正北 ~111m，机头朝南（对准回家方向）
|
||||
px4._state = _state(3000, lat=0.001, lon=0.0, rel_alt=5.0)
|
||||
px4._state.yaw_rad = 3.141592653589793
|
||||
px4.velocity_calls.clear()
|
||||
asyncio.run(r._tick_rtl(3000))
|
||||
|
||||
assert r._rtl_phase == "return"
|
||||
assert len(px4.velocity_calls) == 1
|
||||
out = px4.velocity_calls[0]
|
||||
assert out.vx_body_mps > 0.0 # 朝起飞点前进
|
||||
assert out.vz_mps == pytest.approx(0.0) # 转场高度 5m = 当前高度
|
||||
|
||||
|
||||
def test_return_arrival_switches_to_yaw_phase():
|
||||
px4 = FakePx4(_state(1000, lat=0.0, lon=0.0))
|
||||
r, _ = _armed_runner(px4=px4)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
|
||||
px4._state = _state(3000, lat=0.000005, lon=0.0, rel_alt=5.0) # ~0.5m
|
||||
asyncio.run(r._tick_rtl(3000))
|
||||
assert r._rtl_phase == "yaw"
|
||||
|
||||
|
||||
def test_yaw_phase_restores_heading_then_lands():
|
||||
drone = FakeDrone()
|
||||
px4 = FakePx4(_state(1000, lat=0.0, lon=0.0))
|
||||
r, _ = _armed_runner(px4=px4, drone=drone)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
r._rtl_phase = "yaw"
|
||||
r._yaw_started_ms = 2000
|
||||
|
||||
px4._state = _state(3000, lat=0.0, lon=0.0, rel_alt=5.0) # 航向已回 0
|
||||
px4.velocity_calls.clear()
|
||||
asyncio.run(r._tick_rtl(3000))
|
||||
|
||||
assert len(px4.velocity_calls) == 1 # 先发了一拍"原地保持"指令
|
||||
assert r._rtl_phase == "land"
|
||||
assert drone.landed is True # 航向对齐后降落
|
||||
assert r._land_cmd_sent is True
|
||||
|
||||
|
||||
def test_landing_keeps_telemetry_running():
|
||||
"""降落阶段只退 offboard、保留遥测（否则日志冻结在降落前高度）。"""
|
||||
drone = FakeDrone()
|
||||
bridge = FakeBridge()
|
||||
r, _ = _armed_runner(drone=drone)
|
||||
r._bridge = bridge
|
||||
asyncio.run(r._start_land())
|
||||
assert bridge.landing_stopped is True
|
||||
assert bridge.stopped is False # 尚未全停：遥测/串口仍在
|
||||
assert drone.landed is True
|
||||
|
||||
|
||||
def test_land_phase_completes_on_landed():
|
||||
drone = FakeDrone() # in_air -> False
|
||||
r, _ = _armed_runner(drone=drone)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
r._rtl_phase = "land"
|
||||
asyncio.run(r._tick_rtl(3000))
|
||||
assert r.rtl_done is True
|
||||
assert drone.disarmed is True
|
||||
|
||||
|
||||
def test_return_timeout_falls_back_to_px4_rtl():
|
||||
drone = FakeDrone()
|
||||
r, px4 = _armed_runner(drone=drone)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
|
||||
px4._state = _state(3000, lat=0.001, lon=0.0, rel_alt=5.0)
|
||||
# 超过转场超时
|
||||
late = 2000 + int(r._return_cfg.timeout_s * 1000) + 1000
|
||||
asyncio.run(r._tick_rtl(late))
|
||||
assert r._rtl_phase == "fallback"
|
||||
assert drone.rtl is True # 退回 PX4 原生 RTL
|
||||
|
||||
|
||||
def test_custom_return_disabled_uses_px4_rtl():
|
||||
from task_runner.return_home import ReturnConfig
|
||||
|
||||
drone = FakeDrone()
|
||||
r, _ = _armed_runner(drone=drone,
|
||||
return_cfg=ReturnConfig(enabled=False))
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
assert r._rtl_phase == "fallback"
|
||||
assert drone.rtl is True
|
||||
|
||||
|
||||
def test_rtl_before_takeoff_uses_px4_rtl():
|
||||
"""未起飞（还在待命/地面）时不能自主返航 → 退回 PX4 RTL。"""
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(drone=drone) # 未记录起飞点、未进 offboard
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
assert r._rtl_phase == "fallback"
|
||||
assert drone.rtl is True
|
||||
|
||||
|
||||
def test_safe_abort_prefers_px4_fallback():
|
||||
"""异常兜底不再信任自身状态：强制走 PX4 RTL。"""
|
||||
drone = FakeDrone()
|
||||
r, _ = _armed_runner(drone=drone)
|
||||
asyncio.run(r._safe_abort())
|
||||
assert r._rtl_phase == "fallback"
|
||||
assert drone.rtl is True
|
||||
|
||||
|
||||
# --- 起飞点记录时机（SITL 实测：解锁时 gps_info 可能还没到）------------------
|
||||
def test_home_recorded_when_position_becomes_valid_during_takeoff():
|
||||
"""解锁瞬间定位无效 → 起飞阶段每拍补记，不丢起飞点。"""
|
||||
px4 = FakePx4(_state(1000, lat=0.0, lon=0.0))
|
||||
px4._state.position_valid = False
|
||||
r = _make_runner(px4=px4)
|
||||
r._sm.force_state(TaskState.ARM_TAKEOFF, 0)
|
||||
|
||||
r._collect_inputs(1000)
|
||||
assert r._home_lat is None # 定位无效 → 暂不记录
|
||||
|
||||
px4._state = _state(1000, lat=0.0012, lon=0.0034)
|
||||
r._collect_inputs(1100)
|
||||
assert r._home_lat == pytest.approx(0.0012) # 补记成功
|
||||
assert r._home_lon == pytest.approx(0.0034)
|
||||
|
||||
|
||||
def test_home_not_recorded_once_airborne():
|
||||
"""进入 GOTO（已在空中）后不再把当前位置当作起飞点。"""
|
||||
px4 = FakePx4(_state(1000, lat=0.005, lon=0.0))
|
||||
r = _make_runner(px4=px4)
|
||||
r._sm.force_state(TaskState.GOTO, 0)
|
||||
r._collect_inputs(1000)
|
||||
assert r._home_lat is None
|
||||
|
||||
|
||||
def test_home_recorded_only_once():
|
||||
px4 = FakePx4(_state(1000, lat=0.0, lon=0.0))
|
||||
r = _make_runner(px4=px4)
|
||||
r._sm.force_state(TaskState.ARM_TAKEOFF, 0)
|
||||
r._collect_inputs(1000)
|
||||
first = (r._home_lat, r._home_lon, r._initial_yaw_rad)
|
||||
|
||||
px4._state = _state(1000, lat=0.009, lon=0.009) # 位置已变（飞机在爬升/移动）
|
||||
r._collect_inputs(1100)
|
||||
assert (r._home_lat, r._home_lon, r._initial_yaw_rad) == first
|
||||
|
||||
|
||||
def test_record_home_returns_false_without_fix():
|
||||
px4 = FakePx4(_state(1000))
|
||||
px4._state.position_valid = False
|
||||
r = _make_runner(px4=px4)
|
||||
assert r._record_home() is False
|
||||
assert r._home_lat is None
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Reference in New Issue
Block a user