songhaoyu/code-test-drone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

双目测距方案重构与其他小修改

Browse Source
This commit is contained in:
keita
2026-09-10 16:16:05 +08:00
parent 3325465359
commit 538ae4c6c7
39 changed files with 2285 additions and 1444 deletions
Download Patch File Download Diff File Expand all files Collapse all files
utils/tests/stereo_ranger/config_test.py
-76
View File
@@ -1,76 +0,0 @@
"""StereoConfig 默认值、派生区间与 JSON 加载测试（规格 §2：参数全部外置）。"""
import json
import pytest
from stereo_ranger.config import StereoConfig, load_from_json
VALID = {
"fx_px": 1663.0, "fy_px": 1663.0, "cx_px": 960.0, "cy_px": 540.0,
"baseline_m": 0.12, "z_min_m": 2.0, "z_max_m": 40.0,
"vertical_tau_px": 3.0, "height_ratio_tau": 0.3,
"disparity_min_px": 4.0, "range_tol_m": 3.0, "disp_noise_px": 1.5,
"use_center_fallback": False, "filter_enabled": True,
"filter_alpha": 0.5, "filter_jump_ratio": 0.1, "warn_interval_s": 1.0,
}
def test_defaults_and_disparity_window():
cfg = StereoConfig()
assert cfg.baseline_m == 0.06 and cfg.fx_px == 960.0
# FB = 57.6，z 窗口 [1, 50] m -> D 窗口 [57.6/50, 57.6/1]
lo, hi = cfg.disparity_bounds_px()
assert lo == pytest.approx(1.152)
assert hi == pytest.approx(57.6)
@pytest.mark.parametrize("kwargs", [
{"baseline_m": 0.0},
{"fx_px": -1.0},
{"fy_px": 0.0},
{"cx_px": -1.0},
{"z_min_m": 0.0},
{"z_max_m": 10.0, "z_min_m": 20.0},
{"vertical_tau_px": 0.0},
{"height_ratio_tau": 0.0},
{"disparity_min_px": 0.0},
{"range_tol_m": 0.0},
{"disp_noise_px": 0.0},
{"filter_alpha": 0.0},
{"filter_alpha": 1.5},
{"filter_jump_ratio": -0.1},
{"warn_interval_s": -1.0},
])
def test_invalid_config_raises(kwargs):
with pytest.raises(ValueError):
StereoConfig(**kwargs)
def test_load_from_json_roundtrip(tmp_path):
p = tmp_path / "stereo.json"
p.write_text(json.dumps(VALID), encoding="utf-8")
cfg = load_from_json(str(p))
for field, value in VALID.items():
assert getattr(cfg, field) == value
lo, hi = cfg.disparity_bounds_px()
assert hi == pytest.approx(1663.0 * 0.12 / 2.0) # FB / z_min
assert lo == pytest.approx(1663.0 * 0.12 / 40.0) # FB / z_max
def test_load_from_json_missing_field_reports_name(tmp_path):
p = tmp_path / "stereo.json"
data = dict(VALID)
del data["baseline_m"]
p.write_text(json.dumps(data), encoding="utf-8")
with pytest.raises(ValueError, match="baseline_m"):
load_from_json(str(p))
def test_load_from_json_unknown_field_rejected(tmp_path):
p = tmp_path / "stereo.json"
data = dict(VALID)
data["magic_extra"] = 1
p.write_text(json.dumps(data), encoding="utf-8")
with pytest.raises(ValueError, match="magic_extra"):
load_from_json(str(p))
utils/tests/stereo_ranger/stereo_ranger_test.py
-302
View File
@@ -1,302 +0,0 @@
"""stereo_ranger 测距核心单元测试。
规格：docs/双目测距方案.md §4（几何）、§5（视差/有效性/误差）、§6（关联）、
§7（滤波）。场景全部由"目标在左目系 (X, Y, Z)"解析投影生成左右框，
再反推验证测距/方位输出。
"""
import math
import pytest
from stereo_ranger.config import StereoConfig
from stereo_ranger.stereo_ranger import (
DISP_SOURCE_CENTER,
DISP_SOURCE_EDGE,
StereoRanger,
focal_from_hfov,
range_error_abs,
range_error_rel,
)
from stereo_ranger.types import Detection
CFG = StereoConfig()
FB = CFG.fx_px * CFG.baseline_m # 57.6 px·m
def box(cx, cy, w=100.0, h=60.0, cls_id=0, conf=1.0, track_id=0):
return Detection(cx - w / 2, cy - h / 2, cx + w / 2, cy + h / 2,
conf, cls_id, track_id)
def target_pair(x_m, y_m, z_m, w=100.0, h=60.0, cls_id=0, conf=1.0, track_id=0):
"""目标中心在左目系 (x_m, y_m, z_m)，解析投影为左右目同尺寸框。"""
u_l = CFG.fx_px * x_m / z_m + CFG.cx_px
v_l = CFG.fy_px * y_m / z_m + CFG.cy_px
u_r = u_l - FB / z_m
return (box(u_l, v_l, w, h, cls_id, conf, track_id),
box(u_r, v_l, w, h, cls_id, conf, track_id))
def one_report(ranger, left, right, **kw):
reports = ranger.update(left, right, **kw)
assert len(reports) == len(left)
return reports
# ---- §4.2 / §5.3 几何与误差公式 ----
def test_focal_from_hfov_matches_doc_examples():
assert focal_from_hfov(1920.0, 90.0) == pytest.approx(960.0)
assert focal_from_hfov(1920.0, 60.0) == pytest.approx(1662.769)
def test_range_error_matches_doc_table():
# 文档 §5.3 示意表：B=0.06, F=960, δD=1 px
expected = {5: 0.4340, 10: 1.7361, 15: 3.9063, 20: 6.9444,
30: 15.625, 50: 43.4028, 100: 173.611}
for z, err in expected.items():
assert range_error_abs(z, 960.0, 0.06, 1.0) == pytest.approx(err, rel=1e-3)
assert range_error_rel(10.0, 960.0, 0.06, 1.0) == pytest.approx(1.7361 / 10.0, rel=1e-3)
def test_on_axis_target_z_and_bearing():
left, right = target_pair(0.0, 0.0, 10.0)
r = one_report(StereoRanger(), [left], [right], frame_sequence=7, timestamp_s=1.5)[0]
assert r.matched and r.valid
assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9)
assert r.disparity_px == pytest.approx(FB / 10.0)
assert r.disparity_source == DISP_SOURCE_EDGE
assert r.lateral_m == pytest.approx(0.0, abs=1e-9)
assert r.vertical_m == pytest.approx(0.0, abs=1e-9)
assert r.slant_range_m == pytest.approx(10.0, rel=1e-9)
assert r.frame_sequence == 7 and r.timestamp_s == 1.5
# 虚拟双目中心：两目中心平均，y 取左目（§11.3）
assert r.centre_pixel_x_mid == pytest.approx((left.cx + right.cx) / 2.0)
assert r.centre_pixel_y_mid == left.cy
def test_off_axis_target_xyz_and_slant_range():
left, right = target_pair(1.2, 0.3, 10.0)
r = one_report(StereoRanger(), [left], [right])[0]
assert r.valid
assert r.lateral_m == pytest.approx(1.2, rel=1e-9)
assert r.vertical_m == pytest.approx(0.3, rel=1e-9)
assert r.slant_range_m == pytest.approx(math.sqrt(1.2**2 + 0.3**2 + 10.0**2), rel=1e-9)
assert r.error_est_m == pytest.approx(range_error_abs(10.0, CFG.fx_px, CFG.baseline_m, CFG.disp_noise_px))
def test_x_uses_left_camera_frame_not_virtual_center():
# X 定义在左目系（§4.2），用左目中心像素；方位 x_mid 是虚拟相机量（§11.3）
left, right = target_pair(0.0, 0.0, 10.0)
r = one_report(StereoRanger(), [left], [right])[0]
assert r.lateral_m == pytest.approx(0.0, abs=1e-9) # 目标在左光轴上
assert r.centre_pixel_x_mid < left.cx # 但虚拟中心相对左光轴偏左 B/2
def test_width_diff_on_right_edge_only_does_not_taint_left_edge():
# §5.1 主方案：同侧（左）边缘对视差免疫"宽度差"。右框仅向 x2 侧扩宽
# 时，左边缘不变、中心被拉偏——边缘取点仍得精确 Z。
left, right = target_pair(0.0, 0.0, 10.0)
right_extended = Detection(right.x1, right.y1, right.x2 + 40.0, right.y2)
r = one_report(StereoRanger(), [left], [right_extended])[0]
assert r.matched and r.valid
assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9)
assert r.disparity_source == DISP_SOURCE_EDGE
assert right_extended.width != left.width # 确有宽度差
def test_center_fallback_when_edge_out_of_window(caplog):
# 左框过宽使左边缘视差越出窗口：回退中心并节流警告（§5.1），同实例内不刷屏
left, right = target_pair(0.0, 0.0, 10.0)
left_wider = box(left.cx, left.cy, w=300.0, h=60.0)
ranger = StereoRanger()
r = one_report(ranger, [left_wider], [right])[0]
r2 = one_report(ranger, [left_wider], [right])[0]
assert r.matched and r.valid
assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9)
assert r.disparity_source == DISP_SOURCE_CENTER
assert r2.disparity_source == DISP_SOURCE_CENTER
assert len(caplog.records) == 1
def test_center_fallback_disabled_leaves_unmatched():
left, right = target_pair(0.0, 0.0, 10.0)
left_wider = box(left.cx, left.cy, w=300.0, h=60.0)
cfg = StereoConfig(use_center_fallback=False)
r = one_report(StereoRanger(cfg), [left_wider], [right])[0]
assert not r.matched and not r.valid
assert r.forward_distance_m is None
# ---- §6 关联硬约束与贪心一一配对 ----
def test_class_mismatch_rejected():
left, _ = target_pair(0.0, 0.0, 10.0)
_, right = target_pair(0.0, 0.0, 10.0, cls_id=1)
r = one_report(StereoRanger(), [left], [right])[0]
assert not r.matched
def test_vertical_residual_beyond_tau_rejected():
left, _ = target_pair(0.0, 0.0, 10.0)
right = box(CFG.cx_px - FB / 10.0, left.cy + 3.0) # |Δy| = 3 > τy = 2
r = one_report(StereoRanger(), [left], [right])[0]
assert not r.matched
def test_height_ratio_beyond_tau_rejected():
left, _ = target_pair(0.0, 0.0, 10.0)
_, right = target_pair(0.0, 0.0, 10.0, h=40.0) # |60-40|/60 > 0.25
r = one_report(StereoRanger(), [left], [right])[0]
assert not r.matched
def test_cost_equal_one_rejected():
# 文档 §6：仅当 cost < 1 时接受（严格小于）
# dy = 1 px -> 0.5，dh 比 = 0.125 -> 0.5，cost 精确 = 1.0
left, _ = target_pair(0.0, 0.0, 10.0)
x_off = FB / 10.0
right = Detection(left.x1 - x_off, left.y1 + 4.75,
left.x1 - x_off + 52.5, left.y1 + 4.75 + 52.5)
# 右框中心 = 左框中心 + 1 px（dy=1），高度 52.5（dh 比 0.125）
assert right.cy == pytest.approx(left.cy + 1.0)
r = one_report(StereoRanger(), [left], [right])[0]
assert not r.matched
def test_disparity_out_of_z_window_rejected():
# 目标过近（D > FB/z_min）与过远（D < FB/z_max）均不可配对（§6）
near_l, near_r = target_pair(0.0, 0.0, 0.5)
far_l, far_r = target_pair(0.0, 0.0, 60.0)
r = one_report(StereoRanger(), [near_l], [near_r])[0]
assert not r.matched
r = one_report(StereoRanger(), [far_l], [far_r])[0]
assert not r.matched
def test_greedy_one_to_one_crossing_indices():
# 目标 A(光轴上, Z=10) 与 B(右偏, Z=6)；右目框以下标交叉顺序传入。
# 贪心须按代价配对：L0->R_A、L1->R_B，即使 R_B 在下标 0 处。
l_a, r_a = target_pair(0.0, 0.0, 10.0)
l_b, r_b = target_pair(0.3, 0.0, 6.0)
reports = one_report(StereoRanger(), [l_a, l_b], [r_b, r_a])
assert all(r.matched for r in reports)
assert reports[0].forward_distance_m == pytest.approx(10.0, rel=1e-9)
assert reports[1].forward_distance_m == pytest.approx(6.0, rel=1e-9)
assert reports[0].ref_bbox == (l_a.x1, l_a.y1, l_a.x2, l_a.y2)
def test_track_id_continuity_preference():
# §6：存在轨迹标识时优先匹配相同 track id
l_a, r_a = target_pair(0.0, 0.0, 10.0, track_id=7)
l_b, r_b = target_pair(0.3, 0.0, 8.0, track_id=8)
reports = one_report(StereoRanger(), [l_a, l_b], [r_b, r_a])
by_track = {r.track_id: r.forward_distance_m for r in reports}
assert by_track[7] == pytest.approx(10.0, rel=1e-9)
assert by_track[8] == pytest.approx(8.0, rel=1e-9)
def test_unmatched_left_reports_bearing_only():
left, right = target_pair(0.0, 0.0, 10.0)
stray = box(500.0, 300.0, cls_id=3, conf=0.8)
reports = one_report(StereoRanger(), [left, stray], [right], frame_sequence=42, timestamp_s=1.5)
assert reports[0].matched and reports[0].valid
u = reports[1]
assert not u.matched and not u.valid
assert u.forward_distance_m is None
assert u.centre_pixel_x_mid == stray.cx and u.centre_pixel_y_mid == stray.cy
assert u.confidence == 0.8 and u.class_id == 3
assert u.frame_sequence == 42 and u.timestamp_s == 1.5
def test_empty_sides():
ranger = StereoRanger()
assert ranger.update([], []) == []
r = one_report(ranger, [box(100.0, 100.0)], [])[0]
assert not r.matched
def test_report_order_follows_left_detections():
ranger = StereoRanger()
l1, r1 = target_pair(0.0, 0.0, 10.0, cls_id=0)
l2, r2 = target_pair(0.0, 0.0, 8.0, cls_id=2)
reports = ranger.update([l2, l1], [r1, r2]) # 输出顺序与输入左目一致
assert [r.class_id for r in reports] == [2, 0]
assert reports[0].confidence == min(l2.conf, r2.conf)
# ---- §5.4 有效性判定 ----
def test_beyond_range_tolerance_invalid_but_distance_kept():
left, right = target_pair(0.0, 0.0, 15.0) # δZ ≈ 3.9 m > 2 m
r = one_report(StereoRanger(), [left], [right])[0]
assert r.matched
assert not r.valid
assert r.forward_distance_m == pytest.approx(15.0, rel=1e-9)
assert r.lateral_m is not None # 方位/坐标仍发布（§5.4 域外标记无效）
assert r.error_est_m == pytest.approx(range_error_abs(15.0, CFG.fx_px, CFG.baseline_m, CFG.disp_noise_px))
def test_within_range_tolerance_valid():
left, right = target_pair(0.0, 0.0, 10.0) # δZ ≈ 1.74 m <= 2 m
r = one_report(StereoRanger(), [left], [right])[0]
assert r.valid
def test_invalid_when_disparity_below_dmin():
# 放宽 range_tol 以单独暴露 D_min 判据：D = 2.3 px < 3 px -> 无效
cfg = StereoConfig(range_tol_m=50.0)
left, right = target_pair(0.0, 0.0, 25.0)
r = one_report(StereoRanger(cfg), [left], [right])[0]
assert r.matched and not r.valid
# ---- §7 时间滤波（可选） ----
def moving_pair(z_m, track_id=1, cls_id=0):
"""返回 (左框列表, 右框列表)：目标沿光轴移动，track_id 可配。"""
left = box(CFG.cx_px, CFG.cy_px, cls_id=cls_id, track_id=track_id)
right = box(CFG.cx_px - FB / z_m, CFG.cy_px, cls_id=cls_id, track_id=track_id)
return [left], [right]
def test_filter_smooths_and_resets_on_jump():
cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, filter_jump_ratio=0.2,
range_tol_m=50.0) # 放宽有效域，让 10~13 m 均有效以观察滤波
ranger = StereoRanger(cfg)
z1 = one_report(ranger, *moving_pair(10.0), frame_sequence=1)[0]
assert z1.forward_distance_m == pytest.approx(10.0, rel=1e-9) # 首帧置位
z2 = one_report(ranger, *moving_pair(11.0), frame_sequence=2)[0] # 跳变 10% <= 20%：平滑
assert z2.forward_distance_m == pytest.approx(0.7 * 10.0 + 0.3 * 11.0, rel=1e-9)
z3 = one_report(ranger, *moving_pair(13.0), frame_sequence=3)[0] # 跳变 26% > 20%：重置
assert z3.forward_distance_m == pytest.approx(13.0, rel=1e-9)
def test_filter_skipped_without_track_id():
cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3)
ranger = StereoRanger(cfg)
one_report(ranger, *moving_pair(10.0, track_id=0), frame_sequence=1)
z2 = one_report(ranger, *moving_pair(11.0, track_id=0), frame_sequence=2)[0]
assert z2.forward_distance_m == pytest.approx(11.0, rel=1e-9)
def test_reset_clears_filter_state():
cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, range_tol_m=50.0)
ranger = StereoRanger(cfg)
one_report(ranger, *moving_pair(10.0), frame_sequence=1) # 状态 = 10.0
ranger.reset()
# 若不重置，10 -> 11 会平滑为 10.3；重置后应直接输出原始 11.0
z = one_report(ranger, *moving_pair(11.0), frame_sequence=2)[0]
assert z.forward_distance_m == pytest.approx(11.0, rel=1e-9)
def test_invalid_measurements_do_not_pollute_filter_state():
cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, range_tol_m=50.0)
ranger = StereoRanger(cfg)
one_report(ranger, *moving_pair(10.0), frame_sequence=1)
z2 = one_report(ranger, *moving_pair(25.0), frame_sequence=2)[0] # D < D_min：无效
assert not z2.valid
z3 = one_report(ranger, *moving_pair(10.5), frame_sequence=3)[0]
# 无效帧未污染状态：仍按与首帧 10.0 平滑
assert z3.forward_distance_m == pytest.approx(0.7 * 10.0 + 0.3 * 10.5, rel=1e-9)
utils/tests/visual_track_control/tracking_control_law_test.py
+85 -34
View File
@@ -3,8 +3,9 @@
移植自 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp。
覆盖（物理追踪思路 §2.3/§4/§5/§6/§7/§9）：超龄降级（姿态缺失/过期、视觉
超龄、降级优先级）、判丢零速刹车、Coast 清积分与标志、速率式/位置式航向
通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道三种
无距离降级动作、首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道无距离
降级动作（含 EXIT 门限/grace 重置/零门限立即退出）、距离超龄回落、首拍变化率
限幅（S6）、机体系→NED 输出旋转（A4）。
"""
import math
@@ -26,7 +27,7 @@ def make_config() -> TrackingConfig:
cfg = TrackingConfig()
cfg.control.frequency_hz = 20.0
cfg.control.visual_stale_ms = 200
cfg.control.radar_stale_ms = 500
cfg.control.distance_stale_ms = 500
cfg.control.attitude_stale_ms = 300
cfg.camera.image_width = 1000
cfg.camera.image_height = 1000
@@ -55,7 +56,7 @@ def make_config() -> TrackingConfig:
def make_snapshot(now_ms: int) -> ControlSnapshot:
"""新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜。"""
"""新鲜快照：目标在画面中心，姿态/追踪/距离均新鲜。"""
return ControlSnapshot(
track=TrackState(tracked=True, is_predicted=False,
center=(500.0, 500.0)),
@@ -213,9 +214,9 @@ def test_vertical_velocity_clamped():
def test_distance_approach_when_far():
law = TrackingControlLaw(make_config())
snap = make_snapshot(1000)
snap.radar_distance_m = 30.0 # Δd = +20 → kp 输出 10 → 限幅 3
snap.associated_to_target = True
snap.radar_time_ms = 1000
snap.target_distance_m = 30.0 # Δd = +20 → kp 输出 10 → 限幅 3
snap.distance_valid = True
snap.distance_time_ms = 1000
out = law.update(snap, 1000)
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
@@ -223,17 +224,17 @@ def test_distance_approach_when_far():
def test_distance_retreat_clamped_when_too_close():
law = TrackingControlLaw(make_config())
snap = make_snapshot(1000)
snap.radar_distance_m = 5.0 # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
snap.associated_to_target = True
snap.radar_time_ms = 1000
snap.target_distance_m = 5.0 # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
snap.distance_valid = True
snap.distance_time_ms = 1000
out = law.update(snap, 1000)
assert out.vx_mps == pytest.approx(-1.0, abs=1e-9)
def test_no_distance_hold_zeros_vx():
law = TrackingControlLaw(make_config()) # 缺省 kHold
law = TrackingControlLaw(make_config()) # 基准配置 HOLD
snap = make_snapshot(1000)
snap.associated_to_target = False
snap.distance_valid = False
out = law.update(snap, 1000)
assert out.mode is ControlMode.VELOCITY_HEADING
assert out.vx_mps == 0.0
@@ -244,29 +245,79 @@ def test_no_distance_slow_approach():
cfg.distance.no_distance_action = NoDistanceAction.SLOW_APPROACH
law = TrackingControlLaw(cfg)
snap = make_snapshot(1000)
snap.associated_to_target = False
snap.distance_valid = False
out = law.update(snap, 1000)
assert out.vx_mps == pytest.approx(0.5, abs=1e-9)
def test_no_distance_exit_degrades():
def test_no_distance_exit_waits_grace_then_degrades():
# S1 拍板：EXIT 需持续无距离达门限才降级；grace 期间按 SLOW_APPROACH 行为
cfg = make_config()
cfg.distance.no_distance_action = NoDistanceAction.EXIT
cfg.distance.no_distance_exit_grace_ms = 200
law = TrackingControlLaw(cfg)
snap = make_snapshot(1000)
snap.associated_to_target = False
snap.distance_valid = False
out1 = law.update(snap, 1000)
assert out1.mode is ControlMode.VELOCITY_HEADING
assert out1.vx_mps == pytest.approx(0.5, abs=1e-9)
snap.track_time_ms = 1200 # 保持视觉/姿态新鲜，聚焦距离通道
snap.attitude_time_ms = 1200
out2 = law.update(snap, 1200) # 恰达门限 → 降级
assert out2.mode is ControlMode.DEGRADED_HOLD
assert out2.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
assert not out2.valid
def test_no_distance_exit_grace_resets_on_valid_frame():
cfg = make_config()
cfg.distance.no_distance_action = NoDistanceAction.EXIT
cfg.distance.no_distance_exit_grace_ms = 200
law = TrackingControlLaw(cfg)
snap = make_snapshot(1000)
snap.distance_valid = False
assert law.update(snap, 1000).mode is ControlMode.VELOCITY_HEADING
snap.track_time_ms = 1100 # 有效距离帧：重置计时
snap.attitude_time_ms = 1100
snap.distance_valid = True
snap.target_distance_m = 30.0
snap.distance_time_ms = 1100
assert law.update(snap, 1100).mode is ControlMode.VELOCITY_HEADING
snap.track_time_ms = 1250 # 再次无效：重新计时
snap.attitude_time_ms = 1250
snap.distance_valid = False
snap.target_distance_m = 0.0
assert law.update(snap, 1250).mode is ControlMode.VELOCITY_HEADING # 0 ms < 200 ms
snap.track_time_ms = 1500
snap.attitude_time_ms = 1500
out = law.update(snap, 1500) # 250 ms ≥ 200 ms → 降级
assert out.mode is ControlMode.DEGRADED_HOLD
assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
def test_no_distance_exit_zero_grace_degrades_immediately():
# grace = 0 保留旧"首个无效拍即退出"行为
cfg = make_config()
cfg.distance.no_distance_action = NoDistanceAction.EXIT
cfg.distance.no_distance_exit_grace_ms = 0
law = TrackingControlLaw(cfg)
snap = make_snapshot(1000)
snap.distance_valid = False
out = law.update(snap, 1000)
assert out.mode is ControlMode.DEGRADED_HOLD
assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
assert not out.valid
def test_stale_radar_falls_back_to_no_distance():
law = TrackingControlLaw(make_config()) # kHold
def test_stale_distance_falls_back_to_no_distance():
law = TrackingControlLaw(make_config()) # HOLD
snap = make_snapshot(1000)
snap.radar_distance_m = 30.0
snap.associated_to_target = True
snap.radar_time_ms = 400 # 600 ms 前 > 500 ms 阈值 → 距离不可用
snap.target_distance_m = 30.0
snap.distance_valid = True
snap.distance_time_ms = 400 # 600 ms 前 > 500 ms 阈值 → 距离不可用
out = law.update(snap, 1000)
assert out.mode is ControlMode.VELOCITY_HEADING
assert out.vx_mps == 0.0
@@ -280,9 +331,9 @@ def test_first_tick_vx_slews_from_zero():
cfg.accel_limit.ax_mps2 = 2.0
law = TrackingControlLaw(cfg)
snap = make_snapshot(1000)
snap.radar_distance_m = 30.0 # 通道输出本应为 3.0
snap.associated_to_target = True
snap.radar_time_ms = 1000
snap.target_distance_m = 30.0 # 通道输出本应为 3.0
snap.distance_valid = True
snap.distance_time_ms = 1000
out = law.update(snap, 1000) # 首拍：dt 按 1/20 Hz 兜底
assert out.vx_mps == pytest.approx(2.0 * (1.0 / 20.0), abs=1e-9) # 0.1
@@ -293,9 +344,9 @@ def test_output_rotated_to_ned_by_yaw():
law = TrackingControlLaw(make_config())
snap = make_snapshot(1000)
snap.yaw_rad = math.pi / 2.0 # 机头朝东
snap.radar_distance_m = 30.0
snap.associated_to_target = True
snap.radar_time_ms = 1000
snap.target_distance_m = 30.0
snap.distance_valid = True
snap.distance_time_ms = 1000
out = law.update(snap, 1000)
# 机体前向 3.0 → 东向 3.0，北向 ≈ 0
assert out.vx_mps == pytest.approx(0.0, abs=1e-9)
@@ -306,9 +357,9 @@ def test_zero_yaw_keeps_forward_as_north():
law = TrackingControlLaw(make_config())
snap = make_snapshot(1000)
snap.yaw_rad = 0.0
snap.radar_distance_m = 30.0
snap.associated_to_target = True
snap.radar_time_ms = 1000
snap.target_distance_m = 30.0
snap.distance_valid = True
snap.distance_time_ms = 1000
out = law.update(snap, 1000)
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
assert out.vy_mps == pytest.approx(0.0, abs=1e-9)
@@ -321,17 +372,17 @@ def test_reset_restores_slew_from_zero():
cfg.accel_limit.ax_mps2 = 2.0
law = TrackingControlLaw(cfg)
snap = make_snapshot(1000)
snap.radar_distance_m = 30.0
snap.associated_to_target = True
snap.radar_time_ms = 1000
snap.target_distance_m = 30.0
snap.distance_valid = True
snap.distance_time_ms = 1000
for t in range(1000, 3000, 50):
snap.track_time_ms = t
snap.radar_time_ms = t
snap.distance_time_ms = t
snap.attitude_time_ms = t
law.update(snap, t) # 若干拍后 vx 已爬升到限幅 3.0
law.reset()
snap.track_time_ms = 3000
snap.radar_time_ms = 3000
snap.distance_time_ms = 3000
snap.attitude_time_ms = 3000
out = law.update(snap, 3000)
# Reset 后限幅器归零：又从 0.1 开始渐变
utils/tests/visual_track_control/vtc_config_test.py
+21 -3
View File
@@ -15,7 +15,7 @@ from pathlib import Path
import pytest
from visual_track_control.config import (HeadingMode, NoDistanceAction,
load_from_json)
TrackingConfig, load_from_json)
_REPO_ROOT = Path(__file__).resolve().parents[3]
REAL_CONFIG = _REPO_ROOT / "reference-c++" / "visual_track_control" / "config" / "track_control.json"
@@ -27,7 +27,7 @@ def full_config() -> dict:
"control": {
"frequency_hz": 20.0,
"visual_stale_ms": 200,
"radar_stale_ms": 500,
"distance_stale_ms": 500,
"attitude_stale_ms": 300,
},
"camera": {
@@ -57,6 +57,7 @@ def full_config() -> dict:
"retreat_velocity_limit_mps": 1.0,
"no_distance_action": "hold",
"no_distance_approach_limit_mps": 0.5,
"no_distance_exit_grace_ms": 3000,
},
"accel_limit": {
"ax_mps2": 2.0,
@@ -84,7 +85,9 @@ def test_load_real_config_succeeds():
assert cfg.vertical.vz_gain_mps_per_deg == 0.1
assert cfg.vertical.vz_limit_mps == 2.0
assert cfg.distance.d_exp_m == 10.0
assert cfg.distance.no_distance_action is NoDistanceAction.HOLD
assert cfg.distance.no_distance_action is NoDistanceAction.SLOW_APPROACH
assert cfg.distance.no_distance_exit_grace_ms == 3000
assert cfg.control.distance_stale_ms == 500
assert cfg.accel_limit.ax_mps2 == 2.0
@@ -169,3 +172,18 @@ def test_fov_out_of_range_throws(tmp_path):
with pytest.raises(ValueError) as exc_info:
load_from_json(path)
assert "camera.fov_h_deg" in str(exc_info.value)
def test_default_no_distance_action_is_slow_approach():
# S1 拍板：双目有效域外无距离是常态，默认定速接近（纪要 D4/S1）
assert (TrackingConfig().distance.no_distance_action
is NoDistanceAction.SLOW_APPROACH)
def test_negative_exit_grace_throws(tmp_path):
config = full_config()
config["distance"]["no_distance_exit_grace_ms"] = -1
path = write_temp_json(tmp_path, config, "vtc_cfg_badgrace.json")
with pytest.raises(ValueError) as exc_info:
load_from_json(path)
assert "no_distance_exit_grace_ms" in str(exc_info.value)