精简仓库结构并增强任务链路：归档双目可选后端、新增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
+71
-70
@@ -1,51 +1,57 @@
|
||||
# utils —— reference-c++ 的 Python 移植 + 双目链路新实现
|
||||
# utils —— Python 实现（运行链路）
|
||||
|
||||
将 `reference-c++/` 下的 `target_tracker`（多目标追踪过滤器）与
|
||||
`visual_track_control`（视觉跟踪控制律）移植为 Python，**两包互不依赖**
|
||||
（唯一的数据转换点在胶水层 demo）。
|
||||
> **架构现状（2026-10-20）**：运行链路是「光电吊舱 → `pod_bridge` → `task_runner` → PX4」。
|
||||
> 双目 + YOLO 那套可选感知后端（`stereo_yolo` / `target_tracker` / `stereo_ranger`）
|
||||
> **已整体归档到仓库根的 `legacy/`**，默认不参与 pytest 收集；接回方式见 `legacy/README.md`。
|
||||
|
||||
另有双目链路新实现（非 C++ 移植、按规格实现）：
|
||||
- `stereo_ranger`（双目稀疏目标测距），规格 `docs/双目测距技术路线.md`；
|
||||
- `stereo_yolo`（双目 YOLO 检测前端），规格 `docs/双目YOLO目标识别需求说明.md`，
|
||||
输出直接作为 stereo_ranger 的输入（仅复用其 `types.py` 纯值类型）。
|
||||
本目录包含：
|
||||
|
||||
Python 侧定位为 **C++ 实现的先验验证**：构件划分与语义按规格冻结，便于逐构件
|
||||
移植对照；`stereo_yolo` 的 C++ 移植注记见其包 README。
|
||||
- **运行链路**：`pod_bridge`（吊舱桥接）+ `task_runner`（任务编排）+ `visual_track_control`
|
||||
（控制律原语，被前两者复用）
|
||||
- **入口共享件**：`sitl_support.py`（时钟/遥测订阅/连接准备/`RATE_*`）
|
||||
- **测试**：`tests/`（446 条）、`conftest.py`、`pytest.ini`
|
||||
|
||||
`visual_track_control` 是 `reference-c++/visual_track_control` 的逐构件 Python 移植
|
||||
（语义对齐、含行为修复）；`target_tracker` 的移植版已随归档移入 `legacy/utils/`。
|
||||
|
||||
## 目录结构
|
||||
|
||||
```
|
||||
utils/
|
||||
├── target_tracker/ # 包 1：卡尔曼滤波多目标追踪（无第三方硬依赖，仅 numpy）
|
||||
├── pod_bridge/ # 吊舱 ↔ PX4 桥接（**运行链路核心**）
|
||||
│ ├── bridge.py # PodBridge：L1/L2 切换、目标门控、链路维持
|
||||
│ ├── los_consistency.py # 目标有效性第二判据（LOS 运动学一致性）
|
||||
│ ├── ground_link.py # 地面站 UDP 坐标注入 + 命令队列
|
||||
│ ├── guidance.py # L2 地理引导律
|
||||
│ ├── protocol.py # 表2-1/2-2/3/4 帧编解码
|
||||
│ ├── pod_serial.py # 串口链路（115200 8N1）
|
||||
│ ├── px4_gateway.py # MAVSDK 遥测 + offboard 网关
|
||||
│ ├── virtual_pod.py # 虚拟吊舱（SITL/台架）
|
||||
│ └── {run_bridge,listen_pod,pod_console,sim_fc,verify_px4}.py # 台架工具
|
||||
├── task_runner/ # 任务级编排
|
||||
│ ├── state_machine.py # WAIT→ARM_TAKEOFF→GOTO→SEARCH→TRACK→RTL
|
||||
│ ├── runner.py # 编排器（快照采集/动作/异常兜底）
|
||||
│ ├── return_home.py # 自定义返航（回起飞点→恢复航向→垂直降落）
|
||||
│ ├── pod_control.py # 吊舱识别/锁定指令
|
||||
│ └── log.py # 事件 JSONL + 遥测 CSV + 简报
|
||||
├── visual_track_control/ # 控制律 + MAVSDK 网关（运行链路复用）
|
||||
│ └── README.md # 接口清单 + C++ 差异
|
||||
├── visual_track_control/ # 包 2：三通道控制律 + MAVSDK offboard 网关
|
||||
│ └── README.md # 接口清单 + C++ 差异
|
||||
├── stereo_ranger/ # 包 3：双目稀疏目标测距（纯标准库，无第三方依赖）
|
||||
│ └── README.md # 接口清单 + 与规格的偏差
|
||||
├── stereo_yolo/ # 包 4：双目 YOLO 检测前端（真模型需 ultralytics）
|
||||
│ └── README.md # 接口清单 + C++ 移植注记
|
||||
├── demo_track_control.py # 胶水层集成演示（模拟检测 → 追踪 → 控制律 → 发送）
|
||||
├── demo_stereo_ranger.py # 双目测距演示（合成框对，六场景，无硬件）
|
||||
├── demo_stereo_yolo.py # 检测前端演示（真模型 + 合成素材 → 测距联调）
|
||||
├── assets/stereo_yolo/ # demo 占位素材（AI 合成图，可整体替换为真实素材）
|
||||
├── config/track_control.json # 控制律配置（拷贝自 reference-c++）
|
||||
├── config/stereo_ranger.json # 双目测距配置（占位值，待标定回填）
|
||||
├── config/stereo_yolo.json # 检测前端配置（不存内参，几何参数唯一来源见上）
|
||||
├── tests/ # pytest 测试
|
||||
│ ├── target_tracker/ # 28 条（对应 C++ 测试文件）
|
||||
│ ├── visual_track_control/ # 81 条（对应 C++ 测试文件）
|
||||
│ ├── stereo_ranger/ # 111 条（规格 §11.4 构件级 + 全流程）
|
||||
│ └── stereo_yolo/ # 36 条（假检测器驱动，无模型/素材/硬件）
|
||||
├── conftest.py # 保证可从任意工作目录导入各包
|
||||
└── pytest.ini # 收集 *_test.py 命名（与 C++ 测试文件名对齐）；
|
||||
# importlib 导入模式避免跨包同名测试文件冲突
|
||||
├── config/ # 配置 JSON（pod_bridge.json 为主线）
|
||||
├── tests/ # pytest（446 条）
|
||||
│ ├── pod_bridge/ # 208 条（协议/桥接/引导/注入/节流/LOS）
|
||||
│ ├── task_runner/ # 122 条（状态机/编排器/返航/日志/吊舱控制）
|
||||
│ ├── visual_track_control/ # 86 条（对应 C++ gtest + L1 增量）
|
||||
│ └── ground_station/ # 30 条（页面与日志解读）
|
||||
├── conftest.py # 把 utils/ 插进 sys.path + fake_mavsdk fixture
|
||||
├── pytest.ini # 收集 *_test.py + importlib 导入模式
|
||||
└── sitl_support.py # 入口共用：时钟/遥测订阅/连接准备/速率常量
|
||||
```
|
||||
|
||||
## 环境
|
||||
|
||||
conda env `pytorch`（`D:\program file\conda\envs\pytorch`），Python 3.12，
|
||||
依赖：numpy、mavsdk、pytest；stereo_ranger 仅标准库；stereo_yolo 只有真模型
|
||||
推理路径需要 ultralytics（在检测器内部延迟导入，测试与假检测器路径不受影响）。
|
||||
依赖：numpy、mavsdk(<4)、pytest、flask、pyserial
|
||||
（`opencv-python` 仅供 `tools/` 下的相机脚本；`ultralytics` 仅供已归档的双目链路）。
|
||||
|
||||
## 运行测试
|
||||
|
||||
@@ -54,51 +60,46 @@ cd utils
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
|
||||
```
|
||||
|
||||
256 条测试全部通过：109 条 C++ 移植用例（含 §9.4 六条硬验收、A1/A2/A4
|
||||
回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）+ 111 条 stereo_ranger
|
||||
（合成框对驱动）+ 36 条 stereo_yolo（假检测器驱动）。
|
||||
stereo_ranger / stereo_yolo 测试均不需要真实图像、模型权重或硬件。
|
||||
**运行链路 446 条全部通过（2026-10-20）**：
|
||||
|
||||
## 运行演示
|
||||
| 组 | 条数 |
|
||||
|---|---|
|
||||
| `pod_bridge` | 208 |
|
||||
| `task_runner` | 122 |
|
||||
| `visual_track_control`（含 §9.4 硬验收、A1/A2/A4、C1、C4、S6、L1 角度直入） | 86 |
|
||||
| `ground_station` | 30 |
|
||||
|
||||
归档的可选后端 175 条（stereo_ranger 111 / stereo_yolo 36 / target_tracker 28）
|
||||
在 `legacy/` 下，按需运行：
|
||||
|
||||
```bash
|
||||
# mock 传输（本地记录指令，无实机）
|
||||
python utils/demo_track_control.py --speedup 10
|
||||
|
||||
# MAVSDK 实发（需飞控/SITL 在 udp://:14540）
|
||||
python utils/demo_track_control.py --transport mavsdk
|
||||
|
||||
# 双目测距（合成框对，离线，无相机/飞控）
|
||||
python utils/demo_stereo_ranger.py
|
||||
|
||||
# 双目检测前端（真模型推理；需 ultralytics + models/yolo26n-drone.pt）
|
||||
python utils/demo_stereo_yolo.py
|
||||
cd legacy/utils && PYTHONPATH="../../utils" "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
|
||||
```
|
||||
|
||||
控制链路时间线 13 s（20 Hz）：正常跟踪 6 s（vx 从零渐变 → 逼近限速 3 m/s）→
|
||||
目标消失 4 s（Coast 预测，60 帧后轨道判亡）→ 判零速刹车 3 s。
|
||||
运行链路测试不需要硬件（FakePod / VirtualPod / FakePx4 注入）。
|
||||
|
||||
双目测距演示六场景：逼近（三道有效性门 + 滤波）→ 远场域外（方位仍发布）→
|
||||
单目退化 → 非同帧拒收 → 一致性降级/判无效 → 多目标贪心配对。
|
||||
## 运行入口
|
||||
|
||||
检测前端演示五场景：单图冒烟 → 合成双目对联调（真检测框喂 stereo_ranger，
|
||||
与解析式 D=F·B/Z 对照）→ 空场景 → 左右形状不一致拒收 → 帧周期计时。
|
||||
合成对用 AI 占位素材 `assets/stereo_yolo/drone_sky.png`（可整体替换为真实
|
||||
素材），本机实测 |ΔD|≈0.3 px、|Δy|≈0.2 px；帧周期 25~34 ms（640 输入，
|
||||
双实例并发，随机器负载波动），与 30 fps 预算 33.3 ms 同量级，结论不可外推
|
||||
到机载平台（§5 的算力预算另见平台工程文档）。
|
||||
入口脚本在仓库根（不在 utils 内）：
|
||||
|
||||
```bash
|
||||
PY="D:/program file/conda/envs/pytorch/python.exe"
|
||||
"$PY" task_runner_sitl.py --conn udpin://0.0.0.0:14540 # 任务全流程 SITL
|
||||
"$PY" task_runner_real.py --port COM5 --conn udpin://0.0.0.0:14540 --target-type 1
|
||||
"$PY" l1_sitl_test.py --conn udpin://0.0.0.0:14540 # 仅 L1 控制律闭环
|
||||
"$PY" utils/pod_bridge/run_bridge.py --port COM5 --dry-run # S2 台架干跑
|
||||
```
|
||||
|
||||
## 移植约定
|
||||
|
||||
- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支
|
||||
映射保留 C++ A2 回归语义，详见包 2 README。
|
||||
- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支映射保留
|
||||
C++ A2 回归语义，详见 `visual_track_control/README.md`。
|
||||
- 测试逐条移植 C++ gtest 断言（`EXPECT_DOUBLE_EQ` → 精确 `==`，
|
||||
`EXPECT_NEAR` → `pytest.approx`）；MAVSDK 部分因无 SITL/实机，仅 mock 验证。
|
||||
`EXPECT_NEAR` → `pytest.approx`）。
|
||||
- 行为修复项（C1/C2/C4、A1/A2/A4、S6）在测试文件头注释中标注对应章节。
|
||||
- `stereo_ranger` 不是移植：按规格五步流程新实现，与规格的偏差（越界=有效性门、
|
||||
视差域权威、补充 consistency_jump_px/isolation_gate_px 等）见其包 README；
|
||||
⚠ 全部数值为占位值，标定完成前不得作为飞行控制决策依据。
|
||||
- `stereo_yolo` 不是移植：推理口径为 ultralytics 直跑 `.pt`，letterbox、解码、
|
||||
NMS、坐标回映均在库内完成；C++ 侧复现要点与接口契约硬约束（原生分辨率坐标、
|
||||
左右预处理位级一致、`x1` 即框左边缘、无跨帧状态）见其包 README 的移植注记表。
|
||||
⚠ 占位素材 `assets/stereo_yolo/` 为 AI 合成图，真实双目素材到位后需重跑联调。
|
||||
- ⚠ 全部数值为占位值，标定完成前不得作为飞行控制决策依据。
|
||||
|
||||
## 已归档内容
|
||||
|
||||
`legacy/utils/` 下保留了原 `stereo_yolo` / `stereo_ranger` / `target_tracker` 三包、
|
||||
175 条测试、三个 demo、`stereo_*.json`、素材与 YOLO 权重；说明见 `legacy/README.md`。
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 821 KiB |
@@ -39,7 +39,10 @@
|
||||
"alpha_from_frame": false,
|
||||
"alpha_from_off_target": true,
|
||||
"vertical_enabled": true,
|
||||
"require_vision": false
|
||||
"los_consistency_enabled": true,
|
||||
"los_window_ms": 3000,
|
||||
"los_min_yaw_change_deg": 20.0,
|
||||
"los_max_residual_deg": 8.0
|
||||
},
|
||||
|
||||
"tracking": {
|
||||
|
||||
@@ -1,19 +0,0 @@
|
||||
{
|
||||
"fx_px": 960.0,
|
||||
"fy_px": 960.0,
|
||||
"cx_px": 960.0,
|
||||
"cy_px": 540.0,
|
||||
"baseline_m": 0.06,
|
||||
"tau_y_px": 2.0,
|
||||
"tau_h_ratio": 0.25,
|
||||
"disparity_min_px": 3.0,
|
||||
"disparity_max_px": 57.6,
|
||||
"max_delta_z_m": 2.0,
|
||||
"consistency_N_frames": 3,
|
||||
"consistency_M_frames": 5,
|
||||
"consistency_jump_px": 3.0,
|
||||
"smooth_alpha": 0.3,
|
||||
"jump_reset_m": 5.0,
|
||||
"filter_reset_frames": 5,
|
||||
"isolation_gate_px": 50.0
|
||||
}
|
||||
@@ -1,9 +0,0 @@
|
||||
{
|
||||
"model_path": "models/yolo26n-drone.pt",
|
||||
"imgsz": 640,
|
||||
"conf_threshold": 0.25,
|
||||
"iou_threshold": 0.7,
|
||||
"class_whitelist": null,
|
||||
"device": null,
|
||||
"concurrency": "parallel"
|
||||
}
|
||||
@@ -1,269 +0,0 @@
|
||||
"""demo_stereo_ranger.py —— 双目稀疏目标测距演示（规格 docs/双目测距技术路线.md）。
|
||||
|
||||
全部场景用**解析构造**的左右目检测框驱动：不需要相机、不需要图像、不需要
|
||||
飞控，几何是闭式可验证的（规格 §11.4）。覆盖：
|
||||
|
||||
① 逼近：三条有效性门依次生效（域外 → 预算门 → 有效）+ 时间滤波平滑
|
||||
② 远场域外：valid = false 但 c_mid（虚拟双目中心方位）照常发布
|
||||
③ 单目退化：仅一目检出 → 仅方位记录
|
||||
④ 非同帧拒收：左右帧标识不一致 → 整帧拒收 + 告警节流
|
||||
⑤ 一致性监控：持续视差跳变 → N 帧降置信度 → M 帧判无效
|
||||
⑥ 多目标：贪心一一配对，各自独立状态；未匹配右目退化为仅方位
|
||||
|
||||
用法：
|
||||
python utils/demo_stereo_ranger.py # 默认配置
|
||||
python utils/demo_stereo_ranger.py --config <path> # 指定配置
|
||||
|
||||
⚠ 所有参数均为占位值（焦距/标定待厂商数据）：标定与地面真值验证完成前，
|
||||
本模块输出仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据
|
||||
（规格 §12.3 红线）。
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
from stereo_ranger import (
|
||||
Detection,
|
||||
DetectionFrame,
|
||||
RangeRecord,
|
||||
StereoConfig,
|
||||
StereoRanger,
|
||||
load_from_json,
|
||||
)
|
||||
|
||||
logger = logging.getLogger("demo_stereo_ranger")
|
||||
|
||||
_DEMO_DIR = Path(__file__).resolve().parent
|
||||
_DEFAULT_CONFIG = _DEMO_DIR / "config" / "stereo_ranger.json"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 合成框对
|
||||
|
||||
|
||||
def boxes_for(
|
||||
cfg: StereoConfig,
|
||||
x_center_m: float,
|
||||
y_m: float,
|
||||
z_m: float,
|
||||
*,
|
||||
w_m: float = 0.5,
|
||||
h_m: float = 0.4,
|
||||
class_id: int = 0,
|
||||
track_id: Optional[int] = None,
|
||||
confidence: float = 0.95,
|
||||
) -> Tuple[Detection, Detection]:
|
||||
"""模组中心系 (X_C, Y, Z) 的理想目标 → (左目框, 右目框)。
|
||||
|
||||
两目框同宽，因此边缘视差 = 中心视差 = F·B/Z（§3.2/§6.6 的精确投影）。
|
||||
"""
|
||||
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) -> 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)
|
||||
right = Detection(
|
||||
1000.0 - d_px - width / 2,
|
||||
y_center - height / 2,
|
||||
1000.0 - d_px + width / 2,
|
||||
y_center + height / 2,
|
||||
)
|
||||
return left, right
|
||||
|
||||
|
||||
def frame(frame_id: int, detections: List[Detection]) -> DetectionFrame:
|
||||
return DetectionFrame(frame_id=frame_id, detections=detections)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 打印
|
||||
|
||||
|
||||
def describe(record: RangeRecord) -> str:
|
||||
z_text = f"{record.forward_distance_m:7.3f}" if record.forward_distance_m is not None else " — "
|
||||
r_text = f"{record.slant_range_m:7.3f}" if record.slant_range_m is not None else " — "
|
||||
d_text = f"{record.disparity_px:7.2f}" if record.disparity_px is not None else " — "
|
||||
return (
|
||||
f"[F{record.frame_id:02d}] {record.degradation.name:<12} valid={int(record.valid)} "
|
||||
f"Z={z_text} m R={r_text} m D={d_text} px "
|
||||
f"c_mid=({record.center_mid_px[0]:7.1f},{record.center_mid_px[1]:6.1f}) "
|
||||
f"conf={record.confidence:.2f}"
|
||||
)
|
||||
|
||||
|
||||
def log_scene(title: str) -> None:
|
||||
logger.info("")
|
||||
logger.info("── %s", title)
|
||||
|
||||
|
||||
def log_config_summary(cfg: StereoConfig) -> None:
|
||||
logger.info("配置：B=%.3f m F=%.1f px (F·B=%.2f px·m)", cfg.baseline_m, cfg.fx_px, cfg.focal_baseline_px_m)
|
||||
logger.info(
|
||||
" 视差域 [%.1f, %.1f] px ⇔ 距离域 [%.2f, %.2f] m（视差域权威）",
|
||||
cfg.disparity_min_px,
|
||||
cfg.disparity_max_px,
|
||||
cfg.z_min_m,
|
||||
cfg.z_max_m,
|
||||
)
|
||||
logger.info(
|
||||
" 误差预算 δZ(δD=1px) ≤ %.1f m ⇒ 有效远界 %.2f m；一致性 N=%d / M=%d（跳变门限 %.1f px）",
|
||||
cfg.max_delta_z_m,
|
||||
cfg.z_error_budget_max_m,
|
||||
cfg.consistency_N_frames,
|
||||
cfg.consistency_M_frames,
|
||||
cfg.consistency_jump_px,
|
||||
)
|
||||
logger.info(
|
||||
" 滤波 α=%.2f（≈%.0f 帧群延迟）跳变重置 %.1f m；目标隔离门限 %.0f px",
|
||||
cfg.smooth_alpha,
|
||||
1.0 / cfg.smooth_alpha,
|
||||
cfg.jump_reset_m,
|
||||
cfg.isolation_gate_px,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 场景
|
||||
|
||||
|
||||
def scene_approach(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""① 逼近：Z 20 → 6 m，三条有效性门依次生效，有效帧起低通平滑。"""
|
||||
ranger.reset() # 场景独立：清空跨场景的滤波/一致性/隔离状态
|
||||
log_scene("① 逼近场景：目标沿光轴 20 → 6 m（1 m/帧），观察三道门与滤波")
|
||||
logger.info(" %-7s %-9s %-6s %-7s %s", "真值Z", "D(px)", "valid", "输出Z", "说明")
|
||||
user_ms = 1000
|
||||
for index, z_true in enumerate([20.0, 18.0, 16.0, 14.0, 12.0, 11.0, 10.0, 9.0, 8.0, 7.0, 6.0], start=1):
|
||||
left, right = boxes_for(cfg, 0.0, 0.0, z_true)
|
||||
record = ranger.update(frame(index, [left]), frame(index, [right]), now_ms=user_ms)[0]
|
||||
user_ms += 50
|
||||
if record.disparity_px is not None and record.disparity_px < cfg.disparity_min_px:
|
||||
note = "门① 视差域外 → invalid"
|
||||
elif not record.valid and record.disparity_px is not None:
|
||||
note = "门② 误差预算不满足 → invalid"
|
||||
elif record.forward_distance_m is not None and abs(record.forward_distance_m - z_true) > 1e-9:
|
||||
note = f"有效（滤波后，真值 {z_true:.2f}）"
|
||||
else:
|
||||
note = "有效（滤波器初始化）"
|
||||
logger.info(
|
||||
" %-7.2f %-9.2f %-6d %-7s %s",
|
||||
z_true,
|
||||
record.disparity_px if record.disparity_px is not None else float("nan"),
|
||||
int(record.valid),
|
||||
f"{record.forward_distance_m:.3f}" if record.forward_distance_m is not None else "—",
|
||||
note,
|
||||
)
|
||||
logger.info(
|
||||
" → 远场：域外或 δZ 超预算时诚实输出 invalid（不强行估计）；"
|
||||
"有效帧之间才做一阶低通，输出滞后于真值属预期（消费侧 PID 与限幅吸收）"
|
||||
)
|
||||
|
||||
|
||||
def scene_far_field_bearing(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""② 远场域外：无距离但方位照常发布（远场定速接近策略的输入基础）。"""
|
||||
ranger.reset()
|
||||
log_scene("② 远场域外：Z=25 m（D < 下限），valid=false 但 c_mid 仍发布")
|
||||
x_center_m = 0.6
|
||||
left, right = boxes_for(cfg, x_center_m, 0.0, 25.0)
|
||||
record = ranger.update(frame(20, [left]), frame(20, [right]), now_ms=2000)[0]
|
||||
logger.info(" %s", describe(record))
|
||||
logger.info(
|
||||
" c_mid.x=%.1f = cx + F·X_C/Z = %.1f（虚拟双目中心，无近场视差偏差）",
|
||||
record.center_mid_px[0],
|
||||
cfg.cx_px + cfg.fx_px * x_center_m / 25.0,
|
||||
)
|
||||
logger.info(" → 消费侧据此走\"方位闭环 + 定速接近\"（D4），而不是使用不可靠的远场估计")
|
||||
|
||||
|
||||
def scene_monocular_degradation(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""③ 单目退化：仅一目检出 → 仅方位记录（v1.0 接受近场视差偏差）。"""
|
||||
ranger.reset()
|
||||
log_scene("③ 单目退化：仅左目检出")
|
||||
left, _ = boxes_for(cfg, 0.3, 0.0, 4.0, confidence=0.88)
|
||||
records = ranger.update(frame(21, [left]), frame(21, []), now_ms=3000)
|
||||
logger.info(" %s", describe(records[0]))
|
||||
logger.info(" → 退化 = 仅方位：报该目框中心作方位，valid=false 无距离（§2.2 记录两态）")
|
||||
|
||||
|
||||
def scene_frame_mismatch(ranger: StereoRanger) -> None:
|
||||
"""④ 非同帧拒收：左右帧标识不一致 → 整帧拒收（WARN 节流）。"""
|
||||
ranger.reset()
|
||||
log_scene("④ 同帧契约：左右帧标识不一致（F30 vs F31）")
|
||||
left_pair = pair_with_disparity(9.6)
|
||||
for _ in range(2):
|
||||
records = ranger.update(frame(30, [left_pair[0]]), frame(31, [left_pair[1]]))
|
||||
logger.info(" 两次违约共输出记录 %d 条（应为 0）；告警按节流规范只在第 1 次打印", len(records))
|
||||
logger.info(" → 不同曝光时刻的框配对解算没有物理意义，宁可拒收不可给值（§2.1）")
|
||||
|
||||
|
||||
def scene_consistency(ranger: StereoRanger) -> None:
|
||||
"""⑤ 一致性监控：持续视差跳变 → N 帧降置信度 → M 帧判无效。"""
|
||||
ranger.reset()
|
||||
log_scene("⑤ 一致性监控：视差逐帧跳变 4 px（门限 3 px）")
|
||||
for index, d_px in enumerate([9.5, 13.5, 17.5, 21.5, 25.5, 29.5], start=40):
|
||||
left, right = pair_with_disparity(d_px)
|
||||
record = ranger.update(frame(index, [left]), frame(index, [right]), now_ms=4000)[0]
|
||||
logger.info(" %s", describe(record))
|
||||
logger.info(" → 只能降级、不能纠偏：连续异常帧数达 N 降置信度、达 M 判无效（§6.7）")
|
||||
|
||||
|
||||
def scene_multi_target(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""⑥ 多目标：贪心一一配对；未配对的右目框退化为仅方位记录。"""
|
||||
ranger.reset()
|
||||
log_scene("⑥ 多目标：两个同类别目标 + 一个孤立右目框")
|
||||
left_a, right_a = boxes_for(cfg, -1.0, 0.0, 8.0)
|
||||
left_b, right_b = boxes_for(cfg, 1.2, 0.0, 9.0)
|
||||
extra_right = Detection(360.0, 480.0, 420.0, 540.0)
|
||||
records = ranger.update(frame(50, [left_a, left_b]), frame(50, [right_a, right_b, extra_right]), now_ms=5000)
|
||||
for record in records:
|
||||
logger.info(" %s", describe(record))
|
||||
logger.info(" → 目标数极少（1~2）时 O(n²) 贪心与全局最优几乎必然同解；匹配器接口可替换（§10.3）")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 入口
|
||||
|
||||
|
||||
def run_demo(config_path: str) -> None:
|
||||
cfg = load_from_json(config_path)
|
||||
logger.info("配置来源：%s", config_path)
|
||||
log_config_summary(cfg)
|
||||
logger.warning("⚠ 占位参数未标定：仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据（§12.3）")
|
||||
|
||||
ranger = StereoRanger(cfg) # 演示共用一个实例，状态跨场景延续（逼近 → 后续场景）
|
||||
scene_approach(ranger, cfg)
|
||||
scene_far_field_bearing(ranger, cfg)
|
||||
scene_monocular_degradation(ranger, cfg)
|
||||
scene_frame_mismatch(ranger)
|
||||
scene_consistency(ranger)
|
||||
scene_multi_target(ranger, cfg)
|
||||
logger.info("")
|
||||
logger.info("演示结束：以上全部为合成框对的解析结果，无真实图像与硬件参与")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="双目稀疏目标测距演示（合成框对，离线）")
|
||||
parser.add_argument("--config", default=str(_DEFAULT_CONFIG), help="配置 JSON 路径")
|
||||
args = parser.parse_args()
|
||||
logging.basicConfig(level=logging.INFO, format="%(levelname)s %(message)s")
|
||||
run_demo(args.config)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,286 +0,0 @@
|
||||
"""demo_stereo_yolo.py —— 双目 YOLO 检测前端演示（规格 docs/双目YOLO目标识别需求说明.md）。
|
||||
|
||||
用真实模型（models/yolo26n-drone.pt）与一张占位素材（utils/assets/stereo_yolo/
|
||||
drone_sky.png，AI 合成图、非真实飞行数据）驱动，覆盖：
|
||||
|
||||
① 单图检测冒烟：真实推理 → 原生坐标框 + 出口自检（§2.2）
|
||||
② 合成双目对（已知视差）：同一无人机 patch 按已知视差贴到左右图 →
|
||||
真检测框喂 StereoRanger.update() → 与解析真值 Z=F·B/D 对比（§6 下游对接）
|
||||
③ 空场景：无目标 → 两目空帧 → 测距端返回空列表（正常空帧，不告警）
|
||||
④ 形状不一致：左右图分辨率不同 → 整帧拒收输出空（§2.2 第 2 条）
|
||||
⑤ 帧周期：双流满负荷计时（concurrency=parallel，§5 并发要求）
|
||||
|
||||
用法（从仓库根运行）：
|
||||
python utils/demo_stereo_yolo.py
|
||||
python utils/demo_stereo_yolo.py --frames 20
|
||||
|
||||
⚠ 占位参数 + 合成素材：仅限台架/回放验证，不得接入实飞控制回路（§7 红线）。
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
import time
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from stereo_ranger import (
|
||||
Detection,
|
||||
DetectionFrame,
|
||||
RangeRecord,
|
||||
StereoConfig,
|
||||
StereoRanger,
|
||||
load_from_json as load_ranger_config,
|
||||
)
|
||||
from stereo_yolo import StereoYoloConfig, StereoYoloFrontend, load_from_json
|
||||
|
||||
logger = logging.getLogger("demo_stereo_yolo")
|
||||
|
||||
_DEMO_DIR = Path(__file__).resolve().parent # utils/
|
||||
_REPO_ROOT = _DEMO_DIR.parent
|
||||
_DEFAULT_CONFIG = _DEMO_DIR / "config" / "stereo_yolo.json"
|
||||
_RANGER_CONFIG = _DEMO_DIR / "config" / "stereo_ranger.json"
|
||||
_ASSET = _DEMO_DIR / "assets" / "stereo_yolo" / "drone_sky.png"
|
||||
|
||||
# 合成双目对参数：粘贴同一 patch 的整数水平偏移 = 精确视差
|
||||
_DISPARITY_PX = 12 # ⇔ Z ≈ 4.8 m（F·B=57.6 px·m，域内）
|
||||
_LEFT_CENTER_X = 1200
|
||||
_PATCH_Y_CENTER = 560
|
||||
_CANVAS = (1920, 1080)
|
||||
_FEATHER_PX = 32 # 羽化混合宽度：硬边接缝会被检测器误当目标边缘（实测数 px 框偏移）
|
||||
_RANGER_NOW_MS = 1000
|
||||
|
||||
|
||||
def log_scene(title: str) -> None:
|
||||
logger.info("")
|
||||
logger.info("── %s", title)
|
||||
|
||||
|
||||
def describe(record: RangeRecord) -> str:
|
||||
z_text = f"{record.forward_distance_m:7.3f}" if record.forward_distance_m is not None else " — "
|
||||
d_text = f"{record.disparity_px:7.2f}" if record.disparity_px is not None else " — "
|
||||
return (
|
||||
f"[F{record.frame_id:02d}] {record.degradation.name:<12} valid={int(record.valid)} "
|
||||
f"Z={z_text} m D={d_text} px "
|
||||
f"c_mid=({record.center_mid_px[0]:7.1f},{record.center_mid_px[1]:6.1f}) "
|
||||
f"conf={record.confidence:.2f}"
|
||||
)
|
||||
|
||||
|
||||
def log_frame(eye: str, frame: DetectionFrame) -> None:
|
||||
if not frame.detections:
|
||||
logger.info(" %s：无检出", eye)
|
||||
return
|
||||
for detection in frame.detections:
|
||||
logger.info(
|
||||
" %s：框=(%7.1f,%7.1f,%7.1f,%7.1f) conf=%.3f class=%d",
|
||||
eye,
|
||||
detection.x1,
|
||||
detection.y1,
|
||||
detection.x2,
|
||||
detection.y2,
|
||||
detection.confidence,
|
||||
detection.class_id,
|
||||
)
|
||||
|
||||
|
||||
def _best_box(frame: DetectionFrame) -> Optional[Detection]:
|
||||
if not frame.detections:
|
||||
return None
|
||||
return max(frame.detections, key=lambda detection: detection.confidence)
|
||||
|
||||
|
||||
def _crop_patch(asset: np.ndarray, box: Detection, *, margin_x: float = 0.3, margin_y: float = 0.5) -> np.ndarray:
|
||||
width, height = box.width, box.height
|
||||
x1 = max(0, int(box.x1 - margin_x * width))
|
||||
x2 = min(asset.shape[1], int(box.x2 + margin_x * width))
|
||||
y1 = max(0, int(box.y1 - margin_y * height))
|
||||
y2 = min(asset.shape[0], int(box.y2 + margin_y * height))
|
||||
return asset[y1:y2, x1:x2].copy()
|
||||
|
||||
|
||||
def _sky_canvas(asset: np.ndarray) -> np.ndarray:
|
||||
"""用素材顶部天空条拉伸成画布，避免 patch 与背景出现色差硬边。"""
|
||||
strip = asset[0 : max(8, asset.shape[0] // 12), :, :]
|
||||
return cv2.resize(strip, (_CANVAS[0], _CANVAS[1]))
|
||||
|
||||
|
||||
def _paste_blended(canvas: np.ndarray, patch: np.ndarray, center_x: int, center_y: int) -> None:
|
||||
"""羽化混合粘贴：硬边接缝会被检测器当作目标边缘，造成数 px 的框偏移。"""
|
||||
height, width = patch.shape[:2]
|
||||
alpha = np.zeros((height, width), dtype=np.float32)
|
||||
alpha[_FEATHER_PX : height - _FEATHER_PX, _FEATHER_PX : width - _FEATHER_PX] = 1.0
|
||||
alpha = cv2.GaussianBlur(alpha, (0, 0), _FEATHER_PX * 0.5)[..., None]
|
||||
x0, y0 = int(center_x - width / 2), int(center_y - height / 2)
|
||||
region = canvas[y0 : y0 + height, x0 : x0 + width]
|
||||
canvas[y0 : y0 + height, x0 : x0 + width] = (patch * alpha + region * (1.0 - alpha)).astype(np.uint8)
|
||||
|
||||
|
||||
def build_synthetic_pair(frontend: StereoYoloFrontend, asset: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""素材上检出无人机 → 取 patch → 同一 patch 按已知整数视差贴到左右天空画布。"""
|
||||
frame, _ = frontend.detect(asset, asset, frame_id=0)
|
||||
box = _best_box(frame)
|
||||
if box is None:
|
||||
raise RuntimeError("占位素材未检出无人机，请更换 utils/assets/stereo_yolo/drone_sky.png")
|
||||
patch = _crop_patch(asset, box)
|
||||
left_img, right_img = _sky_canvas(asset), _sky_canvas(asset)
|
||||
_paste_blended(left_img, patch, _LEFT_CENTER_X, _PATCH_Y_CENTER)
|
||||
_paste_blended(right_img, patch, _LEFT_CENTER_X - _DISPARITY_PX, _PATCH_Y_CENTER)
|
||||
return left_img, right_img
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 场景
|
||||
|
||||
|
||||
def scene_single_image(frontend: StereoYoloFrontend, asset: np.ndarray) -> None:
|
||||
log_scene("① 单图检测冒烟：真实模型 + 占位素材（同一图喂左右目，仅看输出契约）")
|
||||
left_frame, right_frame = frontend.detect(asset, asset, frame_id=1)
|
||||
log_frame("左目", left_frame)
|
||||
log_frame("右目", right_frame)
|
||||
logger.info(
|
||||
" → 框位于输入图原生坐标系（%d×%d）；出口自检/白名单已在识别端完成（§2.2 第 6 条）",
|
||||
asset.shape[1],
|
||||
asset.shape[0],
|
||||
)
|
||||
|
||||
|
||||
def scene_synthetic_stereo(
|
||||
frontend: StereoYoloFrontend,
|
||||
ranger: StereoRanger,
|
||||
ranger_cfg: StereoConfig,
|
||||
asset: np.ndarray,
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
log_scene(f"② 合成双目对（粘贴真值视差 {_DISPARITY_PX} px）→ StereoRanger.update()")
|
||||
left_img, right_img = build_synthetic_pair(frontend, asset)
|
||||
left_frame, right_frame = frontend.detect(left_img, right_img, frame_id=2)
|
||||
log_frame("左目", left_frame)
|
||||
log_frame("右目", right_frame)
|
||||
|
||||
left_box, right_box = _best_box(left_frame), _best_box(right_frame)
|
||||
if left_box is not None and right_box is not None:
|
||||
disparity = left_box.x1 - right_box.x1
|
||||
vertical_residual = abs(left_box.y1 - right_box.y1)
|
||||
logger.info(
|
||||
" 实测边缘视差 D=x1_L−x1_R=%.2f px（真值 %d px）；|Δy|=%.2f px",
|
||||
disparity,
|
||||
_DISPARITY_PX,
|
||||
vertical_residual,
|
||||
)
|
||||
logger.info(
|
||||
" 解析对照 Z=F·B/D=%.3f m（F·B=%.2f px·m）",
|
||||
ranger_cfg.focal_baseline_px_m / disparity,
|
||||
ranger_cfg.focal_baseline_px_m,
|
||||
)
|
||||
|
||||
records = ranger.update(left_frame, right_frame, now_ms=_RANGER_NOW_MS)
|
||||
for record in records:
|
||||
logger.info(" %s", describe(record))
|
||||
if left_box is not None and right_box is not None:
|
||||
logger.info(
|
||||
" → 误差来源：检测框边缘定位抖动（本合成对 |ΔD|=%.2f px），小视差下被 Z=F·B/D 放大（§7.1）",
|
||||
abs(disparity - _DISPARITY_PX),
|
||||
)
|
||||
logger.info(" → 与真值投影法（demo_stereo_ranger）同量级；真值验证待真实双目素材回填（§6）")
|
||||
return left_img, right_img
|
||||
|
||||
|
||||
def scene_empty(frontend: StereoYoloFrontend, ranger: StereoRanger, sky: np.ndarray) -> None:
|
||||
log_scene("③ 空场景：无目标 → 两目空帧 → 测距端空列表（正常空帧，不告警）")
|
||||
left_frame, right_frame = frontend.detect(sky, sky, frame_id=3)
|
||||
records = ranger.update(left_frame, right_frame, now_ms=2000)
|
||||
logger.info(
|
||||
" 两目检出数：%d / %d；测距记录数：%d（应为 0）",
|
||||
len(left_frame.detections),
|
||||
len(right_frame.detections),
|
||||
len(records),
|
||||
)
|
||||
logger.info(" → 无检测即无输出；\"目标丢失就该没有距离\"（§2.5 第 3 条）")
|
||||
|
||||
|
||||
def scene_shape_mismatch(frontend: StereoYoloFrontend, sky: np.ndarray) -> None:
|
||||
log_scene("④ 形状不一致拒收：左 1920×1080 vs 右 1280×720")
|
||||
resized = cv2.resize(sky, (1280, 720))
|
||||
left_frame, right_frame = frontend.detect(sky, resized, frame_id=4)
|
||||
logger.info(" 左右检出数：%d / %d（应为 0 / 0，WARN 节流打印）", len(left_frame.detections), len(right_frame.detections))
|
||||
logger.info(" → 禁止左右各自缩放/填充，否则破坏校正后的极线对齐（§2.2 第 2 条）")
|
||||
|
||||
|
||||
def scene_timing(frontend: StereoYoloFrontend, left_img: np.ndarray, right_img: np.ndarray, frames: int) -> None:
|
||||
log_scene(f"⑤ 帧周期：双流满负荷 {frames} 帧（concurrency=parallel）")
|
||||
for index in range(2): # 预热
|
||||
frontend.detect(left_img, right_img, frame_id=100 + index)
|
||||
started = time.perf_counter()
|
||||
for index in range(frames):
|
||||
frontend.detect(left_img, right_img, frame_id=200 + index)
|
||||
per_frame_ms = (time.perf_counter() - started) / frames * 1000.0
|
||||
logger.info(" 平均 %.1f ms/帧（%.1f fps）；30 fps 预算 33.3 ms/帧（§5）", per_frame_ms, 1000.0 / per_frame_ms)
|
||||
logger.info(" → 本值为台架开发机实测（device 配置 None = 自动选择）；平台算力预算另见平台工程文档")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 入口
|
||||
|
||||
|
||||
def _rebase_model_path(cfg: StereoYoloConfig) -> StereoYoloConfig:
|
||||
"""demo 便利：相对 model_path 按仓库根解析（库内规则是启动工作目录）。"""
|
||||
path = Path(cfg.model_path).expanduser()
|
||||
if not path.is_absolute() and not path.is_file():
|
||||
candidate = _REPO_ROOT / path
|
||||
if candidate.is_file():
|
||||
return replace(cfg, model_path=str(candidate))
|
||||
return cfg
|
||||
|
||||
|
||||
def run_demo(config_path: str, frames: int) -> None:
|
||||
cfg = _rebase_model_path(load_from_json(config_path))
|
||||
ranger_cfg = load_ranger_config(str(_RANGER_CONFIG))
|
||||
logger.info("配置来源：%s", config_path)
|
||||
logger.info(
|
||||
"检测前端：model=%s imgsz=%d conf=%.2f iou=%.2f whitelist=%s device=%s concurrency=%s",
|
||||
Path(cfg.model_path).name,
|
||||
cfg.imgsz,
|
||||
cfg.conf_threshold,
|
||||
cfg.iou_threshold,
|
||||
cfg.class_whitelist,
|
||||
cfg.device,
|
||||
cfg.concurrency,
|
||||
)
|
||||
logger.info(
|
||||
"测距几何（唯一来源 utils/config/stereo_ranger.json）：F=%.1f px B=%.3f m F·B=%.2f px·m",
|
||||
ranger_cfg.fx_px,
|
||||
ranger_cfg.baseline_m,
|
||||
ranger_cfg.focal_baseline_px_m,
|
||||
)
|
||||
logger.warning("⚠ 占位参数与合成素材：仅限台架/回放验证，不得接入实飞控制回路（§7 红线同源）")
|
||||
|
||||
asset = cv2.imread(str(_ASSET))
|
||||
if asset is None:
|
||||
raise FileNotFoundError(f"占位素材缺失: {_ASSET}")
|
||||
|
||||
frontend = StereoYoloFrontend(cfg)
|
||||
ranger = StereoRanger(ranger_cfg)
|
||||
try:
|
||||
scene_single_image(frontend, asset)
|
||||
left_img, right_img = scene_synthetic_stereo(frontend, ranger, ranger_cfg, asset)
|
||||
scene_empty(frontend, ranger, _sky_canvas(asset))
|
||||
scene_shape_mismatch(frontend, _sky_canvas(asset))
|
||||
scene_timing(frontend, left_img, right_img, frames)
|
||||
finally:
|
||||
frontend.close()
|
||||
logger.info("")
|
||||
logger.info("演示结束：素材为 AI 生成合成图（替换 utils/assets/stereo_yolo/drone_sky.png 可换真实素材）")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="双目 YOLO 检测前端演示（真实模型 + 占位素材）")
|
||||
parser.add_argument("--config", default=str(_DEFAULT_CONFIG), help="检测前端配置 JSON 路径")
|
||||
parser.add_argument("--frames", type=int, default=10, help="帧周期场景的测量帧数")
|
||||
args = parser.parse_args()
|
||||
logging.basicConfig(level=logging.INFO, format="%(levelname)s %(message)s")
|
||||
run_demo(args.config, args.frames)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,207 +0,0 @@
|
||||
"""demo_track_control.py —— 视觉跟踪控制链路集成演示（胶水层）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/examples/track_control_demo.cpp。
|
||||
|
||||
串联三个组件（target_tracker 与 visual_track_control 互不 import，
|
||||
本脚本是唯一的转换点）：
|
||||
模拟检测序列 → TargetTracker（8 维恒速卡尔曼多目标追踪）
|
||||
→ TrackResult 转 TrackState（解耦边界）
|
||||
→ TrackingControlLaw（三通道控制律 + 限幅 + 状态机）
|
||||
→ CommandSender（mock 记录 / MAVSDK offboard 实发）
|
||||
|
||||
场景时间线（20 Hz，--speedup 可加速）：
|
||||
0~ 6s 正常跟踪：目标匀速横移 + 双目距离 30→10 m 前向逼近
|
||||
6~10s 目标消失：追踪器 Coast 预测（60 帧 = 3 s 后轨道判丢，
|
||||
控制律自动切换零速刹车）
|
||||
10~13s 持续判丢：维持零速刹车
|
||||
|
||||
用法：
|
||||
python utils/demo_track_control.py # mock 传输
|
||||
python utils/demo_track_control.py --transport mavsdk # MAVSDK（需飞控/SITL）
|
||||
python utils/demo_track_control.py --speedup 4 # 4 倍速
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import asyncio
|
||||
import logging
|
||||
from pathlib import Path
|
||||
|
||||
from target_tracker.target_tracker import Detection, TargetTracker, TrackerConfig
|
||||
from visual_track_control.config import load_from_json
|
||||
from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
|
||||
from visual_track_control.tracking_control_law import TrackingControlLaw
|
||||
from visual_track_control.types import (ControlMode, ControlSnapshot,
|
||||
ControlOutput, TrackState)
|
||||
|
||||
logger = logging.getLogger("demo")
|
||||
|
||||
_DEMO_DIR = Path(__file__).resolve().parent
|
||||
_DEFAULT_CONFIG = _DEMO_DIR / "config" / "track_control.json"
|
||||
|
||||
# 模拟场景参数
|
||||
_NORMAL_SECS = 6.0 # 正常跟踪阶段
|
||||
_COAST_SECS = 4.0 # 目标消失 → Coast（含轨道判亡）
|
||||
_LOST_SECS = 3.0 # 判丢后持续刹车阶段
|
||||
_DEMO_MAX_LOST_FRAMES = 60 # 追踪器 Coast 窗口（20 Hz 下 = 3 s）
|
||||
_STATUS_EVERY_TICKS = 10 # 每 0.5 s 打印一行状态
|
||||
|
||||
|
||||
def track_result_to_state(result) -> TrackState:
|
||||
"""两包之间的唯一转换点：TrackResult（box 为对象）→ TrackState（plain tuple）。"""
|
||||
box = result.box
|
||||
raw = result.raw
|
||||
return TrackState(
|
||||
tracked=result.tracked,
|
||||
primary_id=result.primary_id,
|
||||
center=result.center,
|
||||
box=(box.x1, box.y1, box.x2, box.y2) if box is not None else None,
|
||||
confidence=result.confidence,
|
||||
is_predicted=result.is_predicted,
|
||||
lost_frames=result.lost_frames,
|
||||
n_active=result.n_active,
|
||||
raw=(raw.x1, raw.y1, raw.x2, raw.y2) if raw is not None else None,
|
||||
)
|
||||
|
||||
|
||||
def simulate_detections(tick: int, normal_ticks: int, period_s: float):
|
||||
"""场景检测源：正常阶段目标匀速横移，之后消失。"""
|
||||
if tick >= normal_ticks:
|
||||
return []
|
||||
cx = 960.0 + 3.0 * tick # 3 px/帧 匀速右移
|
||||
cy = 540.0
|
||||
w, h = 120.0, 160.0
|
||||
return [Detection(cx - w / 2.0, cy - h / 2.0, cx + w / 2.0, cy + h / 2.0, 0.9)]
|
||||
|
||||
|
||||
def simulate_distance(tick: int, normal_ticks: int, period_s: float):
|
||||
"""场景双目距离源：正常阶段有效、距离 30→10 m；之后无效。"""
|
||||
if tick >= normal_ticks:
|
||||
return 0.0, False
|
||||
distance = max(10.0, 30.0 - tick * period_s * 3.0)
|
||||
return distance, True
|
||||
|
||||
|
||||
class MockCommandSender:
|
||||
"""CommandSender 的 mock 实现：记录最近一条指令描述（无实机验证能力）。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.last_desc = "-"
|
||||
|
||||
async def start(self) -> bool:
|
||||
logger.info("[MockTransport] offboard 启动（模拟）")
|
||||
return True
|
||||
|
||||
async def send_velocity_heading(self, output: ControlOutput,
|
||||
time_boot_ms=None) -> bool:
|
||||
self.last_desc = (f"vx={output.vx_mps:+.2f} vy={output.vy_mps:+.2f} "
|
||||
f"vz={output.vz_mps:+.2f} "
|
||||
f"yaw_rate={output.yaw_rate_dps:+.2f}°/s "
|
||||
f"yaw={output.yaw_deg:+.2f}°")
|
||||
return True
|
||||
|
||||
async def send_brake_hover(self, time_boot_ms=None) -> bool:
|
||||
self.last_desc = "零速刹车（全零机体速度设定点）"
|
||||
return True
|
||||
|
||||
async def stop(self) -> None:
|
||||
logger.info("[MockTransport] offboard 停止（模拟）")
|
||||
|
||||
|
||||
async def make_sender(args) -> object:
|
||||
if args.transport == "mock":
|
||||
return MockCommandSender()
|
||||
from mavsdk import System
|
||||
drone = System()
|
||||
logger.info("连接飞控 %s ...", args.conn)
|
||||
await drone.connect(system_address=args.conn)
|
||||
async for state in drone.core.connection_state():
|
||||
if state.is_connected:
|
||||
logger.info("飞控已连接")
|
||||
break
|
||||
return MavsdkCommandGateway(drone)
|
||||
|
||||
|
||||
async def dispatch(sender, out: ControlOutput, now_ms: int):
|
||||
"""按模式分发指令；DEGRADED_HOLD 不发送（禁止正常控制）。"""
|
||||
if out.mode is ControlMode.VELOCITY_HEADING and out.valid:
|
||||
return await sender.send_velocity_heading(out, now_ms)
|
||||
if out.mode is ControlMode.BRAKE_HOVER and out.valid:
|
||||
return await sender.send_brake_hover(now_ms)
|
||||
return None
|
||||
|
||||
|
||||
async def run_demo(args) -> None:
|
||||
cfg = load_from_json(args.config)
|
||||
period_s = 1.0 / cfg.control.frequency_hz
|
||||
period_ms = int(round(period_s * 1000.0))
|
||||
|
||||
tracker = TargetTracker(TrackerConfig(max_lost_frames=_DEMO_MAX_LOST_FRAMES))
|
||||
law = TrackingControlLaw(cfg)
|
||||
sender = await make_sender(args)
|
||||
|
||||
normal_ticks = int(round(_NORMAL_SECS / period_s))
|
||||
coast_ticks = int(round(_COAST_SECS / period_s))
|
||||
lost_ticks = int(round(_LOST_SECS / period_s))
|
||||
total_ticks = normal_ticks + coast_ticks + lost_ticks
|
||||
|
||||
if not await sender.start():
|
||||
logger.error("offboard 启动失败，退出")
|
||||
return
|
||||
|
||||
logger.info("开始 %d 拍（%.1f s，%.1f Hz）：正常 %.1fs → Coast %.1fs → 判丢 %.1fs",
|
||||
total_ticks, total_ticks * period_s, cfg.control.frequency_hz,
|
||||
_NORMAL_SECS, _COAST_SECS, _LOST_SECS)
|
||||
try:
|
||||
for tick in range(total_ticks):
|
||||
now_ms = 1000 + tick * period_ms
|
||||
detections = simulate_detections(tick, normal_ticks, period_s)
|
||||
distance_m, distance_valid = simulate_distance(tick, normal_ticks, period_s)
|
||||
|
||||
result = tracker.update(detections)
|
||||
snap = ControlSnapshot(
|
||||
track=track_result_to_state(result),
|
||||
track_time_ms=now_ms, # 每拍取样即打戳（视觉始终新鲜）
|
||||
target_distance_m=distance_m,
|
||||
distance_valid=distance_valid,
|
||||
distance_time_ms=now_ms,
|
||||
attitude_present=True,
|
||||
attitude_time_ms=now_ms, # 模拟姿态源持续可用
|
||||
yaw_rad=0.0, # 机头朝北（NED 旋转恒等）
|
||||
)
|
||||
out = law.update(snap, now_ms)
|
||||
await dispatch(sender, out, now_ms)
|
||||
|
||||
if tick % _STATUS_EVERY_TICKS == 0:
|
||||
logger.info(
|
||||
"t=%5.2fs 检测=%d 追踪[trk=%d pred=%d lost=%d] | "
|
||||
"[%s] vx=%+.2f vy=%+.2f vz=%+.2f yaw_rate=%+.2f coast=%d | 发送: %s",
|
||||
(tick * period_s), len(detections), result.tracked,
|
||||
result.is_predicted, result.lost_frames, out.mode.name,
|
||||
out.vx_mps, out.vy_mps, out.vz_mps, out.yaw_rate_dps,
|
||||
out.coast_active, sender.last_desc)
|
||||
await asyncio.sleep(period_s / args.speedup)
|
||||
finally:
|
||||
await sender.stop()
|
||||
logger.info("演示结束")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="视觉跟踪控制链路集成演示")
|
||||
parser.add_argument("--transport", choices=["mock", "mavsdk"], default="mock",
|
||||
help="指令发送方式：mock=本地记录，mavsdk=offboard 实发")
|
||||
parser.add_argument("--conn", default="udp://:14540",
|
||||
help="MAVSDK 连接串（仅 --transport mavsdk 生效）")
|
||||
parser.add_argument("--speedup", type=float, default=1.0,
|
||||
help="时间倍速（>1 加速演示）")
|
||||
parser.add_argument("--config", default=str(_DEFAULT_CONFIG),
|
||||
help="控制律配置文件路径")
|
||||
args = parser.parse_args()
|
||||
|
||||
logging.basicConfig(level=logging.INFO,
|
||||
format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
|
||||
datefmt="%H:%M:%S")
|
||||
asyncio.run(run_demo(args))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+115
-13
@@ -33,6 +33,8 @@ from visual_track_control.types import (ControlMode, ControlSnapshot,
|
||||
from .config import BridgeConfig
|
||||
from .ground_link import GroundTarget
|
||||
from .guidance import GuidanceInput, GuidanceLaw
|
||||
from .los_consistency import (LosConsistencyConfig, LosConsistencyMonitor,
|
||||
LosStatus)
|
||||
from .pod_serial import PodLink, PodSerial
|
||||
from .protocol import (CarrierAttitude, ControlCommand, TargetGeo, Telemetry,
|
||||
encode_attitude, encode_control)
|
||||
@@ -49,23 +51,39 @@ class PodBridge:
|
||||
"""px4：Px4Gateway（或等价 mock）；pod：PodLink（PodSerial 或内存 FakePod）。"""
|
||||
|
||||
def __init__(self, cfg: BridgeConfig, px4: Px4Gateway, pod: PodLink,
|
||||
now_ms_fn=_monotonic_ms, vision_target_fn=None,
|
||||
now_ms_fn=_monotonic_ms,
|
||||
ground_target_fn=None) -> None:
|
||||
self._cfg = cfg
|
||||
self._px4 = px4
|
||||
self._pod = pod
|
||||
self._now_ms = now_ms_fn
|
||||
self._vision_target_fn = vision_target_fn # §2.3 第二判据（可选，S3+ 接入我方视觉）
|
||||
self._ground_target_fn = ground_target_fn # 地面注入坐标访问器（A2，无激光替代表4）
|
||||
self._use_heading_servo = cfg.heading_servo.enabled # 运行时控制律选择（A3 状态机切换）
|
||||
self._guidance = GuidanceLaw(cfg.guidance)
|
||||
self._tracking = TrackingControlLaw(cfg.tracking) # L1 航向伺服（方案 §0）
|
||||
# 目标有效性第二判据：LOS 运动学一致性（取代已废弃的 require_vision）
|
||||
hs = cfg.heading_servo
|
||||
self._los_monitor = LosConsistencyMonitor(LosConsistencyConfig(
|
||||
enabled=hs.los_consistency_enabled,
|
||||
window_ms=hs.los_window_ms,
|
||||
min_yaw_change_deg=hs.los_min_yaw_change_deg,
|
||||
max_residual_deg=hs.los_max_residual_deg))
|
||||
self._los_consistent = True # 最近一次一致性判定（公共门控用）
|
||||
self._los_logged_ok = True # 日志：只在判定翻转时打印
|
||||
self._attitude_period_ms = max(
|
||||
1, int(round(1000.0 / cfg.link.attitude_downlink_hz)))
|
||||
self._last_attitude_ms: Optional[int] = None
|
||||
# 生效律周期：step() 被外部以更高频率调用时（任务编排器统一 20Hz，而
|
||||
# L2 引导律配置为 10Hz），按此节流，避免 dt 相关项/限幅被以双倍速率推进。
|
||||
self._law_period_ms = max(
|
||||
1, int(round(1000.0 / max(1e-6, self._active_frequency_hz()))))
|
||||
self._last_law_ms: Optional[int] = None
|
||||
self._last_law_output = None # 节流期内重发的上一拍输出
|
||||
self._latest_target = None # (TargetGeo, time_ms)
|
||||
self._latest_telemetry = None # (Telemetry, time_ms)
|
||||
self._step_failures = 0 # run() 连续异常计数（告警节流用）
|
||||
self._stopped = False # stop() 幂等标志（真机实测：RTL 与收尾会各调一次）
|
||||
self._landing_stopped = False # 是否已为降落退出 offboard（遥测仍保留）
|
||||
|
||||
# --- 生命周期 -----------------------------------------------------------
|
||||
async def start(self) -> bool:
|
||||
@@ -74,13 +92,22 @@ class PodBridge:
|
||||
if not await self._px4.start_offboard():
|
||||
logger.error("[Bridge] offboard 启动失败，桥接未运行")
|
||||
return False
|
||||
self._stopped = False
|
||||
logger.info("[Bridge] 启动：引导 %.1fHz，姿态下发 %.1fHz",
|
||||
self._cfg.guidance.frequency_hz,
|
||||
self._cfg.link.attitude_downlink_hz)
|
||||
return True
|
||||
|
||||
async def stop(self) -> None:
|
||||
"""安全停机：先零速刹车，再停 offboard / 遥测 / 串口。"""
|
||||
"""安全停机：先零速刹车，再停 offboard / 遥测 / 串口。
|
||||
|
||||
幂等：任务编排器在进入 RTL 时停一次，收尾 `_teardown` 还会再停一次
|
||||
（真机在环实测确认）。重复执行会二次关闭串口、二次停 offboard，
|
||||
在 MAVSDK/串口驱动上产生无意义告警，故第二次直接返回。
|
||||
"""
|
||||
if self._stopped:
|
||||
return
|
||||
self._stopped = True
|
||||
try:
|
||||
await self._px4.send_brake_hover(self._now_ms())
|
||||
finally:
|
||||
@@ -91,6 +118,20 @@ class PodBridge:
|
||||
self._tracking.reset()
|
||||
logger.info("[Bridge] 已停止")
|
||||
|
||||
async def landing_stop(self) -> None:
|
||||
"""降落前停机：零速刹车 + 退出 offboard，但**保留遥测与串口**。
|
||||
|
||||
与 `stop()` 的差别：交给飞控 `land()` 之后仍需遥测来判定落地、记录高度
|
||||
曲线。若此时就 `stop_telemetry()`，落地段的遥测会冻结在最后高度（SITL
|
||||
实测：日志停在 5m，看不到下降）。完整清理留给收尾 `stop()`。
|
||||
"""
|
||||
try:
|
||||
await self._px4.send_brake_hover(self._now_ms())
|
||||
finally:
|
||||
await self._px4.stop_offboard()
|
||||
self._landing_stopped = True
|
||||
logger.info("[Bridge] 已退出 offboard（保留遥测用于落地判定）")
|
||||
|
||||
# --- 主循环 -------------------------------------------------------------
|
||||
async def run(self, stop_event: asyncio.Event) -> None:
|
||||
"""按引导频率循环 step()，直到 stop_event 置位。
|
||||
@@ -133,8 +174,16 @@ class PodBridge:
|
||||
self._use_heading_servo = enabled
|
||||
if enabled:
|
||||
self._tracking.reset()
|
||||
# 进入 L1：LOS 一致性历史从零开始（避免上一段跟踪的样本污染本次判定）
|
||||
self._los_monitor.reset()
|
||||
self._los_consistent = True
|
||||
self._los_logged_ok = True
|
||||
else:
|
||||
self._guidance.reset()
|
||||
# 切律后按新律频率重算节流周期，并允许立即出第一拍
|
||||
self._law_period_ms = max(
|
||||
1, int(round(1000.0 / max(1e-6, self._active_frequency_hz()))))
|
||||
self._last_law_ms = None
|
||||
|
||||
async def step(self, now_ms: int):
|
||||
"""单控制周期（显式传入时钟，便于确定性单测）。返回 ControlOutput。
|
||||
@@ -142,19 +191,43 @@ class PodBridge:
|
||||
按运行时选择的控制律在 L1（TrackingControlLaw，吊舱角度直入）与
|
||||
L2（GuidanceLaw，表4/注入坐标地理引导）间切换（方案 §7）。
|
||||
"""
|
||||
# 链路维持先做：poll 吊舱 + 按 link.attitude_downlink_hz 下发表3 姿态。
|
||||
# 这两件事与生效律频率无关（吊舱解算依赖 表3，目标门控依赖遥测新鲜度）。
|
||||
self._ingest_pod_frames(now_ms)
|
||||
|
||||
own = self._px4.own_state()
|
||||
self._downlink_attitude(own, now_ms)
|
||||
|
||||
# 生效律节流：外部（任务编排器）统一 20Hz 调用，而 L2 引导律配置为 10Hz。
|
||||
# 未到下一拍时**重发上一拍输出**（而非刹车），否则会与本拍的 GOTO 指令打架。
|
||||
if (self._last_law_ms is not None
|
||||
and now_ms - self._last_law_ms < self._law_period_ms
|
||||
and self._last_law_output is not None):
|
||||
await self._dispatch(self._last_law_output, now_ms)
|
||||
return self._last_law_output
|
||||
|
||||
self._last_law_ms = now_ms
|
||||
if self._use_heading_servo:
|
||||
out = self._tracking.update(self._build_snapshot(own, now_ms), now_ms)
|
||||
else:
|
||||
out = self._guidance.update(
|
||||
self._build_guidance_input(own, now_ms), now_ms)
|
||||
await self._dispatch(out, now_ms)
|
||||
self._last_law_output = out
|
||||
return out
|
||||
|
||||
async def search_step(self, now_ms: int, yaw_rate_dps: float) -> None:
|
||||
"""搜索周期（SEARCH 态）：只维持链路与心跳 + 原地偏航扫视，不出控制律。
|
||||
|
||||
之所以不让 SEARCH 走 step()：该态既不用 L2 地理引导（已在参考点，再发
|
||||
速度会漂离），也不用 L1（还没锁定目标，控制律会判丢输出刹车）。此处
|
||||
**仍要 poll 吊舱并下发表3 姿态**——否则搜索期间吊舱遥测超龄、锁定位永远
|
||||
不新鲜，搜到目标也切不进 TRACK。
|
||||
"""
|
||||
self._ingest_pod_frames(now_ms)
|
||||
own = self._px4.own_state()
|
||||
self._downlink_attitude(own, now_ms)
|
||||
await self._px4.send_yaw_rate(yaw_rate_dps)
|
||||
|
||||
# --- 吊舱侧 -------------------------------------------------------------
|
||||
def _ingest_pod_frames(self, now_ms: int) -> None:
|
||||
for frame in self._pod.poll():
|
||||
@@ -239,16 +312,24 @@ class PodBridge:
|
||||
return target
|
||||
|
||||
def pod_locked(self, now_ms: int) -> bool:
|
||||
"""吊舱目标门控（公共接口，供编排层/状态机复用，A3）。"""
|
||||
return self._pod_locked(now_ms)
|
||||
"""目标门控（公共接口，供编排层/状态机复用，A3）。
|
||||
|
||||
= 吊舱 bit2 门控 **AND** LOS 运动学一致性判定。
|
||||
这样"bit2 粘滞但角度冻结"的幽灵目标会让状态机退出 TRACK 重新搜索，
|
||||
而不只是让控制律单方面刹车（否则状态机会一直以为还锁着）。
|
||||
"""
|
||||
return self._pod_locked(now_ms) and self._los_consistent
|
||||
|
||||
def _pod_locked(self, now_ms: int) -> bool:
|
||||
"""吊舱目标门控：表2-2 遥测在 link.telemetry_timeout_ms 内、状态字节1
|
||||
bit2=锁定、且（若 link.require_laser_valid）激光测距有效，才算目标有效。
|
||||
"""吊舱 bit2 门控（**不含** LOS 一致性，供内部合成本拍快照用）。
|
||||
|
||||
表2-2 遥测在 link.telemetry_timeout_ms 内、状态字节1 bit2=锁定、
|
||||
且（若 link.require_laser_valid）激光测距有效，才算"吊舱自报锁定"。
|
||||
|
||||
实测依据：部分吊舱目标丢失后**锁定位置位不复位**（脱靶量也没有无效标志位，
|
||||
恒为 0），仅凭 bit2 会朝过期坐标继续飞；激光无效时吊舱不解算目标经纬度，
|
||||
故此兜底。无激光或激光恒无效的吊舱把 require_laser_valid 置 False。
|
||||
更进一步的"角度是否活体"由 LOS 一致性判据在 _build_snapshot 里叠加。
|
||||
|
||||
从未收到遥测 / 遥测超龄 / 搜索状态 / 激光无效 一律视为目标无效（宁可刹车）。"""
|
||||
tel = self._latest_telemetry
|
||||
@@ -293,17 +374,38 @@ class PodBridge:
|
||||
alpha_deg = 0.0
|
||||
|
||||
tracked = self._pod_locked(now_ms)
|
||||
# §2.3：require_vision 时需我方视觉作第二判据（bit2 粘滞兜底）。未接入视觉源
|
||||
# 时 fail-safe 为 False（无法确认 → 宁可刹车）。
|
||||
if tracked and hs.require_vision:
|
||||
tracked = self._vision_target_fn() if self._vision_target_fn else False
|
||||
# 第二判据：LOS 运动学一致性（独立于 bit2）。
|
||||
# 本吊舱无电机、相机刚性固连机体 → 机头转 Δψ 时真实目标的脱靶量必变 ≈−Δψ；
|
||||
# 机头已明显转动而 β 不动 → 角度冻结/幽灵目标 → 当丢目标处理（fail-safe）。
|
||||
# 仅在"吊舱自报锁定"期间喂样本；解锁即清历史，避免跨段污染。
|
||||
if tracked:
|
||||
status = self._los_monitor.update(
|
||||
now_ms, math.degrees(own.yaw_rad), beta_deg)
|
||||
self._los_consistent = status.consistent
|
||||
if status.frozen:
|
||||
tracked = False
|
||||
if self._los_logged_ok:
|
||||
self._los_logged_ok = False
|
||||
logger.warning(
|
||||
"[Bridge] 吊舱角度与机头转动不一致（Δψ=%.1f° Δβ=%.1f° "
|
||||
"残差=%.1f°，超 %.1f°）→ 判为丢目标",
|
||||
status.delta_yaw_deg, status.delta_beta_deg,
|
||||
status.residual_deg, hs.los_max_residual_deg)
|
||||
elif self._los_consistent and not self._los_logged_ok:
|
||||
self._los_logged_ok = True
|
||||
logger.info("[Bridge] 吊舱角度一致性恢复（残差 %.1f°）",
|
||||
status.residual_deg)
|
||||
else:
|
||||
self._los_monitor.reset()
|
||||
self._los_consistent = True
|
||||
self._los_logged_ok = True
|
||||
|
||||
return ControlSnapshot(
|
||||
track=TrackState(tracked=tracked),
|
||||
track_time_ms=t_ms, # 无遥测(t_ms=0) → 控制律判视觉超龄 → 降级
|
||||
los_beta_deg=beta_deg,
|
||||
los_alpha_deg=alpha_deg,
|
||||
distance_valid=False, # L1 无距离（L2 换 stereo_ranger.forward_distance_m）
|
||||
distance_valid=False, # L1 无距离（距离源接入后改为可用，见 prompt 文档）
|
||||
attitude_present=own.has_attitude,
|
||||
attitude_time_ms=own.time_ms,
|
||||
yaw_rad=own.yaw_rad,
|
||||
|
||||
@@ -67,7 +67,14 @@ class HeadingServoConfig:
|
||||
alpha_from_frame: bool = False # α 是否叠加框架角 frame_pitch_deg
|
||||
alpha_from_off_target: bool = True # α 是否叠加脱靶量 off_target_y_deg
|
||||
vertical_enabled: bool = True # 无人机 L1 开垂直（决策3）；false → α 强制 0
|
||||
require_vision: bool = False # true = 需我方视觉作第二判据（S3 起，§2.3）
|
||||
# --- 目标有效性第二判据：LOS 运动学一致性（2026-10-19 取代 require_vision）---
|
||||
# 背景：吊舱已取代双目+YOLO 成为唯一目标源，"我方视觉"不再存在；而 bit2
|
||||
# 丢失后不复位、脱靶量丢失表现为 0。故改用"机头转动 vs 脱靶量变化"的一致性
|
||||
# 作为独立于 bit2 的活体判据（详见 los_consistency.py）。
|
||||
los_consistency_enabled: bool = True
|
||||
los_window_ms: int = 3000 # 比较窗口（毫秒）
|
||||
los_min_yaw_change_deg: float = 20.0 # 窗口内机头至少转这么多才判定（激励门槛）
|
||||
los_max_residual_deg: float = 8.0 # |Δβ+Δψ| 上限，超过判不一致
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -283,8 +290,20 @@ def _load_heading_servo(root: dict) -> HeadingServoConfig:
|
||||
s, "heading_servo", "alpha_from_off_target", c.alpha_from_off_target)
|
||||
c.vertical_enabled = _get_bool(
|
||||
s, "heading_servo", "vertical_enabled", c.vertical_enabled)
|
||||
c.require_vision = _get_bool(
|
||||
s, "heading_servo", "require_vision", c.require_vision)
|
||||
c.los_consistency_enabled = _get_bool(
|
||||
s, "heading_servo", "los_consistency_enabled",
|
||||
c.los_consistency_enabled)
|
||||
c.los_window_ms = int(_positive(
|
||||
_get_integer(s, "heading_servo", "los_window_ms", c.los_window_ms),
|
||||
"heading_servo.los_window_ms"))
|
||||
c.los_min_yaw_change_deg = _in_range(
|
||||
_get_number(s, "heading_servo", "los_min_yaw_change_deg",
|
||||
c.los_min_yaw_change_deg),
|
||||
0.0, 180.0, "heading_servo.los_min_yaw_change_deg")
|
||||
c.los_max_residual_deg = _in_range(
|
||||
_get_number(s, "heading_servo", "los_max_residual_deg",
|
||||
c.los_max_residual_deg),
|
||||
0.0, 180.0, "heading_servo.los_max_residual_deg")
|
||||
return c
|
||||
|
||||
|
||||
|
||||
@@ -1,20 +1,25 @@
|
||||
"""ground_link.py —— 地面坐标注入通道（预定流程.md「地面给坐标」）。
|
||||
"""ground_link.py —— 地面站链路（坐标注入 + 控制命令，预定流程.md「地面给坐标」）。
|
||||
|
||||
本吊舱无激光 → 表4 目标经纬度永不出（部署文档 §10.0），目标坐标改由地面站注入。
|
||||
本模块负责接收地面站下发的目标坐标（UDP JSON），维护一份带本地打戳的注入目标
|
||||
缓存，供 PodBridge 的 L2 地理引导**优先消费**（回退表4）。
|
||||
本模块负责接收地面站下发的两类 UDP JSON 消息：
|
||||
* 目标坐标 {"lat":..,"lon":..,"alt":..} → 维护一份带本地打戳的注入目标缓存，
|
||||
供 PodBridge 的 L2 地理引导**优先消费**（回退表4）；
|
||||
* 控制命令 {"cmd":"<name>"}（rtl / emergency / pod_*）→ 入命令队列，
|
||||
由 TaskRunner 主循环 drain_commands() 取走并分派。
|
||||
|
||||
设计要点：
|
||||
* UDP 无连接监听，每包 JSON {"lat":..,"lon":..,"alt":..}（可选 "ts" 仅诊断，不用）。
|
||||
* UDP 无连接监听；坐标包可选 "ts" 仅诊断，不用。
|
||||
* 新鲜度用「本地接收时刻」打戳（now_ms_fn 单调时钟），**不信任地面站时钟**
|
||||
（两端时钟不同步）；超龄由下游按 GroundInjectConfig.timeout_ms 判无效。
|
||||
* parse_payload 为纯函数（可单测）；socket / 事件循环拆到 run()，且 socket
|
||||
延迟导入，模块 import 无副作用（bridge 仅复用 GroundTarget 类型）。
|
||||
* 命令是事件（非状态）：队列缓存、满则丢最旧；未知命令名由上层忽略。
|
||||
* parse_payload / parse_message 为纯函数（可单测）；socket / 事件循环拆到
|
||||
run()，且 socket 延迟导入，模块 import 无副作用（bridge 仅复用 GroundTarget 类型）。
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import json
|
||||
import logging
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
from typing import Callable, Optional
|
||||
|
||||
@@ -42,6 +47,18 @@ class GroundTarget:
|
||||
time_ms: int = 0 # 本地接收时刻（单调毫秒），0 = 从未注入
|
||||
|
||||
|
||||
@dataclass
|
||||
class GroundCommand:
|
||||
"""地面站控制命令（事件语义）。
|
||||
|
||||
name 为小写命令名（rtl / emergency / pod_detect_on / …）；time_ms 为本地
|
||||
接收时刻（单调毫秒，仅日志/诊断用）。合法性（白名单）由上层分派时判。
|
||||
"""
|
||||
|
||||
name: str = ""
|
||||
time_ms: int = 0
|
||||
|
||||
|
||||
@dataclass
|
||||
class GroundInjectConfig:
|
||||
"""地面坐标注入配置（全外置；数值占位，待落地时回填）。"""
|
||||
@@ -53,12 +70,13 @@ class GroundInjectConfig:
|
||||
|
||||
|
||||
class GroundLink:
|
||||
"""地面坐标注入接收器。
|
||||
"""地面站链路接收器（坐标注入 + 控制命令队列）。
|
||||
|
||||
用法（在 runner 外部以 asyncio task 运行）：
|
||||
link = GroundLink(cfg, now_ms_fn)
|
||||
task = asyncio.create_task(link.run(stop_event))
|
||||
PodBridge 通过 `ground_target_fn=lambda: link.latest()` 读取注入坐标。
|
||||
PodBridge 通过 `ground_target_fn=lambda: link.latest()` 读取注入坐标；
|
||||
TaskRunner 主循环通过 `drain_commands()` 取控制命令。
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: GroundInjectConfig,
|
||||
@@ -66,6 +84,7 @@ class GroundLink:
|
||||
self._cfg = cfg
|
||||
self._now_ms = now_ms_fn
|
||||
self._target = GroundTarget()
|
||||
self._commands: deque[str] = deque(maxlen=16)
|
||||
|
||||
def latest(self) -> GroundTarget:
|
||||
"""返回当前注入目标（可能 time_ms=0 表示从未注入）。"""
|
||||
@@ -76,6 +95,12 @@ class GroundLink:
|
||||
target.time_ms = self._now_ms()
|
||||
self._target = target
|
||||
|
||||
def drain_commands(self) -> list[str]:
|
||||
"""取走全部待处理命令（FIFO，取后清空）。命令为事件，无新鲜度概念。"""
|
||||
cmds = list(self._commands)
|
||||
self._commands.clear()
|
||||
return cmds
|
||||
|
||||
def is_valid(self, now_ms: int) -> bool:
|
||||
"""注入坐标是否新鲜有效（未注入 / 超龄 → False）。"""
|
||||
t = self._target
|
||||
@@ -84,15 +109,19 @@ class GroundLink:
|
||||
return not is_stale(t.time_ms, now_ms, self._cfg.timeout_ms)
|
||||
|
||||
@staticmethod
|
||||
def parse_payload(payload: bytes) -> Optional[GroundTarget]:
|
||||
"""解析地面站 JSON 包（纯函数，可单测）。返回不含 time_ms 的目标；
|
||||
非法 JSON / 非对象 / 缺 lat 或 lon → None（静默丢弃）。"""
|
||||
def _load_object(payload: bytes) -> Optional[dict]:
|
||||
"""解 JSON 且要求顶层为对象（纯函数）。非法 JSON / 非对象 → None。"""
|
||||
try:
|
||||
obj = json.loads(payload.decode("utf-8"))
|
||||
except (UnicodeDecodeError, json.JSONDecodeError):
|
||||
return None
|
||||
if not isinstance(obj, dict):
|
||||
return None
|
||||
return obj
|
||||
|
||||
@staticmethod
|
||||
def _target_from_object(obj: dict) -> Optional[GroundTarget]:
|
||||
"""从 dict 提取坐标（缺 lat/lon 或类型错 → None）。"""
|
||||
lat = obj.get("lat")
|
||||
lon = obj.get("lon")
|
||||
if not _is_number(lat) or not _is_number(lon):
|
||||
@@ -103,6 +132,27 @@ class GroundLink:
|
||||
return GroundTarget(latitude_deg=float(lat), longitude_deg=float(lon),
|
||||
abs_alt_m=float(alt))
|
||||
|
||||
@staticmethod
|
||||
def parse_payload(payload: bytes) -> Optional[GroundTarget]:
|
||||
"""解析坐标包（纯函数，可单测）。返回不含 time_ms 的目标；
|
||||
非法 JSON / 非对象 / 缺 lat 或 lon → None（静默丢弃）。"""
|
||||
obj = GroundLink._load_object(payload)
|
||||
if obj is None:
|
||||
return None
|
||||
return GroundLink._target_from_object(obj)
|
||||
|
||||
@staticmethod
|
||||
def parse_message(payload: bytes) -> Optional[GroundTarget | GroundCommand]:
|
||||
"""解析任意地面站包（纯函数）：{"cmd":".."} → GroundCommand（优先），
|
||||
否则按坐标包解析；非法包 → None（静默丢弃）。"""
|
||||
obj = GroundLink._load_object(payload)
|
||||
if obj is None:
|
||||
return None
|
||||
cmd = obj.get("cmd")
|
||||
if isinstance(cmd, str) and cmd.strip():
|
||||
return GroundCommand(name=cmd.strip().lower())
|
||||
return GroundLink._target_from_object(obj)
|
||||
|
||||
async def run(self, stop_event: asyncio.Event) -> None:
|
||||
"""UDP 接收循环：每收一包合法 JSON 更新注入目标并本地打戳。"""
|
||||
import socket # 延迟导入：仅真正跑 socket 时才需要
|
||||
@@ -123,13 +173,17 @@ class GroundLink:
|
||||
logger.warning("[GroundLink] 接收异常: %s", exc)
|
||||
await asyncio.sleep(0.05)
|
||||
continue
|
||||
target = self.parse_payload(data)
|
||||
if target is None:
|
||||
msg = self.parse_message(data)
|
||||
if msg is None:
|
||||
continue # 非法包静默丢弃（热路径不打日志）
|
||||
target.time_ms = self._now_ms()
|
||||
self._target = target
|
||||
msg.time_ms = self._now_ms()
|
||||
if isinstance(msg, GroundCommand):
|
||||
self._commands.append(msg.name)
|
||||
logger.debug("[GroundLink] 收到命令: %s", msg.name)
|
||||
continue
|
||||
self._target = msg
|
||||
logger.debug("[GroundLink] 更新注入目标 (%f, %f, %fm)",
|
||||
target.latitude_deg, target.longitude_deg,
|
||||
target.abs_alt_m)
|
||||
msg.latitude_deg, msg.longitude_deg,
|
||||
msg.abs_alt_m)
|
||||
finally:
|
||||
sock.close()
|
||||
|
||||
@@ -0,0 +1,115 @@
|
||||
"""los_consistency.py —— 吊舱角度「运动学一致性」校验（替代旧的 require_vision）。
|
||||
|
||||
为什么需要它
|
||||
------------
|
||||
吊舱协议有两处已知缺陷（部署文档 §10）：
|
||||
* bit2 锁定位在目标丢失后**不复位**；
|
||||
* 脱靶量没有无效标志位，**丢失时表现为 0**。
|
||||
只信 bit2 会让飞机朝"过期坐标/幽灵目标"继续飞。
|
||||
|
||||
原设计用「我方双目视觉」作第二判据（`require_vision`）。但后续决定**吊舱取代
|
||||
双目 + YOLO 成为唯一目标源**，于是"我方视觉"不再存在：该判据要么恒 False
|
||||
（链路永远进不了 TRACK），要么指向吊舱自身（自证，零信息量）。
|
||||
|
||||
本模块给出**不依赖任何额外传感器**的替代判据：
|
||||
本吊舱无电机、框架刚性固连机体 → 相机光轴随刚体转动。
|
||||
因此目标在世界系静止时，机头转 Δψ，脱靶量必须变化约 −Δψ。
|
||||
若机头已经明显转动（|Δψ| ≥ 阈值）而脱靶量几乎不动 → 角度冻结/幽灵，
|
||||
判为不一致（fail-safe：当丢目标处理）。
|
||||
|
||||
判别量：`residual = Δβ + Δψ`（β 为右正视线角，ψ 为 NED 航向，均取度）。
|
||||
* 一致：|residual| ≤ max_residual_deg（目标在世界系缓慢移动也会落在余量内）
|
||||
* 不一致：|residual| 超限且 |Δψ| 足够大（否则是"没有激励"，不做判断）
|
||||
|
||||
局限（必须在文档里写清）：它只能证明「角度是活的」，**不能证明「是那个目标」**。
|
||||
若吊舱锁错了物体但角度正常变化，本判据无法识别 —— 那属于识别问题，需另接
|
||||
独立传感器（双目/其他），不是本模块能覆盖的。
|
||||
"""
|
||||
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
|
||||
|
||||
def wrap_deg_180(deg: float) -> float:
|
||||
"""角度回绕到 [-180, 180)。"""
|
||||
d = math.fmod(deg + 180.0, 360.0)
|
||||
if d < 0.0:
|
||||
d += 360.0
|
||||
return d - 180.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class LosConsistencyConfig:
|
||||
"""一致性判据参数（全外置）。"""
|
||||
|
||||
enabled: bool = True # False → 关掉判据（退回"只信 bit2"）
|
||||
window_ms: int = 3000 # 比较窗口：用"当前"与"约 window 前"的样本比对
|
||||
min_yaw_change_deg: float = 20.0 # 窗口内机头至少转这么多才做判断（激励门槛）
|
||||
max_residual_deg: float = 8.0 # |Δβ+Δψ| 超过即判不一致
|
||||
history_factor: float = 2.0 # 历史缓冲 = window × factor
|
||||
|
||||
|
||||
@dataclass
|
||||
class LosStatus:
|
||||
"""单次判定结果（供门控与日志）。"""
|
||||
|
||||
consistent: bool = True # 是否一致（默认一致 = 不干扰正常跟踪）
|
||||
excited: bool = False # 本窗口是否达到"激励门槛"（转过足够角度）
|
||||
residual_deg: float = 0.0 # Δβ + Δψ
|
||||
delta_yaw_deg: float = 0.0
|
||||
delta_beta_deg: float = 0.0
|
||||
|
||||
@property
|
||||
def frozen(self) -> bool:
|
||||
"""判为"角度冻结/幽灵"：有激励但不一致。"""
|
||||
return self.excited and not self.consistent
|
||||
|
||||
|
||||
class LosConsistencyMonitor:
|
||||
"""按窗口比较 Δβ 与 Δψ，判断吊舱角度是否与机体转动一致。
|
||||
|
||||
用法：每个控制周期喂一次（仅在吊舱报告锁定期间喂），
|
||||
`update(now_ms, yaw_deg, beta_deg)` 返回 `LosStatus`。
|
||||
吊舱解锁时应调用 `reset()`，避免上一次跟踪的历史污染下一次。
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: LosConsistencyConfig | None = None) -> None:
|
||||
self._cfg = cfg or LosConsistencyConfig()
|
||||
self._samples: deque = deque()
|
||||
|
||||
def reset(self) -> None:
|
||||
self._samples.clear()
|
||||
|
||||
def update(self, now_ms: int, yaw_deg: float, beta_deg: float) -> LosStatus:
|
||||
cfg = self._cfg
|
||||
if not cfg.enabled:
|
||||
return LosStatus(consistent=True, excited=False)
|
||||
|
||||
self._samples.append((now_ms, float(yaw_deg), float(beta_deg)))
|
||||
keep_ms = cfg.window_ms * max(1.0, cfg.history_factor)
|
||||
while self._samples and now_ms - self._samples[0][0] > keep_ms:
|
||||
self._samples.popleft()
|
||||
|
||||
# 参考样本：窗口内**最老**的那个
|
||||
ref = None
|
||||
for sample in self._samples:
|
||||
if now_ms - sample[0] <= cfg.window_ms:
|
||||
ref = sample
|
||||
break
|
||||
if ref is None:
|
||||
return LosStatus(consistent=True, excited=False)
|
||||
|
||||
span_ms = now_ms - ref[0]
|
||||
# 历史不足一个窗口的 3/4 时，Δψ 天然偏小，不做判断（避免误判丢目标）
|
||||
if span_ms < cfg.window_ms * 0.75:
|
||||
return LosStatus(consistent=True, excited=False)
|
||||
|
||||
delta_yaw = wrap_deg_180(yaw_deg - ref[1])
|
||||
delta_beta = beta_deg - ref[2]
|
||||
residual = delta_beta + delta_yaw
|
||||
excited = abs(delta_yaw) >= cfg.min_yaw_change_deg
|
||||
consistent = (not excited) or (abs(residual) <= cfg.max_residual_deg)
|
||||
return LosStatus(consistent=consistent, excited=excited,
|
||||
residual_deg=residual, delta_yaw_deg=delta_yaw,
|
||||
delta_beta_deg=delta_beta)
|
||||
@@ -3,6 +3,8 @@
|
||||
职责（桥接的飞控侧）：
|
||||
* 订阅 PX4 遥测（position / attitude_euler / gps_info / velocity_ned），缓存为
|
||||
OwnState 快照，供桥接层构造 表3(载机姿态下发) 与 GuidanceInput(引导输入)；
|
||||
* 订阅飞控状态文本 status_text()（STATUSTEXT），缓存供任务日志/地面站显示
|
||||
（飞控警告/异常信息）；
|
||||
* 复用 visual_track_control.MavsdkCommandGateway 发送 offboard 速度设定点
|
||||
（不重造三分支映射，直接喂 GuidanceLaw 的 ControlOutput）。
|
||||
|
||||
@@ -15,6 +17,7 @@ import asyncio
|
||||
import logging
|
||||
import math
|
||||
import time
|
||||
from collections import deque
|
||||
from dataclasses import dataclass, replace
|
||||
from typing import Any, Callable, Optional
|
||||
|
||||
@@ -23,6 +26,7 @@ from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
_GPS_FIX_3D = 3 # mavsdk GpsFixType.THREE_D_FIX 起视为定位有效
|
||||
_FC_MSG_BUFFER = 64 # 飞控状态文本缓存上限（低频流，仅供日志消费）
|
||||
|
||||
|
||||
def _enum_int(value: Any) -> int:
|
||||
@@ -69,6 +73,7 @@ class Px4Gateway:
|
||||
self._now_ms = now_ms_fn or (lambda: int(time.monotonic() * 1000))
|
||||
self._state = OwnState()
|
||||
self._tasks: list = []
|
||||
self._fc_msgs: deque = deque(maxlen=_FC_MSG_BUFFER)
|
||||
|
||||
# --- 连接（实机）-------------------------------------------------------
|
||||
@classmethod
|
||||
@@ -123,13 +128,25 @@ class Px4Gateway:
|
||||
"""返回当前缓存的快照副本（调用方修改不影响内部状态）。"""
|
||||
return replace(self._state)
|
||||
|
||||
def update_fc_status(self, st: Any) -> None:
|
||||
"""缓存一条飞控状态文本（STATUSTEXT）；severity 取枚举名（如 WARNING）。"""
|
||||
sev = getattr(st, "type", None)
|
||||
sev_name = getattr(sev, "name", None) or str(sev or "UNKNOWN")
|
||||
self._fc_msgs.append((sev_name, str(getattr(st, "text", ""))))
|
||||
|
||||
def take_fc_messages(self) -> list:
|
||||
"""取走并清空飞控状态文本（供任务日志落盘；无则返回 []）。"""
|
||||
msgs = list(self._fc_msgs)
|
||||
self._fc_msgs.clear()
|
||||
return msgs
|
||||
|
||||
def _recompute_valid(self) -> None:
|
||||
self._state.position_valid = (self._state.has_position
|
||||
and self._state.fix_type >= _GPS_FIX_3D)
|
||||
|
||||
# --- 遥测订阅（异步胶水）----------------------------------------------
|
||||
async def start_telemetry(self) -> None:
|
||||
"""订阅四路遥测流，后台任务持续刷新缓存。幂等：已订阅则直接返回。"""
|
||||
"""订阅遥测流 + 飞控状态文本，后台任务持续刷新缓存。幂等：已订阅则直接返回。"""
|
||||
if self._tasks:
|
||||
return # 已订阅，避免重复起任务（runner 会先订阅，bridge.start 再触发时跳过）
|
||||
t = self._drone.telemetry
|
||||
@@ -140,6 +157,8 @@ class Px4Gateway:
|
||||
asyncio.ensure_future(self._consume(t.gps_info(), self.update_gps)),
|
||||
asyncio.ensure_future(
|
||||
self._consume(t.velocity_ned(), self.update_velocity_ned)),
|
||||
asyncio.ensure_future(
|
||||
self._consume(t.status_text(), self.update_fc_status)),
|
||||
]
|
||||
logger.info("[Px4Gateway] 遥测订阅已启动")
|
||||
|
||||
@@ -171,5 +190,9 @@ class Px4Gateway:
|
||||
async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool:
|
||||
return await self._cmd.send_brake_hover(time_boot_ms)
|
||||
|
||||
async def send_yaw_rate(self, yaw_rate_dps: float) -> bool:
|
||||
"""原地偏航扫视（SEARCH 态）：零速 + 指定角速度，保持 offboard 心跳。"""
|
||||
return await self._cmd.send_yaw_rate(yaw_rate_dps)
|
||||
|
||||
async def stop_offboard(self) -> None:
|
||||
await self._cmd.stop()
|
||||
|
||||
@@ -0,0 +1,131 @@
|
||||
"""sitl_support.py —— SITL / 真机入口共用的 MAVSDK 小工具。
|
||||
|
||||
从根目录 `pod_sitl_test.py` 拆出：原先两个现行入口
|
||||
（`task_runner_sitl.py` / `task_runner_real.py`）都从这个"测试脚本"里 import
|
||||
`now_ms` / `wait_until_ready` 等，语义上很别扭。现归位为独立支持模块，
|
||||
入口只依赖它，`l1_sitl_test.py` 则作为纯粹的 L1 集成测试脚本。
|
||||
|
||||
内容（刻意保持无业务逻辑）：
|
||||
* `RATE_*`：遥测流速率**唯一来源**（各订阅方共用；PX4 速率由最后一次设置决定）
|
||||
* `now_ms`：单调毫秒时钟（与 PodBridge / TaskRunner 同源）
|
||||
* `TelemetryHub`：后台订阅 NED 位置 + 欧拉角，供 VirtualPod 几何合成
|
||||
* `wait_until_ready` / `set_telemetry_rates` / `print_flight_mode`：连接准备与诊断
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import math
|
||||
import time
|
||||
|
||||
from pod_bridge.virtual_pod import CarrierView
|
||||
|
||||
# 遥测流速率（Hz）——**唯一来源**，供各订阅方（Px4Gateway / TelemetryHub）共用。
|
||||
# PX4 的消息速率由最后一次 SET_MESSAGE_INTERVAL 决定，多个订阅方用不同值会互相
|
||||
# 覆盖；速率越高链路负荷越大（实测压出 MAVSDK 回调队列积压告警）。
|
||||
# 取值依据见 set_telemetry_rates()。
|
||||
RATE_ATTITUDE_HZ = 20.0 # 控制律 attitude_stale_ms=300 要用姿态
|
||||
RATE_POSITION_HZ = 10.0 # 起飞到位 / 到参考点距离
|
||||
RATE_POSITION_VELOCITY_NED_HZ = 10.0 # 虚拟吊舱几何（TelemetryHub）
|
||||
RATE_GPS_HZ = 2.0
|
||||
RATE_VELOCITY_HZ = 2.0
|
||||
RATE_IN_AIR_HZ = 2.0
|
||||
|
||||
|
||||
def now_ms() -> int:
|
||||
"""单调毫秒（与 Px4Gateway / PodBridge / TaskRunner 共用同一时钟源）。"""
|
||||
return int(time.monotonic() * 1000.0)
|
||||
|
||||
|
||||
class TelemetryHub:
|
||||
"""后台订阅 NED 位置/姿态，供 VirtualPod 几何合成与锚点记录。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.yaw_rad = 0.0
|
||||
self.north_m = 0.0
|
||||
self.east_m = 0.0
|
||||
self.down_m = 0.0
|
||||
self.attitude_time_ms = 0
|
||||
self._tasks: list = []
|
||||
|
||||
def start(self, drone) -> None:
|
||||
self._tasks.append(asyncio.create_task(self._attitude_loop(drone)))
|
||||
self._tasks.append(asyncio.create_task(self._position_loop(drone)))
|
||||
|
||||
async def _attitude_loop(self, drone) -> None:
|
||||
async for euler in drone.telemetry.attitude_euler():
|
||||
self.yaw_rad = math.radians(euler.yaw_deg)
|
||||
self.attitude_time_ms = now_ms()
|
||||
|
||||
async def _position_loop(self, drone) -> None:
|
||||
async for pos_vel in drone.telemetry.position_velocity_ned():
|
||||
pos = pos_vel.position
|
||||
self.north_m = pos.north_m
|
||||
self.east_m = pos.east_m
|
||||
self.down_m = pos.down_m
|
||||
|
||||
async def stop(self) -> None:
|
||||
for task in self._tasks:
|
||||
task.cancel()
|
||||
for task in self._tasks:
|
||||
try:
|
||||
await task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
self._tasks.clear()
|
||||
|
||||
def view(self) -> CarrierView:
|
||||
return CarrierView(north_m=self.north_m, east_m=self.east_m,
|
||||
down_m=self.down_m, yaw_rad=self.yaw_rad)
|
||||
|
||||
|
||||
async def print_flight_mode(drone):
|
||||
async for flight_mode in drone.telemetry.flight_mode():
|
||||
print(f"[模式] 当前飞行模式: {flight_mode}")
|
||||
return flight_mode
|
||||
|
||||
|
||||
async def wait_until_ready(drone):
|
||||
"""等待连接 + 位置估计就绪（各阶段限时 30 s）。"""
|
||||
print("等待无人机连接...")
|
||||
try:
|
||||
async with asyncio.timeout(30.0):
|
||||
async for state in drone.core.connection_state():
|
||||
if state.is_connected:
|
||||
print("-- 已连接")
|
||||
break
|
||||
except TimeoutError:
|
||||
raise RuntimeError("30 s 内未建立 MAVSDK 连接（检查 SITL 是否在运行）") from None
|
||||
|
||||
print("等待全球位置估计就绪...")
|
||||
try:
|
||||
async with asyncio.timeout(30.0):
|
||||
async for health in drone.telemetry.health():
|
||||
if health.is_global_position_ok and health.is_home_position_ok:
|
||||
print("-- 位置估计 OK")
|
||||
break
|
||||
except TimeoutError:
|
||||
raise RuntimeError("30 s 内位置估计未就绪") from None
|
||||
|
||||
|
||||
async def set_telemetry_rates(drone):
|
||||
"""按**实际消费需求**设定遥测流速率（不是越高越好）。
|
||||
|
||||
取值依据（2026-10-15 复核）：
|
||||
* `attitude_euler` 20Hz —— 控制律 `attitude_stale_ms=300` 要用姿态；20Hz 足够
|
||||
* `position` 10Hz —— 任务层只需「起飞到位」与「到参考点距离」，10Hz 足够
|
||||
* `position_velocity_ned` 10Hz —— 仅虚拟吊舱几何用（真机链路不订阅）
|
||||
* `gps_info` / `velocity_ned` / `in_air` 2Hz —— 状态量，变化极慢
|
||||
|
||||
原值（position/attitude 各 50Hz + pvn 30Hz ≈ 130 msg/s）在 CPU 紧张的宿主机上
|
||||
会压出 MAVSDK「User callback queue slow」告警；下调后链路负荷降一个量级。
|
||||
失败沿用默认值并告警。
|
||||
"""
|
||||
for stream, rate in (("position", RATE_POSITION_HZ),
|
||||
("attitude_euler", RATE_ATTITUDE_HZ),
|
||||
("position_velocity_ned", RATE_POSITION_VELOCITY_NED_HZ),
|
||||
("gps_info", RATE_GPS_HZ),
|
||||
("velocity_ned", RATE_VELOCITY_HZ),
|
||||
("in_air", RATE_IN_AIR_HZ)):
|
||||
try:
|
||||
await getattr(drone.telemetry, f"set_rate_{stream}")(rate)
|
||||
except Exception as e:
|
||||
print(f"! 设置 {stream} 速率失败（沿用默认值）: {e}")
|
||||
@@ -1,112 +0,0 @@
|
||||
# stereo_ranger（Python 版）
|
||||
|
||||
双目稀疏目标测距：对**同一时刻**左右两目已立体校正图像上的目标检测框做立体
|
||||
几何解算，输出少量目标的**前向距离 Z + 斜距 R（参考）+ 虚拟双目中心方位
|
||||
c_mid + 有效性/置信度**。不做目标识别、图像采集/立体校正、稠密深度。
|
||||
|
||||
规格：`docs/双目测距技术路线.md`（算法与语义权威）。
|
||||
|
||||
⚠ **全部参数为占位值**（焦距/标定待厂商数据）：标定与地面真值验证完成前，
|
||||
本模块输出**仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据**（§12.3）。
|
||||
|
||||
## 接口清单
|
||||
|
||||
| 名称 | 说明 |
|
||||
| --- | --- |
|
||||
| `StereoRanger(config=None, matcher=None)` | 主入口；`matcher` 可注入自定义匹配器 |
|
||||
| `.update(left, right, now_ms=0) -> List[RangeRecord]` | 单周期处理（§5 五步），纯计算、无 IO |
|
||||
| `.reset()` | 清空全部运行期状态（滤波/一致性/隔离/节流计数） |
|
||||
| `.config` / `.matcher` | 只读访问（配置与匹配器） |
|
||||
| `StereoConfig` / `load_from_json(path)` | 17 项配置；缺项/未知字段/类型错误/非法取值一律 `ValueError`（fail-fast） |
|
||||
| `.focal_baseline_px_m` / `.z_min_m` / `.z_max_m` / `.z_error_budget_max_m` | 派生只读量（F·B 常量、视差域互推的距离域、误差预算远界） |
|
||||
| `Detection(x1,y1,x2,y2, confidence, class_id, track_id)` | 单框（校正后坐标系；含 `cx/cy/width/height/bbox` 属性） |
|
||||
| `DetectionFrame(frame_id, detections)` | 单目单帧 |
|
||||
| `RangeRecord` | 输出记录：`frame_id / class_id / degradation / valid / track_id / forward_distance_m(Z) / slant_range_m(R) / disparity_px(D) / center_mid_px(c_mid) / reference_box / reference_eye / confidence / timestamp_ms` |
|
||||
| `Degradation` / `Eye` / `DisparitySource` | 枚举：完整/仅方位；左/右目；同侧边缘/中心回退 |
|
||||
| `sample_point` / `SamplePoint` / `select_disparity` / `Disparity` | 取点器（§6.1，纯函数） |
|
||||
| `Matcher`（Protocol）/ `GreedyMatcher` / `MatchPair` | 匹配器（§6.2，接口注入，可换匈牙利等） |
|
||||
| `solve_range` / `RangeSolution` / `virtual_center_px` / `forward_distance_m` | 测距与方位源（§3.2/§6.6，纯函数） |
|
||||
| `evaluate_validity` / `ValidityDecision` / `range_error_abs_m` / `range_error_rel` / `confidence_from_cost` / `DELTA_D_PX` | 有效性三道门与误差模型（§6.4/§7.1，纯函数） |
|
||||
| `ConsistencyMonitor` / `ConsistencyStatus` | 一致性监控（§6.7，每目标状态） |
|
||||
| `DistanceFilter` | 距离低通 + 跳变重置（§6.5，每目标状态） |
|
||||
| `TargetStore` | 目标隔离与老化清理（§4，track_id 优先 + 最近邻兜底） |
|
||||
| `WarnThrottle` | 告警节流（第 1 次 + 每满 100 次，带累计计数） |
|
||||
|
||||
`logger` 名称为 `stereo_ranger`，走标准 `logging`。
|
||||
|
||||
## 单帧流程（`update` 五步）
|
||||
|
||||
1. **同帧契约**：`left.frame_id != right.frame_id` → 拒收整帧、返回空列表、WARN 节流（§2.1）；
|
||||
2. **取点**：逐框取同侧（左）边缘 x 与框中心；坐标非有限值的框丢弃并 WARN（不参与解算也不发布）；
|
||||
3. **匹配**：硬约束（类别一致、`|Δy| ≤ tau_y_px`、框高相对差 ≤ `tau_h_ratio`）+ 代价升序贪心一一配对，`cost < 1` 才接受；
|
||||
4. **配对目标**：`D → Z = F·B/D`（`D ≤ 0` 判违例）→ `R` 参考 → `c_mid = (cL + cR)/2` → 门① 域内 / 门② 误差预算 / 门③ 一致性 → 有效帧过 `DistanceFilter`，记 `Z_out`；
|
||||
5. **未配对框**：发布仅方位记录（`BEARING_ONLY`，`valid = false`，右目未匹配同样发布）；帧末对未出现目标记失效、连续 `filter_reset_frames` 帧未出现整体清理。
|
||||
|
||||
记录顺序：先按左目框顺序（配对→完整 / 未配对→仅方位），再给未配对右目框的仅方位记录。
|
||||
|
||||
## 实现决策（v1.0 冻结，已回填规格）
|
||||
|
||||
规格 `docs/双目测距技术路线.md` 已按下列决策修订（§4/§6.1/§6.2/§6.4/§6.7/
|
||||
§9/§11 均已更新），本节保留决策动机与实现落点，供重写/移植核对。
|
||||
|
||||
1. **越界 = 有效性门，不是配对硬约束**：规格 §6.2 原把"视差在域内"列入
|
||||
硬约束，与 §5/§6.3/§6.4/§2.3 的表述冲突；按 §5 主流程实现并回填——
|
||||
越界目标仍配对、仍发布 `D` 与 `c_mid`（远场方位闭环的输入基础），仅
|
||||
`valid = false`。
|
||||
2. **视差域权威**：`disparity_min_px / disparity_max_px` 为真源，`z_min_m / z_max_m`
|
||||
为派生只读属性；原 §9.2 占位值不自洽（`D_min = 3 px ⇔ 19.2 m ≠ 20 m`），
|
||||
距离域配置项已从规格删除（§6.4/§9.2）。
|
||||
3. **不实现 §6.1 的"左右边缘各算一次取中值"精化**：两支的 median 恒等于均值，
|
||||
即框中心视差，与"同侧边缘"的理由矛盾，规格已删除该项（早期笔误），留
|
||||
v1.1 亚像素精化。
|
||||
4. **补充配置项**（原文 §9 未列、算法需要，规格 §9.3/§9.5 已回填）：
|
||||
`consistency_jump_px`（§6.7 视差跳变门限）、`isolation_gate_px`（§4 最近邻
|
||||
兜底的门限）。
|
||||
5. **δD 固定 1 px**：`DELTA_D_PX = 1.0`（§7.1 ⚠占位）；标定回填后可参数化。
|
||||
6. **R 与已发布 Z 同基准**：`R = R_raw · z_out / z_raw`（X/Y/Z 均 ∝ Z，精确换基，
|
||||
保证 `R ≥ Z`）；无效帧无 `Z_out` 时 `R` 取本帧原始值（诊断参考）。规格
|
||||
§2.2/§6.5 已注明。
|
||||
7. **置信度映射**（§6.4 ⚠占位）：`clamp(1 − cost, 0, 1)`；连续跳变 ≥ N 帧乘 0.5
|
||||
（`_CONFIDENCE_DEGRADE_FACTOR`）；判无效置 0。
|
||||
8. **一致性只在配对成功帧推进**：仅方位帧没有 D 观测量，不推进 D 序列；但目标
|
||||
未出现的帧会记一次滤波失效（§6.5 失效清理），两条状态机职责不混。
|
||||
9. **取点回退规则**：边缘视差越窗且中心视差在域内 → 回退中心
|
||||
（`CENTER_FALLBACK` + WARN）；两者均越窗 → 保留边缘值供诊断与违例告警。
|
||||
10. **track_id 优先路径提前启用**（v1.1 语义，规格 §4 已按此冻结）：左右
|
||||
`track_id` 一致且 > 0 时用 `(track, 类别, id)` 作隔离键；最近邻只在无
|
||||
track_id 的状态间匹配，一一归属。
|
||||
11. **§3.2 勘误**：原文 `X_C = X + B/2` 与 §6.6 的符号相反（应为 `X_C = X − B/2`），
|
||||
规格已修正；代码不输出 `X_C`，以 §6.6 恒等式为准。
|
||||
|
||||
## 运行期状态清单（§11.1，漏一个就变行为）
|
||||
|
||||
| 状态 | 所属 | 初值 | 改变时机 |
|
||||
| --- | --- | --- | --- |
|
||||
| 连续异常计数 ×目标 | `ConsistencyMonitor` | 0 | 每帧异常 +1 / 正常帧清零 |
|
||||
| 上次滤波值、初始化标志、连续失效计数 ×目标 | `DistanceFilter` | 无 / false / 0 | 有效帧写入 / 首有效帧置位 / 无效帧 +1 |
|
||||
| 最近位置、最近出现帧 ×目标 | `TargetStore` | 首帧写入 | 每次 `resolve` |
|
||||
| 帧序号 | `StereoRanger` | 0 | 每帧 +1（拒收帧不加） |
|
||||
| 告警计数 ×异常类 | `WarnThrottle` | 0 | 每次异常 |
|
||||
|
||||
## 测试与演示
|
||||
|
||||
```bash
|
||||
cd utils
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest tests/stereo_ranger -q # 111 条
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q # 全量 220 条
|
||||
python utils/demo_stereo_ranger.py # 六场景合成框对演示
|
||||
```
|
||||
|
||||
构件级测试逐条对应 §11.4（取点器/匹配器/测距/有效性/滤波器/一致性/全流程），
|
||||
**全部用合成框对驱动，无需真实图像**（几何是解析可断言的）：
|
||||
|
||||
- 取点：同侧边缘取值、框宽差不进入视差、中心对照、越窗回退、边界含等号、负视差；
|
||||
- 匹配：硬约束各一例、`cost ≥ 1` 拒绝、贪心排序、一一配对；
|
||||
- 测距：`Z = F·B/D` 回收、`D ≤ 0` 违例、`c_mid` 等权平均恒等式、误差平方增长；
|
||||
- 有效性：三道门与边界归类、预算门通过/拦截两侧；
|
||||
- 滤波/一致性：低通收敛、跳变重置不拖尾、失效清理、N 帧降级 / M 帧判无效 / 正常帧清零；
|
||||
- 全流程：单目标、滤波、远场越界仍配对、单目退化、记录顺序、非同帧拒收与节流、
|
||||
空输入、非有限框、track_id 跨大位移保持状态、最近邻越门限新建状态、匹配器注入、`reset`。
|
||||
|
||||
demo 覆盖：① 逼近（三道门依次生效 + 滤波）② 远场域外（方位照常发布）
|
||||
③ 单目退化 ④ 非同帧拒收与节流 ⑤ 一致性降级/判无效 ⑥ 多目标贪心配对。
|
||||
@@ -1,72 +0,0 @@
|
||||
"""stereo_ranger —— 双目稀疏目标测距（规格：docs/双目测距技术路线.md）。
|
||||
|
||||
纯算法包：输入校正后坐标系的同帧左右目检测框，输出目标距离记录（Z/R/D、
|
||||
虚拟双目中心方位、valid/confidence）。不做目标识别、图像采集与立体校正；
|
||||
仅依赖标准库。⚠ 全部数值参数为占位值，标定与地面验证完成前不得作为飞行
|
||||
控制决策依据（§12.3 红线）。
|
||||
"""
|
||||
|
||||
from .config import StereoConfig, load_from_json
|
||||
from .consistency import ConsistencyMonitor, ConsistencyStatus
|
||||
from .distance_filter import DistanceFilter
|
||||
from .geometry import (
|
||||
DELTA_D_PX,
|
||||
RangeSolution,
|
||||
ValidityDecision,
|
||||
confidence_from_cost,
|
||||
evaluate_validity,
|
||||
forward_distance_m,
|
||||
range_error_abs_m,
|
||||
range_error_rel,
|
||||
solve_range,
|
||||
virtual_center_px,
|
||||
)
|
||||
from .matching import GreedyMatcher, MatchPair, Matcher
|
||||
from .sampling import Disparity, SamplePoint, sample_point, select_disparity
|
||||
from .stereo_ranger import StereoRanger
|
||||
from .target_store import TargetStore
|
||||
from .types import (
|
||||
BBoxXYXY,
|
||||
Degradation,
|
||||
Detection,
|
||||
DetectionFrame,
|
||||
DisparitySource,
|
||||
Eye,
|
||||
RangeRecord,
|
||||
)
|
||||
from .warn_throttle import WarnThrottle
|
||||
|
||||
__all__ = [
|
||||
"BBoxXYXY",
|
||||
"DELTA_D_PX",
|
||||
"ConsistencyMonitor",
|
||||
"ConsistencyStatus",
|
||||
"Degradation",
|
||||
"Detection",
|
||||
"DetectionFrame",
|
||||
"Disparity",
|
||||
"DisparitySource",
|
||||
"DistanceFilter",
|
||||
"Eye",
|
||||
"GreedyMatcher",
|
||||
"MatchPair",
|
||||
"Matcher",
|
||||
"RangeRecord",
|
||||
"RangeSolution",
|
||||
"SamplePoint",
|
||||
"StereoConfig",
|
||||
"StereoRanger",
|
||||
"TargetStore",
|
||||
"ValidityDecision",
|
||||
"WarnThrottle",
|
||||
"confidence_from_cost",
|
||||
"evaluate_validity",
|
||||
"forward_distance_m",
|
||||
"load_from_json",
|
||||
"range_error_abs_m",
|
||||
"range_error_rel",
|
||||
"sample_point",
|
||||
"select_disparity",
|
||||
"solve_range",
|
||||
"virtual_center_px",
|
||||
]
|
||||
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.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,123 +0,0 @@
|
||||
"""StereoConfig 与 JSON 配置加载（§9 配置参数全量表）。
|
||||
|
||||
全部参数配置外置、构造期一次加载并校验：缺项、类型错误、非法取值一律拒绝
|
||||
进入运行期（fail-fast）。所有默认值均为 ⚠占位值，未整定前不得用于飞行决策
|
||||
（§12.3 红线 1/2）。
|
||||
|
||||
域权威（实现决策）：视差域 disparity_min_px / disparity_max_px 为权威，
|
||||
距离域 z_min_m / z_max_m 由 Z = F·B/D 派生为只读属性——域判定使用视差
|
||||
（§11.2 直接观测量），两套阈值不再冗余存储、避免互推舍入不一致（§6.4）。
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
from dataclasses import dataclass, fields
|
||||
from typing import Any, Dict
|
||||
|
||||
# JSON 加载时的整数型字段（其余按数值校验）
|
||||
_INT_FIELDS = frozenset({"consistency_N_frames", "consistency_M_frames", "filter_reset_frames"})
|
||||
|
||||
|
||||
@dataclass
|
||||
class StereoConfig:
|
||||
# ---- 几何组（来自标定 P1/P2，§9.1；⚠全部为占位值）----
|
||||
fx_px: float = 960.0
|
||||
fy_px: float = 960.0
|
||||
cx_px: float = 960.0
|
||||
cy_px: float = 540.0
|
||||
baseline_m: float = 0.06
|
||||
|
||||
# ---- 匹配组（§9.2）----
|
||||
tau_y_px: float = 2.0
|
||||
tau_h_ratio: float = 0.25
|
||||
disparity_min_px: float = 3.0
|
||||
disparity_max_px: float = 57.6 # ⚠占位：F=960/B=0.06 下 ⇔ 近界 1.0 m
|
||||
|
||||
# ---- 有效性组（§9.3）----
|
||||
max_delta_z_m: float = 2.0 # 误差预算 δZ(δD = 1px) 上限
|
||||
consistency_N_frames: int = 3 # 连续跳变降置信度的帧数
|
||||
consistency_M_frames: int = 5 # 连续异常判 invalid 的帧数
|
||||
consistency_jump_px: float = 3.0 # ⚠补充项：§9 未列，§6.7 的视差跳变门限
|
||||
|
||||
# ---- 滤波组（§9.4）----
|
||||
smooth_alpha: float = 0.3
|
||||
jump_reset_m: float = 5.0
|
||||
filter_reset_frames: int = 5
|
||||
|
||||
# ---- 目标隔离（§4：类别 + 框位置最近邻；⚠补充项）----
|
||||
isolation_gate_px: float = 50.0
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
_require(self.fx_px > 0.0, "fx_px", "必须为正")
|
||||
_require(self.fy_px > 0.0, "fy_px", "必须为正")
|
||||
_require(self.cx_px >= 0.0, "cx_px", "必须非负")
|
||||
_require(self.cy_px >= 0.0, "cy_px", "必须非负")
|
||||
_require(self.baseline_m > 0.0, "baseline_m", "必须为正")
|
||||
_require(self.tau_y_px > 0.0, "tau_y_px", "必须为正")
|
||||
_require(self.tau_h_ratio > 0.0, "tau_h_ratio", "必须为正")
|
||||
_require(
|
||||
0.0 < self.disparity_min_px < self.disparity_max_px,
|
||||
"disparity_min_px/disparity_max_px",
|
||||
"须满足 0 < 下限 < 上限",
|
||||
)
|
||||
_require(self.max_delta_z_m > 0.0, "max_delta_z_m", "必须为正")
|
||||
_require(self.consistency_N_frames >= 1, "consistency_N_frames", "必须 ≥ 1")
|
||||
_require(
|
||||
self.consistency_M_frames >= self.consistency_N_frames,
|
||||
"consistency_M_frames",
|
||||
"必须 ≥ consistency_N_frames",
|
||||
)
|
||||
_require(self.consistency_jump_px > 0.0, "consistency_jump_px", "必须为正")
|
||||
_require(0.0 < self.smooth_alpha <= 1.0, "smooth_alpha", "须落在 (0, 1]")
|
||||
_require(self.jump_reset_m > 0.0, "jump_reset_m", "必须为正")
|
||||
_require(self.filter_reset_frames >= 1, "filter_reset_frames", "必须 ≥ 1")
|
||||
_require(self.isolation_gate_px > 0.0, "isolation_gate_px", "必须为正")
|
||||
|
||||
@property
|
||||
def focal_baseline_px_m(self) -> float:
|
||||
"""F·B：构造期一次算好的常量，热路径只做乘除（§11.1）。"""
|
||||
return self.fx_px * self.baseline_m
|
||||
|
||||
@property
|
||||
def z_min_m(self) -> float:
|
||||
"""近界（由 disparity_max_px 派生）。"""
|
||||
return self.focal_baseline_px_m / self.disparity_max_px
|
||||
|
||||
@property
|
||||
def z_max_m(self) -> float:
|
||||
"""远界（由 disparity_min_px 派生）。"""
|
||||
return self.focal_baseline_px_m / self.disparity_min_px
|
||||
|
||||
@property
|
||||
def z_error_budget_max_m(self) -> float:
|
||||
"""误差预算门限对应的最远距离 sqrt(δZ_max·F·B)（δD = 1px，§7.2）。"""
|
||||
return math.sqrt(self.max_delta_z_m * self.focal_baseline_px_m)
|
||||
|
||||
|
||||
def load_from_json(path: str) -> StereoConfig:
|
||||
"""从 JSON 文件构建配置；缺项/未知字段/类型错误/非法取值一律抛 ValueError。"""
|
||||
with open(path, encoding="utf-8") as handle:
|
||||
data: Dict[str, Any] = json.load(handle)
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError("配置非法: 顶层须为 JSON 对象")
|
||||
known = {item.name for item in fields(StereoConfig)}
|
||||
unknown = sorted(set(data) - known)
|
||||
if unknown:
|
||||
raise ValueError(f"未知配置字段: {unknown}")
|
||||
missing = sorted(known - set(data))
|
||||
if missing:
|
||||
raise ValueError(f"缺少配置字段: {missing}")
|
||||
for name, value in data.items():
|
||||
if isinstance(value, bool):
|
||||
raise ValueError(f"配置类型非法: {name} 须为数值")
|
||||
if name in _INT_FIELDS:
|
||||
if not isinstance(value, int):
|
||||
raise ValueError(f"配置类型非法: {name} 须为整数")
|
||||
elif not isinstance(value, (int, float)):
|
||||
raise ValueError(f"配置类型非法: {name} 须为数值")
|
||||
return StereoConfig(**data)
|
||||
|
||||
|
||||
def _require(condition: bool, field_name: str, message: str) -> None:
|
||||
if not condition:
|
||||
raise ValueError(f"配置非法: {field_name} {message}")
|
||||
@@ -1,74 +0,0 @@
|
||||
"""一致性监控（§6.7）：时序层面质量自检，每目标独立状态。
|
||||
|
||||
机载激光移除后无外部距离基准，飞行中只能靠自身时序一致性监控数据质量：
|
||||
**只能降级、不能纠偏**（绝对精度靠地面静态验证）。匹配层面的质量（|Δy|
|
||||
残差、框高一致性、代价门限）由匹配硬约束承担，本节只管时序层面。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, Hashable, Optional
|
||||
|
||||
|
||||
@dataclass
|
||||
class ConsistencyStatus:
|
||||
"""单目标单帧一致性判定结果（§6.7）。"""
|
||||
|
||||
jump_px: Optional[float] # |D − 上次 D|；首帧无参考为 None
|
||||
abnormal: bool # 本帧是否跳变超门限
|
||||
streak: int # 连续异常帧数（含本帧，正常帧清零）
|
||||
confidence_degraded: bool # streak ≥ N：置信度降级
|
||||
invalid: bool # streak ≥ M：质量语义判无效
|
||||
|
||||
|
||||
class ConsistencyMonitor:
|
||||
"""每目标：上次视差 + 连续异常计数（§11.1 必须持久化的状态）。"""
|
||||
|
||||
def __init__(self, jump_px: float, degrade_frames: int, invalid_frames: int) -> None:
|
||||
if jump_px <= 0.0:
|
||||
raise ValueError("jump_px 必须为正")
|
||||
if degrade_frames < 1:
|
||||
raise ValueError("degrade_frames 必须 ≥ 1")
|
||||
if invalid_frames < degrade_frames:
|
||||
raise ValueError("invalid_frames 必须 ≥ degrade_frames")
|
||||
self._jump_px = jump_px
|
||||
self._degrade_frames = degrade_frames
|
||||
self._invalid_frames = invalid_frames
|
||||
self._last_d: Dict[Hashable, float] = {}
|
||||
self._streak: Dict[Hashable, int] = {}
|
||||
|
||||
def advance(self, key: Hashable, disparity_px: float) -> ConsistencyStatus:
|
||||
"""推进目标 key 的一致性判定（配对成功帧每帧一次）。"""
|
||||
last = self._last_d.get(key)
|
||||
if last is None:
|
||||
jump: Optional[float] = None
|
||||
abnormal = False
|
||||
streak = 0
|
||||
else:
|
||||
jump = abs(disparity_px - last)
|
||||
abnormal = jump > self._jump_px
|
||||
streak = self._streak.get(key, 0) + 1 if abnormal else 0
|
||||
self._last_d[key] = disparity_px
|
||||
if streak:
|
||||
self._streak[key] = streak
|
||||
else:
|
||||
self._streak.pop(key, None)
|
||||
return ConsistencyStatus(
|
||||
jump_px=jump,
|
||||
abnormal=abnormal,
|
||||
streak=streak,
|
||||
confidence_degraded=streak >= self._degrade_frames,
|
||||
invalid=streak >= self._invalid_frames,
|
||||
)
|
||||
|
||||
def streak(self, key: Hashable) -> int:
|
||||
"""目标 key 当前连续异常计数。"""
|
||||
return self._streak.get(key, 0)
|
||||
|
||||
def drop(self, key: Hashable) -> None:
|
||||
"""清理目标 key 的状态（目标老化清理时调用）。"""
|
||||
self._last_d.pop(key, None)
|
||||
self._streak.pop(key, None)
|
||||
|
||||
def clear(self) -> None:
|
||||
self._last_d.clear()
|
||||
self._streak.clear()
|
||||
@@ -1,68 +0,0 @@
|
||||
"""时间滤波器（§6.5）：一阶低通 + 单帧跳变重置，每目标独立状态。
|
||||
|
||||
- 单帧毛刺由跳变重置处理，持续性异常由一致性监控（§6.7）处理，两者职责
|
||||
不同不得合并；
|
||||
- 滤波只在**有效帧**之间进行，无效/缺席帧不参与平滑；α = 1 等效不滤波；
|
||||
- 连续失效（无效或缺席）达 filter_reset_frames 帧清空状态，下次有效帧重新
|
||||
初始化（防止用陈旧状态平滑新测量）。
|
||||
"""
|
||||
|
||||
from typing import Dict, Hashable, Optional
|
||||
|
||||
|
||||
class DistanceFilter:
|
||||
"""每目标：上次滤波值 + 连续失效计数（§11.1 必须持久化的状态）。"""
|
||||
|
||||
def __init__(self, alpha: float, jump_reset_m: float, reset_frames: int) -> None:
|
||||
if not 0.0 < alpha <= 1.0:
|
||||
raise ValueError("alpha 须落在 (0, 1]")
|
||||
if jump_reset_m <= 0.0:
|
||||
raise ValueError("jump_reset_m 必须为正")
|
||||
if reset_frames < 1:
|
||||
raise ValueError("reset_frames 必须 ≥ 1")
|
||||
self._alpha = alpha
|
||||
self._jump_reset_m = jump_reset_m
|
||||
self._reset_frames = reset_frames
|
||||
self._values: Dict[Hashable, float] = {}
|
||||
self._invalid_streak: Dict[Hashable, int] = {}
|
||||
|
||||
def advance(self, key: Hashable, z_m: float) -> float:
|
||||
"""有效帧推进：未初始化→直接置值；跳变超门限→重置；否则一阶低通。"""
|
||||
previous = self._values.get(key)
|
||||
self._invalid_streak.pop(key, None)
|
||||
if previous is None:
|
||||
self._values[key] = z_m
|
||||
return z_m
|
||||
if abs(z_m - previous) > self._jump_reset_m:
|
||||
self._values[key] = z_m # 直接重置而非滤波，防止拖尾
|
||||
return z_m
|
||||
smoothed = previous + self._alpha * (z_m - previous)
|
||||
self._values[key] = smoothed
|
||||
return smoothed
|
||||
|
||||
def invalidate(self, key: Hashable) -> None:
|
||||
"""无效/缺席帧：连续失效达门限后清空该目标滤波状态（§6.5 记失效）。"""
|
||||
if key not in self._values:
|
||||
return
|
||||
streak = self._invalid_streak.get(key, 0) + 1
|
||||
if streak >= self._reset_frames:
|
||||
self._values.pop(key, None)
|
||||
self._invalid_streak.pop(key, None)
|
||||
else:
|
||||
self._invalid_streak[key] = streak
|
||||
|
||||
def value(self, key: Hashable) -> Optional[float]:
|
||||
"""目标 key 当前滤波值；未初始化/已清空为 None。"""
|
||||
return self._values.get(key)
|
||||
|
||||
def invalid_streak(self, key: Hashable) -> int:
|
||||
"""目标 key 当前连续失效计数（内部状态，供测试/诊断）。"""
|
||||
return self._invalid_streak.get(key, 0)
|
||||
|
||||
def drop(self, key: Hashable) -> None:
|
||||
self._values.pop(key, None)
|
||||
self._invalid_streak.pop(key, None)
|
||||
|
||||
def clear(self) -> None:
|
||||
self._values.clear()
|
||||
self._invalid_streak.clear()
|
||||
@@ -1,84 +0,0 @@
|
||||
"""测距计算与有效性判定（§3.2/§6.4/§7.1）：纯函数、无状态。
|
||||
|
||||
Z = F·B/D（前向分量，距离通道唯一消费量，§10.5）；X/Y 以左目系表达；
|
||||
R = sqrt(X² + Y² + Z²) 仅参考输出；c_mid = 两目框中心等权平均（虚拟双目
|
||||
中心，§6.6，逐点精确、无近似）。
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional, Tuple
|
||||
|
||||
from .config import StereoConfig
|
||||
from .types import Detection
|
||||
|
||||
DELTA_D_PX = 1.0 # 误差预算的δD 基准（§7.2 定义 δZ(δD = 1px)）
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RangeSolution:
|
||||
"""测距解算结果（Z 为前向分量，X/Y 属左目系，§3.2）。"""
|
||||
|
||||
z_m: float
|
||||
x_m: float
|
||||
y_m: float
|
||||
r_m: float # 斜距（参考值，不进控制回路）
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ValidityDecision:
|
||||
"""有效性判定结果（§6.4 三道门的前两道；门③一致性由调用方合流）。"""
|
||||
|
||||
in_domain: bool # ① 域内：D ∈ [D_min, D_max]
|
||||
budget_ok: bool # ② 误差预算：δZ(δD = 1px) ≤ δZ_max
|
||||
valid: bool # ① 且 ②
|
||||
|
||||
|
||||
def forward_distance_m(disparity_px: float, config: StereoConfig) -> float:
|
||||
"""Z = F·B/D；调用方须先判 D ≤ 0 物理违例（§6.3/§11.2 除零纪律）。"""
|
||||
if disparity_px <= 0.0:
|
||||
raise ValueError(f"视差须为正（先判违例再除）: D={disparity_px}")
|
||||
return config.focal_baseline_px_m / disparity_px
|
||||
|
||||
|
||||
def solve_range(left_box: Detection, right_box: Detection, disparity_px: float, config: StereoConfig) -> RangeSolution:
|
||||
"""由视差与参考目（左目）框中心解算 (Z, X, Y, R)（§3.2）。"""
|
||||
z_m = forward_distance_m(disparity_px, config)
|
||||
x_m = (left_box.cx - config.cx_px) * z_m / config.fx_px
|
||||
y_m = (left_box.cy - config.cy_px) * z_m / config.fy_px
|
||||
r_m = math.sqrt(x_m * x_m + y_m * y_m + z_m * z_m)
|
||||
return RangeSolution(z_m=z_m, x_m=x_m, y_m=y_m, r_m=r_m)
|
||||
|
||||
|
||||
def virtual_center_px(left_box: Detection, right_box: Detection) -> Tuple[float, float]:
|
||||
"""虚拟双目中心 c_mid = (cL + cR)/2（§6.6：等权平均 = 把相机搬到模组中线）。
|
||||
|
||||
与视差取点（同侧边缘）是两个不同的取点，不得混用（§6.6）。
|
||||
"""
|
||||
return ((left_box.cx + right_box.cx) / 2.0, (left_box.cy + right_box.cy) / 2.0)
|
||||
|
||||
|
||||
def range_error_abs_m(z_m: float, config: StereoConfig, delta_d_px: float = DELTA_D_PX) -> float:
|
||||
"""绝对误差估计 δZ ≈ Z²·δD/(F·B)（§7.1）。"""
|
||||
return z_m * z_m * delta_d_px / config.focal_baseline_px_m
|
||||
|
||||
|
||||
def range_error_rel(z_m: float, config: StereoConfig, delta_d_px: float = DELTA_D_PX) -> float:
|
||||
"""相对误差估计 δZ/Z ≈ Z·δD/(F·B)（§7.1）。"""
|
||||
return z_m * delta_d_px / config.focal_baseline_px_m
|
||||
|
||||
|
||||
def evaluate_validity(disparity_px: float, z_m: Optional[float], config: StereoConfig) -> ValidityDecision:
|
||||
"""有效性门①②（§6.4）：域内 + 误差预算；z_m=None（D ≤ 0）时预算判失败。"""
|
||||
in_domain = config.disparity_min_px <= disparity_px <= config.disparity_max_px
|
||||
budget_ok = (
|
||||
z_m is not None
|
||||
and math.isfinite(z_m)
|
||||
and range_error_abs_m(z_m, config) <= config.max_delta_z_m
|
||||
)
|
||||
return ValidityDecision(in_domain=in_domain, budget_ok=budget_ok, valid=in_domain and budget_ok)
|
||||
|
||||
|
||||
def confidence_from_cost(cost: float) -> float:
|
||||
"""匹配代价 → 置信度基值：线性映射 clamp(1 − cost, 0, 1)（§6.4 ⚠占位映射）。"""
|
||||
return min(1.0, max(0.0, 1.0 - cost))
|
||||
@@ -1,82 +0,0 @@
|
||||
"""左右匹配器（§6.2）：硬约束 + 代价升序贪心一一配对；接口注入。
|
||||
|
||||
默认 GreedyMatcher 为纯函数（§4 构件表）；调用方可通过
|
||||
StereoRanger(matcher=...) 注入匈牙利等替换实现而不动调用方（§10.3）。
|
||||
|
||||
硬约束（任一不满足直接拒绝配对，不进代价排序）：类别一致、|Δy| ≤ τ_y、
|
||||
框高相对差 ≤ τ_h。视差域 [D_min, D_max] 不参与配对：越界属有效性门
|
||||
（§6.2 已回填，与 §5 失败路径表/§6.3/§6.4/§2.3 一致）。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import List, Protocol, Sequence
|
||||
|
||||
from .config import StereoConfig
|
||||
from .sampling import SamplePoint
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MatchPair:
|
||||
"""一一配对结果：左右下标 + 匹配代价（§6.2）。"""
|
||||
|
||||
left_index: int
|
||||
right_index: int
|
||||
cost: float
|
||||
|
||||
|
||||
class Matcher(Protocol):
|
||||
"""匹配器接口（§4：接口注入，默认贪心）。"""
|
||||
|
||||
def match(
|
||||
self,
|
||||
left: Sequence[SamplePoint],
|
||||
right: Sequence[SamplePoint],
|
||||
config: StereoConfig,
|
||||
) -> List[MatchPair]:
|
||||
...
|
||||
|
||||
|
||||
class GreedyMatcher:
|
||||
"""硬约束过滤 + 代价升序贪心：cost < 1 才接受，一一配对。"""
|
||||
|
||||
def match(
|
||||
self,
|
||||
left: Sequence[SamplePoint],
|
||||
right: Sequence[SamplePoint],
|
||||
config: StereoConfig,
|
||||
) -> List[MatchPair]:
|
||||
tau_y = config.tau_y_px
|
||||
tau_h = config.tau_h_ratio
|
||||
candidates = []
|
||||
for i, point_l in enumerate(left):
|
||||
if not point_l.finite:
|
||||
continue
|
||||
for j, point_r in enumerate(right):
|
||||
if not point_r.finite:
|
||||
continue
|
||||
if point_l.box.class_id != point_r.box.class_id:
|
||||
continue
|
||||
dy = abs(point_l.center_y - point_r.center_y)
|
||||
if dy > tau_y:
|
||||
continue
|
||||
height_max = max(point_l.box.height, point_r.box.height)
|
||||
if height_max <= 0.0:
|
||||
continue
|
||||
dh_ratio = abs(point_l.box.height - point_r.box.height) / height_max
|
||||
if dh_ratio > tau_h:
|
||||
continue
|
||||
cost = dy / tau_y + dh_ratio / tau_h
|
||||
if cost >= 1.0:
|
||||
continue
|
||||
candidates.append((cost, i, j))
|
||||
candidates.sort(key=lambda item: (item[0], item[1], item[2]))
|
||||
used_left = set()
|
||||
used_right = set()
|
||||
pairs: List[MatchPair] = []
|
||||
for cost, i, j in candidates:
|
||||
if i in used_left or j in used_right:
|
||||
continue
|
||||
pairs.append(MatchPair(left_index=i, right_index=j, cost=cost))
|
||||
used_left.add(i)
|
||||
used_right.add(j)
|
||||
return pairs
|
||||
@@ -1,73 +0,0 @@
|
||||
"""视差取点器（§6.1）：同侧（左）边缘为主方案，边缘视差越窗时回退框中心。
|
||||
|
||||
纯函数、无状态（§4 构件表）。为什么用同侧边缘：同一物理目标在两目中的
|
||||
检测框宽并不严格相等，框中心视差会混入框宽差的一半，同侧边缘视差可抵消
|
||||
框宽项。
|
||||
|
||||
注：§6.1 的"左右两边缘各算一次视差取中值"精化在数学上恒等于框中心视差
|
||||
（两支的 median = 均值），与"中心视差混入框宽差"的理由矛盾，属原文笔误，
|
||||
v1.0 不实现；规格 §6.1 已删除该项并注明（见包 README「实现决策」）。
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
from .config import StereoConfig
|
||||
from .types import Detection, DisparitySource
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SamplePoint:
|
||||
"""单目单框取点结果：同侧边缘候选与框中心（§6.1/§6.6）。
|
||||
|
||||
finite=False 表示坐标非有限值：该框不可参与解算，由调用方丢弃并告警。
|
||||
"""
|
||||
|
||||
box: Detection
|
||||
edge_x: float # 同侧（左）边缘 x = box.x1
|
||||
center_x: float
|
||||
center_y: float
|
||||
finite: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Disparity:
|
||||
"""配对后的视差取点结果。"""
|
||||
|
||||
d_px: float
|
||||
source: DisparitySource
|
||||
fallback_used: bool # 边缘视差越窗、回退中心（需告警节流，§6.1）
|
||||
in_domain: bool # d ∈ [D_min, D_max]（含边界，§11.2）
|
||||
positive: bool # d > 0（正视差是物理前提，§6.3）
|
||||
|
||||
|
||||
def sample_point(detection: Detection) -> SamplePoint:
|
||||
"""逐框取点（§5 第 ② 步，每目每框一次）。"""
|
||||
finite = all(math.isfinite(value) for value in (detection.x1, detection.y1, detection.x2, detection.y2))
|
||||
return SamplePoint(
|
||||
box=detection,
|
||||
edge_x=detection.x1,
|
||||
center_x=detection.cx,
|
||||
center_y=detection.cy,
|
||||
finite=finite,
|
||||
)
|
||||
|
||||
|
||||
def select_disparity(left: SamplePoint, right: SamplePoint, config: StereoConfig) -> Disparity:
|
||||
"""在同侧边缘视差与框中心视差之间选择（§6.1 主取点 + 越窗回退）。
|
||||
|
||||
规则：边缘视差在域内取边缘；否则若中心视差在域内则回退中心（告警节流）；
|
||||
两者均越窗时保留边缘值（供诊断与违例告警），由有效性门判无效。
|
||||
"""
|
||||
d_edge = left.edge_x - right.edge_x
|
||||
if _in_domain(d_edge, config):
|
||||
return Disparity(d_edge, DisparitySource.LEFT_EDGE, False, True, d_edge > 0.0)
|
||||
d_center = left.center_x - right.center_x
|
||||
if _in_domain(d_center, config):
|
||||
return Disparity(d_center, DisparitySource.CENTER_FALLBACK, True, True, d_center > 0.0)
|
||||
d = d_edge if math.isfinite(d_edge) else d_center
|
||||
return Disparity(d, DisparitySource.LEFT_EDGE, False, False, d > 0.0)
|
||||
|
||||
|
||||
def _in_domain(d_px: float, config: StereoConfig) -> bool:
|
||||
return config.disparity_min_px <= d_px <= config.disparity_max_px
|
||||
@@ -1,227 +0,0 @@
|
||||
"""StereoRanger —— 双目稀疏目标测距（规格 docs/双目测距技术路线.md）。
|
||||
|
||||
单周期五步（§5）：① 同帧契约检查 → ② 取点 → ③ 左右匹配 →
|
||||
④ 配对目标：测距 + 方位源 + 有效性 + 滤波 → ⑤ 单目退化发布。
|
||||
|
||||
输入为左右目各自、校正后原生坐标系下的检测框；不做目标识别，不做图像
|
||||
采集/立体校正。纯计算单元（§11.3）：单周期处理无阻塞、无 IO；同一实例
|
||||
不得并发调用 update()。
|
||||
"""
|
||||
|
||||
import logging
|
||||
from typing import List, Optional, Sequence
|
||||
|
||||
from .config import StereoConfig
|
||||
from .geometry import confidence_from_cost, evaluate_validity, solve_range, virtual_center_px
|
||||
from .matching import GreedyMatcher, Matcher, MatchPair
|
||||
from .sampling import SamplePoint, sample_point, select_disparity
|
||||
from .target_store import TargetStore
|
||||
from .types import Degradation, Detection, DetectionFrame, Eye, RangeRecord
|
||||
from .warn_throttle import WarnThrottle
|
||||
|
||||
logger = logging.getLogger("stereo_ranger")
|
||||
|
||||
_CONFIDENCE_DEGRADE_FACTOR = 0.5 # ⚠占位：连续跳变 ≥ N 帧的置信度降级系数（§6.4）
|
||||
|
||||
|
||||
class StereoRanger:
|
||||
"""双目稀疏目标测距器：每帧调用一次 update()，传入同帧左右目检测结果。"""
|
||||
|
||||
def __init__(self, config: Optional[StereoConfig] = None, matcher: Optional[Matcher] = None) -> None:
|
||||
self._config = config if config is not None else StereoConfig()
|
||||
self._matcher = matcher if matcher is not None else GreedyMatcher()
|
||||
self._store = TargetStore(self._config)
|
||||
self._warn = WarnThrottle()
|
||||
self._frame_index = 0
|
||||
|
||||
@property
|
||||
def config(self) -> StereoConfig:
|
||||
return self._config
|
||||
|
||||
@property
|
||||
def matcher(self) -> Matcher:
|
||||
return self._matcher
|
||||
|
||||
def reset(self) -> None:
|
||||
"""清空全部运行期状态（滤波 / 一致性 / 隔离 / 节流计数）。"""
|
||||
self._store.clear()
|
||||
self._warn.clear()
|
||||
self._frame_index = 0
|
||||
|
||||
def update(self, left: DetectionFrame, right: DetectionFrame, now_ms: int = 0) -> List[RangeRecord]:
|
||||
"""单周期处理（§5）：返回本帧记录列表。
|
||||
|
||||
顺序：先按左目框顺序给出各自记录（配对→完整记录 / 未配对→仅方位），
|
||||
再给出未配对的右目框的仅方位记录；左目存在时记录数与左目框数一致。
|
||||
"""
|
||||
config = self._config
|
||||
if left.frame_id != right.frame_id:
|
||||
self._warn_emit(
|
||||
"frame_mismatch",
|
||||
"左右帧标识不一致（L=%s, R=%s），拒收本帧",
|
||||
left.frame_id,
|
||||
right.frame_id,
|
||||
)
|
||||
return []
|
||||
|
||||
self._frame_index += 1
|
||||
frame_index = self._frame_index
|
||||
left_points = self._drop_invalid([sample_point(det) for det in left.detections])
|
||||
right_points = self._drop_invalid([sample_point(det) for det in right.detections])
|
||||
|
||||
pairs = self._matcher.match(left_points, right_points, config)
|
||||
pair_of_left = {pair.left_index: pair for pair in pairs}
|
||||
paired_right = {pair.right_index for pair in pairs}
|
||||
|
||||
records: List[RangeRecord] = []
|
||||
for index, point_l in enumerate(left_points):
|
||||
pair = pair_of_left.get(index)
|
||||
if pair is None:
|
||||
records.append(self._bearing_only_record(point_l, left.frame_id, now_ms, Eye.LEFT))
|
||||
continue
|
||||
records.append(
|
||||
self._complete_record(
|
||||
point_l,
|
||||
right_points[pair.right_index],
|
||||
pair,
|
||||
left.frame_id,
|
||||
now_ms,
|
||||
frame_index,
|
||||
)
|
||||
)
|
||||
for index, point_r in enumerate(right_points):
|
||||
if index in paired_right:
|
||||
continue
|
||||
records.append(self._bearing_only_record(point_r, left.frame_id, now_ms, Eye.RIGHT))
|
||||
|
||||
self._store.end_frame(frame_index)
|
||||
return records
|
||||
|
||||
# ---- 内部实现 ----
|
||||
|
||||
def _complete_record(
|
||||
self,
|
||||
point_l: SamplePoint,
|
||||
point_r: SamplePoint,
|
||||
pair: MatchPair,
|
||||
frame_id: int,
|
||||
now_ms: int,
|
||||
frame_index: int,
|
||||
) -> RangeRecord:
|
||||
"""配对成功的记录：测距 + 方位源 + 有效性（门①②）+ 一致性（门③）+ 滤波。"""
|
||||
config = self._config
|
||||
box_l = point_l.box
|
||||
box_r = point_r.box
|
||||
disparity = select_disparity(point_l, point_r, config)
|
||||
center_mid = virtual_center_px(box_l, box_r)
|
||||
|
||||
if disparity.fallback_used:
|
||||
self._warn_emit("center_fallback", "边缘视差越窗，回退框中心（框宽差可能混入视差）")
|
||||
if not disparity.positive:
|
||||
self._warn_emit("disparity_violation", "视差违例 D=%.2f px（须为正），该目标判无效", disparity.d_px)
|
||||
elif not disparity.in_domain:
|
||||
self._warn_emit(
|
||||
"disparity_violation",
|
||||
"视差超界 D=%.2f px（域 [%.2f, %.2f]），该目标判无效",
|
||||
disparity.d_px,
|
||||
config.disparity_min_px,
|
||||
config.disparity_max_px,
|
||||
)
|
||||
|
||||
key = self._store.resolve(
|
||||
box_l.class_id,
|
||||
_shared_track_id(box_l, box_r),
|
||||
center_mid,
|
||||
frame_index,
|
||||
)
|
||||
status = self._store.consistency.advance(key, disparity.d_px)
|
||||
if status.invalid:
|
||||
self._warn_emit(
|
||||
"consistency_invalid",
|
||||
"连续 %d 帧视差跳变超门限（%.1f px），质量判无效",
|
||||
status.streak,
|
||||
config.consistency_jump_px,
|
||||
)
|
||||
|
||||
z_raw = config.focal_baseline_px_m / disparity.d_px if disparity.positive else None
|
||||
solution = solve_range(box_l, box_r, disparity.d_px, config) if z_raw is not None else None
|
||||
decision = evaluate_validity(disparity.d_px, z_raw, config)
|
||||
valid = decision.valid and not status.invalid
|
||||
|
||||
confidence = confidence_from_cost(pair.cost)
|
||||
if status.confidence_degraded:
|
||||
confidence *= _CONFIDENCE_DEGRADE_FACTOR
|
||||
if status.invalid:
|
||||
confidence = 0.0
|
||||
|
||||
if valid:
|
||||
z_out = self._store.distance_filter.advance(key, z_raw)
|
||||
else:
|
||||
self._store.distance_filter.invalidate(key)
|
||||
z_out = None
|
||||
|
||||
# 参考斜距与已发布的前向距离同基准：X/Y/Z 均 ∝ Z，按 z_out/z_raw 缩放即精确换基
|
||||
slant_range_m = None
|
||||
if solution is not None:
|
||||
slant_range_m = solution.r_m if z_out is None else solution.r_m * (z_out / z_raw)
|
||||
|
||||
return RangeRecord(
|
||||
frame_id=frame_id,
|
||||
class_id=box_l.class_id,
|
||||
degradation=Degradation.COMPLETE,
|
||||
valid=valid,
|
||||
track_id=box_l.track_id if box_l.track_id is not None else box_r.track_id,
|
||||
forward_distance_m=z_out,
|
||||
slant_range_m=slant_range_m,
|
||||
disparity_px=disparity.d_px,
|
||||
center_mid_px=center_mid,
|
||||
reference_box=box_l.bbox,
|
||||
reference_eye=Eye.LEFT,
|
||||
confidence=confidence,
|
||||
timestamp_ms=now_ms,
|
||||
)
|
||||
|
||||
def _bearing_only_record(
|
||||
self,
|
||||
point: SamplePoint,
|
||||
frame_id: int,
|
||||
now_ms: int,
|
||||
eye: Eye,
|
||||
) -> RangeRecord:
|
||||
"""单目退化记录（§5 第 ⑤ 步）：仅方位，valid = false，无距离。"""
|
||||
detection = point.box
|
||||
return RangeRecord(
|
||||
frame_id=frame_id,
|
||||
class_id=detection.class_id,
|
||||
degradation=Degradation.BEARING_ONLY,
|
||||
valid=False,
|
||||
track_id=detection.track_id,
|
||||
center_mid_px=(detection.cx, detection.cy),
|
||||
reference_box=detection.bbox,
|
||||
reference_eye=eye,
|
||||
confidence=detection.confidence,
|
||||
timestamp_ms=now_ms,
|
||||
)
|
||||
|
||||
def _drop_invalid(self, points: Sequence[SamplePoint]) -> List[SamplePoint]:
|
||||
"""丢弃坐标非有限值的框（不可参与匹配，也不发布记录），告警节流。"""
|
||||
kept: List[SamplePoint] = []
|
||||
for point in points:
|
||||
if point.finite:
|
||||
kept.append(point)
|
||||
else:
|
||||
self._warn_emit("invalid_box", "检测框坐标非法（非有限值），已丢弃该框")
|
||||
return kept
|
||||
|
||||
def _warn_emit(self, category: str, message: str, *args: object) -> None:
|
||||
"""异常日志（§5 节流规范：第 1 次 + 每满 100 次，带累计计数）。"""
|
||||
if not self._warn.should_emit(category):
|
||||
return
|
||||
logger.warning("[%s] " + message + "（累计 %d 次）", category, *args, self._warn.count(category))
|
||||
|
||||
|
||||
def _shared_track_id(box_l: Detection, box_r: Detection) -> Optional[int]:
|
||||
"""左右目轨道关联一致且有效时返回该 id（目标隔离键优先路径，§4）。"""
|
||||
if box_l.track_id is not None and box_l.track_id > 0 and box_l.track_id == box_r.track_id:
|
||||
return box_l.track_id
|
||||
return None
|
||||
@@ -1,119 +0,0 @@
|
||||
"""目标状态隔离（§4）：track_id 优先 + 框位置最近邻兜底。
|
||||
|
||||
目标隔离键：track_id 可用（左右一致且 > 0）时优先以 (track, 类别, track_id)
|
||||
为键——上游 target_tracker 已在链路中，规格 §4 已按此冻结；否则回退类别 +
|
||||
框位置最近邻（isolation_gate_px 门限，见包 README「实现决策」）。
|
||||
|
||||
连续 filter_reset_frames 帧未再出现的目标状态整体清理（位置 + 距离滤波 +
|
||||
一致性），避免最近邻把新目标接到陈旧状态上。
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, Hashable, Optional, Tuple
|
||||
|
||||
from .config import StereoConfig
|
||||
from .consistency import ConsistencyMonitor
|
||||
from .distance_filter import DistanceFilter
|
||||
|
||||
_KIND_TRACK = "track"
|
||||
_KIND_NEAREST = "nn"
|
||||
|
||||
|
||||
@dataclass
|
||||
class _TargetState:
|
||||
class_id: int
|
||||
track_id: Optional[int]
|
||||
center_px: Tuple[float, float]
|
||||
last_seen_frame: int
|
||||
|
||||
|
||||
class TargetStore:
|
||||
"""每目标状态容器：隔离键解析 + 帧末老化清理（§11.1 状态清单）。"""
|
||||
|
||||
def __init__(self, config: StereoConfig) -> None:
|
||||
self._config = config
|
||||
self._distance_filter = DistanceFilter(
|
||||
config.smooth_alpha, config.jump_reset_m, config.filter_reset_frames
|
||||
)
|
||||
self._consistency = ConsistencyMonitor(
|
||||
config.consistency_jump_px, config.consistency_N_frames, config.consistency_M_frames
|
||||
)
|
||||
self._states: Dict[Hashable, _TargetState] = {}
|
||||
self._next_nearest_id = 0
|
||||
|
||||
@property
|
||||
def distance_filter(self) -> DistanceFilter:
|
||||
return self._distance_filter
|
||||
|
||||
@property
|
||||
def consistency(self) -> ConsistencyMonitor:
|
||||
return self._consistency
|
||||
|
||||
@property
|
||||
def count(self) -> int:
|
||||
"""当前存活的目标状态数。"""
|
||||
return len(self._states)
|
||||
|
||||
def resolve(
|
||||
self,
|
||||
class_id: int,
|
||||
track_id: Optional[int],
|
||||
center_px: Tuple[float, float],
|
||||
frame_index: int,
|
||||
) -> Hashable:
|
||||
"""解析（或新建）目标 key，并刷新该目标最近位置与出现帧。"""
|
||||
if track_id is not None and track_id > 0:
|
||||
key: Hashable = (_KIND_TRACK, class_id, track_id)
|
||||
self._states[key] = _TargetState(class_id, track_id, center_px, frame_index)
|
||||
return key
|
||||
key = self._nearest_key(class_id, center_px, frame_index)
|
||||
if key is None:
|
||||
self._next_nearest_id += 1
|
||||
key = (_KIND_NEAREST, class_id, self._next_nearest_id)
|
||||
self._states[key] = _TargetState(class_id, None, center_px, frame_index)
|
||||
else:
|
||||
state = self._states[key]
|
||||
state.center_px = center_px
|
||||
state.last_seen_frame = frame_index
|
||||
return key
|
||||
|
||||
def end_frame(self, frame_index: int) -> None:
|
||||
"""帧末收尾：未出现的目标记滤波失效，到期整体清理三类状态。"""
|
||||
stale: List[Hashable] = []
|
||||
for key, state in self._states.items():
|
||||
if state.last_seen_frame == frame_index:
|
||||
continue
|
||||
self._distance_filter.invalidate(key)
|
||||
if frame_index - state.last_seen_frame >= self._config.filter_reset_frames:
|
||||
stale.append(key)
|
||||
for key in stale:
|
||||
del self._states[key]
|
||||
self._distance_filter.drop(key)
|
||||
self._consistency.drop(key)
|
||||
|
||||
def clear(self) -> None:
|
||||
self._states.clear()
|
||||
self._distance_filter.clear()
|
||||
self._consistency.clear()
|
||||
self._next_nearest_id = 0
|
||||
|
||||
def _nearest_key(
|
||||
self,
|
||||
class_id: int,
|
||||
center_px: Tuple[float, float],
|
||||
frame_index: int,
|
||||
) -> Optional[Hashable]:
|
||||
"""同类别、isolation_gate_px 内、本帧尚未被占用的最近状态。"""
|
||||
gate = self._config.isolation_gate_px
|
||||
best_key: Optional[Hashable] = None
|
||||
best_distance: Optional[float] = None
|
||||
for key, state in self._states.items():
|
||||
if key[0] != _KIND_NEAREST or state.class_id != class_id:
|
||||
continue
|
||||
if state.last_seen_frame == frame_index:
|
||||
continue # 本帧已被其他框接管，一一归属
|
||||
distance = math.hypot(center_px[0] - state.center_px[0], center_px[1] - state.center_px[1])
|
||||
if distance <= gate and (best_distance is None or distance < best_distance):
|
||||
best_key, best_distance = key, distance
|
||||
return best_key
|
||||
@@ -1,107 +0,0 @@
|
||||
"""stereo_ranger 输入输出数据结构（纯值语义）。
|
||||
|
||||
规格：docs/双目测距技术路线.md —— §2 输入输出接口、§3 符号约定、§6.6 方位源。
|
||||
|
||||
约定：所有像素坐标位于**已立体校正**的图像坐标系，检测框已映射回该基准
|
||||
（§2.1 校正契约 / 坐标基准一致）。本包仅依赖标准库。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, auto
|
||||
from typing import Optional, Sequence, Tuple
|
||||
|
||||
BBoxXYXY = Tuple[float, float, float, float]
|
||||
|
||||
|
||||
class Eye(Enum):
|
||||
"""物理目（参考目框来源，§2.2）。"""
|
||||
|
||||
LEFT = auto() # 左目（解算参考系）
|
||||
RIGHT = auto() # 右目
|
||||
|
||||
|
||||
class DisparitySource(Enum):
|
||||
"""视差取点来源（§6.1）。"""
|
||||
|
||||
LEFT_EDGE = auto() # 同侧（左）边缘：v1.0 主方案
|
||||
CENTER_FALLBACK = auto() # 边缘视差越窗后回退框中心
|
||||
|
||||
|
||||
class Degradation(Enum):
|
||||
"""记录退化标记（§2.2）。描述记录的来源形态，质量由 valid 单独表达。"""
|
||||
|
||||
COMPLETE = auto() # 完整：配对成功（距离 + 方位两路都在）
|
||||
BEARING_ONLY = auto() # 仅方位：单目检出，无距离
|
||||
|
||||
|
||||
@dataclass
|
||||
class Detection:
|
||||
"""校正后图像坐标系下的一个检测框（px；x1/y1 左上角，x2/y2 右下角）。"""
|
||||
|
||||
x1: float
|
||||
y1: float
|
||||
x2: float
|
||||
y2: float
|
||||
confidence: float = 1.0
|
||||
class_id: int = 0
|
||||
track_id: Optional[int] = None # 检测侧轨道关联；None = 无（§2.2，v1.0 可无）
|
||||
|
||||
@property
|
||||
def width(self) -> float:
|
||||
return self.x2 - self.x1
|
||||
|
||||
@property
|
||||
def height(self) -> float:
|
||||
return self.y2 - self.y1
|
||||
|
||||
@property
|
||||
def cx(self) -> float:
|
||||
return (self.x1 + self.x2) / 2.0
|
||||
|
||||
@property
|
||||
def cy(self) -> float:
|
||||
return (self.y1 + self.y2) / 2.0
|
||||
|
||||
@property
|
||||
def bbox(self) -> BBoxXYXY:
|
||||
return (self.x1, self.y1, self.x2, self.y2)
|
||||
|
||||
|
||||
@dataclass
|
||||
class DetectionFrame:
|
||||
"""单目一帧：帧标识 + 检测框列表。
|
||||
|
||||
同帧契约（§2.1）：左右两帧的 frame_id 必须一致，否则整帧拒收、输出空。
|
||||
"""
|
||||
|
||||
frame_id: int
|
||||
detections: Sequence[Detection] = field(default_factory=tuple)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RangeRecord:
|
||||
"""目标距离记录（§2.2，字段与文档表格逐项对应）。
|
||||
|
||||
- forward_distance_m（Z）：唯一供距离通道消费的量；valid=False 或单目
|
||||
退化时为 None（不得强行估计，§12.3 红线 3）；
|
||||
- slant_range_m（R）：参考值，不进控制回路；无效但仍可解算时保留原始
|
||||
测量值供诊断；
|
||||
- disparity_px（D）：本次视差（调试与交叉校验用）；
|
||||
- center_mid_px（c_mid）：方位源；配对记录为两目框中心等权平均（虚拟
|
||||
双目中心，§6.6），单目退化为该目框中心；
|
||||
- reference_box / reference_eye：参考目框（配对记录恒为左目）。
|
||||
"""
|
||||
|
||||
frame_id: int
|
||||
class_id: int
|
||||
degradation: Degradation
|
||||
valid: bool
|
||||
track_id: Optional[int] = None
|
||||
forward_distance_m: Optional[float] = None
|
||||
slant_range_m: Optional[float] = None
|
||||
disparity_px: Optional[float] = None
|
||||
center_mid_px: Tuple[float, float] = (0.0, 0.0)
|
||||
reference_box: Optional[BBoxXYXY] = None
|
||||
reference_eye: Eye = Eye.LEFT
|
||||
confidence: float = 0.0
|
||||
timestamp_ms: int = 0
|
||||
@@ -1,30 +0,0 @@
|
||||
"""告警节流器（§4/§5）：每类异常"第 1 次 + 每满 100 次"打印，带累计计数。
|
||||
|
||||
高频热路径（每帧数学）不打日志；所有异常日志走本通道，且不落在数学热路径
|
||||
的决策上（§11.3 单周期处理无阻塞、无 IO）。
|
||||
"""
|
||||
|
||||
from typing import Dict
|
||||
|
||||
|
||||
class WarnThrottle:
|
||||
"""按异常类独立计数与限频。"""
|
||||
|
||||
def __init__(self, every: int = 100) -> None:
|
||||
if every < 1:
|
||||
raise ValueError("every 必须 ≥ 1")
|
||||
self._every = every
|
||||
self._counts: Dict[str, int] = {}
|
||||
|
||||
def should_emit(self, category: str) -> bool:
|
||||
"""推进该类异常计数并返回本次是否应打印（第 1 次 + 每满 every 次）。"""
|
||||
count = self._counts.get(category, 0) + 1
|
||||
self._counts[category] = count
|
||||
return count == 1 or count % self._every == 0
|
||||
|
||||
def count(self, category: str) -> int:
|
||||
"""该类异常的累计次数。"""
|
||||
return self._counts.get(category, 0)
|
||||
|
||||
def clear(self) -> None:
|
||||
self._counts.clear()
|
||||
@@ -1,109 +0,0 @@
|
||||
# stereo_yolo（Python 版）
|
||||
|
||||
双目 YOLO 检测前端：在左右目图像上**独立运行** YOLO 检测器，输出符合
|
||||
`stereo_ranger` 输入契约的检测框（每目一个 `DetectionFrame`、共用 frame_id）。
|
||||
不做测距、左右目关联与立体校正。
|
||||
|
||||
规格：`docs/双目YOLO目标识别需求说明.md`（接口契约以其为准；几何语义权威
|
||||
`docs/双目测距技术路线.md`）。
|
||||
|
||||
⚠ 全部参数为占位值、demo 素材为 AI 合成图：标定与地面真值验证完成前，
|
||||
仅限台架/回放验证，不得接入实飞控制回路（规格 §7 红线同源）。
|
||||
|
||||
## 接口清单
|
||||
|
||||
| 名称 | 说明 |
|
||||
| --- | --- |
|
||||
| `StereoYoloFrontend(config, detector=None)` | 主入口；`detector` 可注入（接口 `detect(img) -> list[Detection]`，单测用假检测器） |
|
||||
| `.detect(left_img, right_img, frame_id) -> (DetectionFrame, DetectionFrame)` | 单帧检测；BGR ndarray（cv2 约定），输出框位于输入图原生坐标系 |
|
||||
| `.close()` | 释放并发线程池 |
|
||||
| `.config` | 只读配置访问 |
|
||||
| `StereoYoloConfig` / `load_from_json(path)` | 7 项配置；缺项/未知字段/类型错误/非法取值一律 `ValueError`（fail-fast） |
|
||||
| `UltralyticsDetector` | ultralytics YOLO `.pt` 后端（双实例池；`ultralytics` 延迟导入） |
|
||||
| `split_sbs(image) -> (left, right)` | side-by-side 单流裁切（宽须为偶数） |
|
||||
| `Detection` / `DetectionFrame` | 直接复用 `stereo_ranger.types`（纯值语义，§2.1 推荐） |
|
||||
| `WarnThrottle` | 告警节流（第 1 次 + 每满 100 次，带累计计数） |
|
||||
|
||||
`logger` 名称为 `stereo_yolo`，走标准 `logging`。
|
||||
|
||||
## 单帧流程（`detect` 四步）
|
||||
|
||||
1. **形状一致性**：左右图形状不一致 → 整帧拒收输出空 + WARN 节流
|
||||
（§2.2 第 2 条：禁止左右各自缩放/填充）；
|
||||
2. **左右推理**：`parallel`（默认，ThreadPoolExecutor(2)，§5 硬性要求）或
|
||||
`sequential`（基准对照）；单目异常被隔离，该目输出空帧列表（§5 失败处理）；
|
||||
3. **出口自检 + 白名单**：丢弃非有限值 / `x2<=x1` / `y2<=y1` 的框
|
||||
（§2.2 第 6 条）；`class_whitelist` 非空时过滤类别；
|
||||
4. **封装**：每目一个 `DetectionFrame(frame_id, detections)`，`frame_id`
|
||||
左右共用（§2.3 同帧契约）。
|
||||
|
||||
## 实现决策（v1.0 冻结）
|
||||
|
||||
1. **ultralytics 直跑 `.pt`**（项目拍板口径）：letterbox、输出解码、NMS、
|
||||
框映射回原生坐标系全部由库内完成；识别端只负责阈值外置、出口自检与
|
||||
契约封装。
|
||||
2. **双实例池**：ultralytics 单实例非线程安全，左右并发各持一份模型
|
||||
（§4 并发策略的"双实例"形态）。
|
||||
3. **无跨帧状态**：前端不持有任何历史（无追踪、无平滑、无 Coast）——
|
||||
结构性满足"只送本帧原生实测框"（§2.5 第 1 条）。
|
||||
4. **形状不一致不 raise**：按"拒收 + 空帧 + 节流 WARN"处理，与 stereo_ranger
|
||||
非同帧拒收同哲学（不阻塞整体）。
|
||||
5. **几何参数单一来源**：`stereo_yolo.json` 不存内参/原分辨率（输出框天然在
|
||||
输入图坐标系），F/B/cx/cy 唯一来源是 `utils/config/stereo_ranger.json`，
|
||||
避免两处漂移（§4 配置一致性）。
|
||||
6. **`track_id` 留 `None`**：外部追踪器注入属链路后段（§2.1；v1.1 可选）。
|
||||
|
||||
## 移植注记（C++ 侧需自行复现的库内行为）
|
||||
|
||||
本次为 ultralytics 直跑口径，以下行为在库内完成、C++ 实现必须逐项复现
|
||||
（不一致会在视差取点上引入系统性偏差）：
|
||||
|
||||
| 库内行为 | C++ 需复现要点 |
|
||||
| --- | --- |
|
||||
| letterbox | 等比缩放到 `imgsz`、灰边填充（ultralytics 预测默认填充值 114）；左右同一套参数（形状一致时天然满足） |
|
||||
| 输出解码 | YOLO26 检测头输出布局与解码（建议导出一份 ONNX 作为参照物；本仓环境：ultralytics 8.4 / torch 2.11 / onnx 1.18） |
|
||||
| NMS | `conf` 先过滤、NMS 用配置 IoU 门限；`max_det` 默认 300 |
|
||||
| 坐标回映 | letterbox 逆变换（scale/pad）到原生分辨率；输出 `x1,y1,x2,y2` 浮点 |
|
||||
| 并发 | 双实例双线程（单实例并发调用不安全） |
|
||||
|
||||
## 配置项（全部外置，占位值）
|
||||
|
||||
| 键 | 占位值 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| `model_path` | `models/yolo26n-drone.pt` | 相对路径按启动工作目录解析（demo 内按仓库根兜底） |
|
||||
| `imgsz` | 640 | 须为 32 的整数倍（YOLO 步长约束） |
|
||||
| `conf_threshold` | 0.25 | 识别端自身过滤口径（测距端不消费检测置信度，§2.4） |
|
||||
| `iou_threshold` | 0.7 | NMS IoU 门限 |
|
||||
| `class_whitelist` | `null` | 可选白名单；同一目标左右目类别必须一致（§2.2 第 5 条） |
|
||||
| `device` | `null` | null = 自动选择（`cpu` / `cuda:0` / …） |
|
||||
| `concurrency` | `parallel` | `parallel` / `sequential` |
|
||||
|
||||
## 测试与演示
|
||||
|
||||
```bash
|
||||
cd utils
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest tests/stereo_yolo -q # 36 条
|
||||
python utils/demo_stereo_yolo.py # 真模型 + 占位素材
|
||||
```
|
||||
|
||||
单测全部用注入的假检测器驱动（无模型、无素材、无硬件）：frame_id 左右共用、
|
||||
出口自检丢弃退化框、白名单过滤、单目失败隔离、形状不一致拒收、双人屏障
|
||||
断言真并发（串行实现会屏障超时）。
|
||||
|
||||
demo（真模型 + AI 合成素材 `utils/assets/stereo_yolo/drone_sky.png`）：
|
||||
① 单图冒烟 → ② 合成双目对（同一 patch 羽化粘贴、已知视差 12 px，实测
|
||||
|ΔD|≈0.3 px、|Δy|≈0.2 px）→ 真检测框喂 `StereoRanger.update()`，记录值与
|
||||
解析值 `Z=F·B/D` 同量级 → ③ 空场景（正常空帧不告警）→ ④ 形状不一致拒收
|
||||
→ ⑤ 帧周期实测（台架开发机 ≈34 ms/帧，占位）。替换素材文件即可换真实图片。
|
||||
|
||||
## 验收对照（规格 §6）
|
||||
|
||||
| 项 | 状态 |
|
||||
| --- | --- |
|
||||
| 坐标回映误差 <0.5 px | 由库内完成，无法构件级单测；以合成对整体校验（|ΔD|≈0.3 px @1080p，含检测边缘噪声）；C++ 移植后需实测 |
|
||||
| 垂直对齐 P95 ≤1 px | 合成对实测 |Δy|≈0.2 px（合成口径，非真实校正质量）；待真实素材 |
|
||||
| 类别一致性 | 模型单类 `Drone`，合成对天然一致；待真实素材 |
|
||||
| 输出合法性 | ✓ 单测（非有限值/退化框构造用例） |
|
||||
| 输入纯净性 | ✓ 结构性保证（前端无跨帧状态） |
|
||||
| 下游对接 | ✓ demo 场景②；δZ ∝ Z² 趋势由测距端单测覆盖 |
|
||||
| 帧周期 | △ 台架开发机实测 ≈34 ms/帧（30 fps 预算 33.3 ms）；待平台实测（P7） |
|
||||
@@ -1,20 +0,0 @@
|
||||
"""stereo_yolo —— 双目 YOLO 检测前端（规格：docs/双目YOLO目标识别需求说明.md）。
|
||||
|
||||
在左右目图像上独立运行 YOLO 检测器，输出**符合 stereo_ranger 输入契约**的
|
||||
检测框列表（每目一个 DetectionFrame）；不做测距、左右目关联与立体校正。
|
||||
⚠ 全部参数为占位值：标定与地面验证完成前仅限台架/回放验证，不得接入实飞
|
||||
控制回路（需求说明 §7 红线同源）。
|
||||
"""
|
||||
|
||||
from .config import StereoYoloConfig, load_from_json
|
||||
from .detector import UltralyticsDetector
|
||||
from .frontend import StereoYoloFrontend
|
||||
from .sbs import split_sbs
|
||||
|
||||
__all__ = [
|
||||
"StereoYoloConfig",
|
||||
"StereoYoloFrontend",
|
||||
"UltralyticsDetector",
|
||||
"load_from_json",
|
||||
"split_sbs",
|
||||
]
|
||||
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,86 +0,0 @@
|
||||
"""StereoYoloConfig 与 JSON 配置加载（需求说明 §4 配置项需求）。
|
||||
|
||||
全部配置外置、构造期一次加载并校验：缺项、未知字段、类型错误、非法取值
|
||||
一律拒绝进入运行期（fail-fast）。几何参数（F/B/cx/cy/原分辨率）不在此
|
||||
重复存储——输出框位于**输入图像自身的原生坐标系**，几何参数的唯一来源
|
||||
是 utils/config/stereo_ranger.json（§4 配置一致性要求：同一配置源驱动，
|
||||
杜绝两处漂移）。
|
||||
"""
|
||||
|
||||
import json
|
||||
from dataclasses import dataclass, fields
|
||||
from typing import Any, Dict, Optional, Tuple
|
||||
|
||||
_CONCURRENCY_MODES = ("parallel", "sequential")
|
||||
_INT_FIELDS = frozenset({"imgsz"})
|
||||
_STR_FIELDS = frozenset({"model_path", "concurrency"})
|
||||
|
||||
|
||||
@dataclass
|
||||
class StereoYoloConfig:
|
||||
model_path: str = "models/yolo26n-drone.pt"
|
||||
imgsz: int = 640 # 左右共用（§2.2 第 2 条：同一缩放、同一填充）
|
||||
conf_threshold: float = 0.25 # ⚠占位：识别端自身过滤口径（§2.4）
|
||||
iou_threshold: float = 0.7 # ⚠占位：NMS IoU 门限
|
||||
class_whitelist: Optional[Tuple[int, ...]] = None # None = 不过滤（§4 可选白名单）
|
||||
device: Optional[str] = None # None = 自动选择（cpu / cuda:0 / …）
|
||||
concurrency: str = "parallel" # parallel = 左右并发（§5 硬性要求，串行会吃掉全部预算）
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
_require(bool(self.model_path.strip()), "model_path", "不得为空")
|
||||
_require(
|
||||
self.imgsz >= 32 and self.imgsz % 32 == 0,
|
||||
"imgsz",
|
||||
"须为 32 的整数倍且 ≥ 32（YOLO 步长约束）",
|
||||
)
|
||||
_require(0.0 <= self.conf_threshold <= 1.0, "conf_threshold", "须落在 [0, 1]")
|
||||
_require(0.0 < self.iou_threshold <= 1.0, "iou_threshold", "须落在 (0, 1]")
|
||||
_require(self.concurrency in _CONCURRENCY_MODES, "concurrency", f"须为 {_CONCURRENCY_MODES} 之一")
|
||||
_require(self.device is None or bool(self.device.strip()), "device", "须为非空字符串或 null")
|
||||
if self.class_whitelist is not None:
|
||||
ids = tuple(self.class_whitelist)
|
||||
_require(
|
||||
len(ids) > 0 and all(isinstance(item, int) and not isinstance(item, bool) and item >= 0 for item in ids),
|
||||
"class_whitelist",
|
||||
"须为非空非负整数列表或 null",
|
||||
)
|
||||
self.class_whitelist = ids
|
||||
|
||||
|
||||
def load_from_json(path: str) -> StereoYoloConfig:
|
||||
"""从 JSON 文件构建配置；缺项/未知字段/类型错误/非法取值一律抛 ValueError。"""
|
||||
with open(path, encoding="utf-8") as handle:
|
||||
data: Dict[str, Any] = json.load(handle)
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError("配置非法: 顶层须为 JSON 对象")
|
||||
known = {item.name for item in fields(StereoYoloConfig)}
|
||||
unknown = sorted(set(data) - known)
|
||||
if unknown:
|
||||
raise ValueError(f"未知配置字段: {unknown}")
|
||||
missing = sorted(known - set(data))
|
||||
if missing:
|
||||
raise ValueError(f"缺少配置字段: {missing}")
|
||||
for name, value in data.items():
|
||||
if name == "class_whitelist":
|
||||
if value is not None and not (
|
||||
isinstance(value, list)
|
||||
and all(isinstance(item, int) and not isinstance(item, bool) for item in value)
|
||||
):
|
||||
raise ValueError("配置类型非法: class_whitelist 须为整数数组或 null")
|
||||
elif name == "device":
|
||||
if value is not None and not isinstance(value, str):
|
||||
raise ValueError("配置类型非法: device 须为字符串或 null")
|
||||
elif name in _STR_FIELDS:
|
||||
if not isinstance(value, str):
|
||||
raise ValueError(f"配置类型非法: {name} 须为字符串")
|
||||
elif name in _INT_FIELDS:
|
||||
if not isinstance(value, int) or isinstance(value, bool):
|
||||
raise ValueError(f"配置类型非法: {name} 须为整数")
|
||||
elif isinstance(value, bool) or not isinstance(value, (int, float)):
|
||||
raise ValueError(f"配置类型非法: {name} 须为数值")
|
||||
return StereoYoloConfig(**data)
|
||||
|
||||
|
||||
def _require(condition: bool, field_name: str, message: str) -> None:
|
||||
if not condition:
|
||||
raise ValueError(f"配置非法: {field_name} {message}")
|
||||
@@ -1,78 +0,0 @@
|
||||
"""UltralyticsDetector —— ultralytics YOLO .pt 直载的检测后端。
|
||||
|
||||
⚠ 移植注记（C++ 侧需自行复现，详见 README）：letterbox（缩放 + 填充到
|
||||
imgsz）、输出张量解码、NMS、框映射回输入图原生坐标系，本例由 ultralytics
|
||||
库内完成；移植时必须逐项复现同一几何（步长对齐、填充方式、IoU 门限、
|
||||
原生坐标回映），否则视差取点会引入系统性偏差。
|
||||
"""
|
||||
|
||||
import queue
|
||||
from pathlib import Path
|
||||
from typing import List
|
||||
|
||||
from stereo_ranger.types import Detection
|
||||
|
||||
from .config import StereoYoloConfig
|
||||
|
||||
|
||||
class UltralyticsDetector:
|
||||
"""双实例池：ultralytics 单实例非线程安全，左右并发各持一份（§3/§4）。
|
||||
|
||||
接口约定（可注入替换）：``detect(image) -> list[Detection]``，
|
||||
image 为 BGR ndarray（cv2 约定），返回框位于该图原生坐标系。
|
||||
"""
|
||||
|
||||
def __init__(self, config: StereoYoloConfig) -> None:
|
||||
self._cfg = config
|
||||
self._model_path = _resolve_model_path(config.model_path)
|
||||
self._pool: "queue.SimpleQueue" = queue.SimpleQueue()
|
||||
from ultralytics import YOLO # 延迟导入：仅真实推理付加载成本，单测注入假检测器（§6）
|
||||
|
||||
for _ in range(2):
|
||||
self._pool.put(YOLO(str(self._model_path)))
|
||||
|
||||
def detect(self, image) -> List[Detection]:
|
||||
model = self._pool.get()
|
||||
try:
|
||||
results = model.predict(
|
||||
source=image,
|
||||
imgsz=self._cfg.imgsz,
|
||||
conf=self._cfg.conf_threshold,
|
||||
iou=self._cfg.iou_threshold,
|
||||
device=self._cfg.device,
|
||||
verbose=False,
|
||||
)
|
||||
finally:
|
||||
self._pool.put(model)
|
||||
if not results:
|
||||
return []
|
||||
return _result_to_detections(results[0])
|
||||
|
||||
|
||||
def _result_to_detections(result) -> List[Detection]:
|
||||
boxes = getattr(result, "boxes", None)
|
||||
if boxes is None or len(boxes) == 0:
|
||||
return []
|
||||
xyxy = boxes.xyxy.cpu().numpy()
|
||||
conf = boxes.conf.cpu().numpy()
|
||||
cls = boxes.cls.cpu().numpy()
|
||||
return [
|
||||
Detection(
|
||||
float(box[0]),
|
||||
float(box[1]),
|
||||
float(box[2]),
|
||||
float(box[3]),
|
||||
confidence=float(score),
|
||||
class_id=int(class_id),
|
||||
)
|
||||
for box, score, class_id in zip(xyxy, conf, cls)
|
||||
]
|
||||
|
||||
|
||||
def _resolve_model_path(raw: str) -> Path:
|
||||
path = Path(raw).expanduser()
|
||||
if not path.is_absolute():
|
||||
path = Path.cwd() / path
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f"模型权重不存在: {path}（model_path 相对路径按启动工作目录解析）")
|
||||
return path.resolve()
|
||||
@@ -1,113 +0,0 @@
|
||||
"""StereoYoloFrontend —— 双目检测前端主类（需求说明 §2/§3）。
|
||||
|
||||
单帧流程：左右图（已校正、原生分辨率）→ 左右并发检测 → 出口自检 + 类别
|
||||
白名单 → 每目封装 DetectionFrame（共用 frame_id，§2.3 同帧契约）。
|
||||
|
||||
前端**无跨帧状态**：没有历史就没有外推，结构性满足"只送本帧原生实测框"
|
||||
的输入纯净性硬约束（§2.5 第 1 条）。
|
||||
"""
|
||||
|
||||
import logging
|
||||
import math
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
from typing import List, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from stereo_ranger.types import Detection, DetectionFrame
|
||||
|
||||
from .config import StereoYoloConfig
|
||||
from .detector import UltralyticsDetector
|
||||
from .warn_throttle import WarnThrottle
|
||||
|
||||
logger = logging.getLogger("stereo_yolo")
|
||||
|
||||
_EYE_LABELS = ("左目", "右目")
|
||||
|
||||
|
||||
class StereoYoloFrontend:
|
||||
"""图像 → 符合 stereo_ranger 输入契约的检测框（§2 接口契约）。
|
||||
|
||||
``detector`` 可注入替换：接口为 ``detect(img) -> list[Detection]``
|
||||
（单测用假检测器驱动，§6 输出合法性/失败隔离用例无需真实模型）。
|
||||
"""
|
||||
|
||||
def __init__(self, config: StereoYoloConfig, detector: Optional[object] = None) -> None:
|
||||
self._cfg = config
|
||||
self._detector = detector if detector is not None else UltralyticsDetector(config)
|
||||
self._throttle = WarnThrottle()
|
||||
self._executor = (
|
||||
ThreadPoolExecutor(max_workers=2, thread_name_prefix="stereo_yolo")
|
||||
if config.concurrency == "parallel"
|
||||
else None
|
||||
)
|
||||
|
||||
@property
|
||||
def config(self) -> StereoYoloConfig:
|
||||
return self._cfg
|
||||
|
||||
def detect(
|
||||
self,
|
||||
left_img: np.ndarray,
|
||||
right_img: np.ndarray,
|
||||
frame_id: int,
|
||||
) -> Tuple[DetectionFrame, DetectionFrame]:
|
||||
"""左右各输出一个 DetectionFrame；两帧共用同一 frame_id（§2.3）。"""
|
||||
if left_img.shape != right_img.shape:
|
||||
self._warn(
|
||||
"shape_mismatch",
|
||||
f"左右图尺寸不一致（{left_img.shape} vs {right_img.shape}），整帧拒收输出空（§2.2 第 2 条）",
|
||||
)
|
||||
return DetectionFrame(frame_id, ()), DetectionFrame(frame_id, ())
|
||||
|
||||
if self._executor is not None:
|
||||
left_future = self._executor.submit(self._detect_eye, left_img, _EYE_LABELS[0])
|
||||
right_future = self._executor.submit(self._detect_eye, right_img, _EYE_LABELS[1])
|
||||
left, right = left_future.result(), right_future.result()
|
||||
else:
|
||||
left = self._detect_eye(left_img, _EYE_LABELS[0])
|
||||
right = self._detect_eye(right_img, _EYE_LABELS[1])
|
||||
|
||||
return DetectionFrame(frame_id, tuple(left)), DetectionFrame(frame_id, tuple(right))
|
||||
|
||||
def close(self) -> None:
|
||||
"""释放并发线程池（前端不再使用时调用）。"""
|
||||
if self._executor is not None:
|
||||
self._executor.shutdown(wait=True)
|
||||
|
||||
# ---------------- 内部 ----------------
|
||||
|
||||
def _detect_eye(self, image: np.ndarray, eye_label: str) -> List[Detection]:
|
||||
try:
|
||||
raw = self._detector.detect(image)
|
||||
except Exception as exc: # 单目失败隔离：不阻塞整体，该目输出空帧列表（§5 失败处理）
|
||||
self._warn(f"infer_failed:{eye_label}", f"{eye_label}推理失败，该目输出空帧列表：{exc!r}")
|
||||
return []
|
||||
return self._sanitize(raw)
|
||||
|
||||
def _sanitize(self, detections: Sequence[Detection]) -> List[Detection]:
|
||||
"""出口自检（§2.2 第 6 条）：丢弃非有限值 / x2<=x1 / y2<=y1 的框；白名单过滤。"""
|
||||
kept: List[Detection] = []
|
||||
dropped = 0
|
||||
whitelist = self._cfg.class_whitelist
|
||||
for detection in detections:
|
||||
if not _is_valid_box(detection):
|
||||
dropped += 1
|
||||
continue
|
||||
if whitelist is not None and detection.class_id not in whitelist:
|
||||
continue
|
||||
kept.append(detection)
|
||||
if dropped:
|
||||
self._warn("degenerate_box", f"丢弃退化检测框 {dropped} 个（非有限值或 x2<=x1/y2<=y1）")
|
||||
return kept
|
||||
|
||||
def _warn(self, category: str, message: str) -> None:
|
||||
if self._throttle.should_emit(category):
|
||||
logger.warning("%s（累计 %d 次）", message, self._throttle.count(category))
|
||||
|
||||
|
||||
def _is_valid_box(detection: Detection) -> bool:
|
||||
values = (detection.x1, detection.y1, detection.x2, detection.y2)
|
||||
if not all(math.isfinite(value) for value in values):
|
||||
return False
|
||||
return detection.x2 > detection.x1 and detection.y2 > detection.y1
|
||||
@@ -1,16 +0,0 @@
|
||||
"""side-by-side 单流裁切（需求说明 §2.3：同帧拼接形态推荐优先支持）。
|
||||
|
||||
解码一次、裁切为左右图，天然同曝光、天然同帧；宽须为偶数。
|
||||
"""
|
||||
|
||||
from typing import Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def split_sbs(image: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""左右各半裁切，返回 (左图, 右图)。"""
|
||||
if image.ndim < 2 or image.shape[1] % 2 != 0:
|
||||
raise ValueError(f"side-by-side 图像宽度须为偶数，实际 shape={image.shape}")
|
||||
half = int(image.shape[1] // 2)
|
||||
return image[:, :half].copy(), image[:, half:].copy()
|
||||
@@ -1,26 +0,0 @@
|
||||
"""告警节流器（需求说明 §5 失败处理）：每类异常"第 1 次 + 每满 100 次"打印。
|
||||
|
||||
语义与 stereo_ranger 的节流器一致，但按前端/测距解耦要求（§7）不跨包复用。
|
||||
"""
|
||||
|
||||
from typing import Dict
|
||||
|
||||
|
||||
class WarnThrottle:
|
||||
"""按异常类独立计数与限频。"""
|
||||
|
||||
def __init__(self, every: int = 100) -> None:
|
||||
if every < 1:
|
||||
raise ValueError("every 必须 ≥ 1")
|
||||
self._every = every
|
||||
self._counts: Dict[str, int] = {}
|
||||
|
||||
def should_emit(self, category: str) -> bool:
|
||||
"""推进该类异常计数并返回本次是否应打印（第 1 次 + 每满 every 次）。"""
|
||||
count = self._counts.get(category, 0) + 1
|
||||
self._counts[category] = count
|
||||
return count == 1 or count % self._every == 0
|
||||
|
||||
def count(self, category: str) -> int:
|
||||
"""该类异常的累计次数。"""
|
||||
return self._counts.get(category, 0)
|
||||
@@ -1,29 +0,0 @@
|
||||
# target_tracker（Python 版）
|
||||
|
||||
多目标追踪过滤器：8 维恒速卡尔曼滤波 + 贪婪关联 + 主目标锁定选举。
|
||||
移植自 `reference-c++/target_tracker/`（规格：docs/视觉追踪技术路线.md）。
|
||||
|
||||
## 接口清单
|
||||
|
||||
| 名称 | 说明 |
|
||||
| --- | --- |
|
||||
| `Detection(x1, y1, x2, y2, conf, cls_id=0)` | 单帧检测框输入 |
|
||||
| `FrameShape(height, width)` | 画面尺寸（仅 Coast 兜底选举取画面中心用） |
|
||||
| `TrackResult` | 每帧输出九字段：tracked / primary_id / center / box / confidence / is_predicted / lost_frames / n_active / raw |
|
||||
| `TrackerConfig(max_lost_frames=8, max_association_dist=200.0, min_hits=3, dist_weight=0.5, iou_weight=0.5)` | 追踪器配置 |
|
||||
| `TargetTracker(config=None)` | 追踪器主类 |
|
||||
| `.update(detections, frame_shape=None) -> TrackResult` | 单帧主流程（§7 六步） |
|
||||
| `.last_result() -> TrackResult` | 跨线程读取最近一帧结果（加锁拷贝） |
|
||||
| `.reset()` / `.active_count()` / `.active_tracks()` / `.get_config()` | 生命周期与查询 |
|
||||
| `KalmanBoxFilter` / `BoxCXCYWH` | 单目标滤波器（可独立使用：initiate / predict / update） |
|
||||
| `Tracklet` / `BoxXYXY` / `to_center_size` / `to_xyxy` / `compute_iou` | 轨道与几何工具 |
|
||||
|
||||
## 与 C++ 版的差异
|
||||
|
||||
- **依赖**：矩阵运算用 numpy（C++ 手写定长运算避免 Eigen）；关联/选举路径用纯标量，与 C++ 逐位同序。
|
||||
- **Optional 语义**：C++ `std::optional<T>` → Python `None`；`State()` → `state` 属性（返回拷贝）。
|
||||
- **线程模型**：与 C++ 一致——仅 `last_result()` 加 `threading.Lock`，`update()` 单线程调用。
|
||||
- **字段命名**：成员函数蛇形命名（`IsDead` → `is_dead` 属性，`MarkMiss` → `mark_miss`）。
|
||||
- 行为语义（死亡判定 `lost_count > max_lost` 严格大于、best_cost=1.0 严格小于、
|
||||
ID 严格递增不复用、Q 用状态原值 / R 用 abs×2 的刻意不对称、S 求逆前 1e-9 正则化）
|
||||
与 C++ 完全一致，由 27 条移植测试锁定。
|
||||
@@ -1,15 +0,0 @@
|
||||
"""target_tracker —— 轻量级多目标追踪过滤器（C++ 版 reference-c++/target_tracker 的 Python 移植）。
|
||||
|
||||
纯算法包，零第三方运行依赖之外的约束：仅依赖 numpy，不依赖 visual_track_control。
|
||||
"""
|
||||
|
||||
from .kalman_box_filter import STD_POS, STD_VEL, INI_POS, INI_VEL, R_SCALE, BoxCXCYWH, KalmanBoxFilter
|
||||
from .target_tracker import Detection, FrameShape, TargetTracker, TrackerConfig, TrackResult
|
||||
from .tracklet import BoxXYXY, Tracklet, compute_iou, to_center_size, to_xyxy
|
||||
|
||||
__all__ = [
|
||||
"STD_POS", "STD_VEL", "INI_POS", "INI_VEL", "R_SCALE",
|
||||
"BoxCXCYWH", "BoxXYXY", "KalmanBoxFilter", "Tracklet",
|
||||
"Detection", "FrameShape", "TargetTracker", "TrackerConfig", "TrackResult",
|
||||
"compute_iou", "to_center_size", "to_xyxy",
|
||||
]
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,111 +0,0 @@
|
||||
"""单目标 8 维恒定速度卡尔曼滤波器。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/target_tracker（规格：docs/视觉追踪技术路线.md §4）：
|
||||
状态 x[8] = [cx, cy, w, h, vx, vy, vw, vh]，观测 z[4] = [cx, cy, w, h]，dt = 1 帧。
|
||||
矩阵运算用 numpy 表达（C++ 版为避免 Eigen 手写定长运算，此处等价）。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
# §4.2 / §8.2 内部常量（经验值，针对 640×480 画面调优）
|
||||
STD_POS = 1.0 / 50.0 # 位置噪声比例系数
|
||||
STD_VEL = 1.0 / 200.0 # 速度噪声比例系数
|
||||
INI_POS = 2.0 # 初始位置协方差
|
||||
INI_VEL = 100.0 # 初始速度协方差
|
||||
R_SCALE = 2.0 # R 额外缩放因子（增强平滑）
|
||||
_S_REG = 1e-9 # S 求逆前的对角正则化（§9.3）
|
||||
|
||||
# 状态转移矩阵 F（§4.1）：I4 + 右上 I4
|
||||
_F = np.eye(8, dtype=float)
|
||||
_F[0:4, 4:8] = np.eye(4)
|
||||
|
||||
# 观测矩阵 H（§4.1）：[I4 | 0]，直接观测位置与尺寸、不观测速度
|
||||
_H_COLS = slice(0, 4)
|
||||
|
||||
|
||||
@dataclass
|
||||
class BoxCXCYWH:
|
||||
"""轴对齐边界框（中心点 + 宽高表示，卡尔曼观测空间）。"""
|
||||
|
||||
cx: float = 0.0
|
||||
cy: float = 0.0
|
||||
w: float = 0.0
|
||||
h: float = 0.0
|
||||
|
||||
|
||||
class KalmanBoxFilter:
|
||||
def __init__(self) -> None:
|
||||
self._x = np.zeros(8, dtype=float)
|
||||
self._p = np.zeros((8, 8), dtype=float)
|
||||
self._initialized = False
|
||||
|
||||
@property
|
||||
def initialized(self) -> bool:
|
||||
return self._initialized
|
||||
|
||||
@property
|
||||
def state(self) -> np.ndarray:
|
||||
"""状态向量 [cx, cy, w, h, vx, vy, vw, vh]（拷贝）。"""
|
||||
return self._x.copy()
|
||||
|
||||
def initiate(self, z: BoxCXCYWH) -> None:
|
||||
# §4.3：速度置零；P = diag(2,2,2,2,100,100,100,100)
|
||||
self._x = np.array([z.cx, z.cy, z.w, z.h, 0.0, 0.0, 0.0, 0.0])
|
||||
self._p = np.diag([INI_POS] * 4 + [INI_VEL] * 4)
|
||||
self._initialized = True
|
||||
|
||||
def predict(self) -> BoxCXCYWH | None:
|
||||
# §4.4：未初始化返回 None
|
||||
if not self._initialized:
|
||||
return None
|
||||
|
||||
# §4.2 动态过程噪声：Q = diag(σ_pos², σ_vel²)，σ 按状态中 w/h 缩放。
|
||||
# Q 直接使用状态值、不做 abs，与 R 的 abs 处理刻意不同（规格 §4.2 注记）。
|
||||
w = self._x[2]
|
||||
h = self._x[3]
|
||||
std_pos = np.array([w * STD_POS, h * STD_POS, w * STD_POS, h * STD_POS])
|
||||
std_vel = np.array([w * STD_VEL, h * STD_VEL, w * STD_VEL, h * STD_VEL])
|
||||
q = np.zeros((8, 8), dtype=float)
|
||||
q[np.arange(4), np.arange(4)] = std_pos * std_pos
|
||||
q[np.arange(4) + 4, np.arange(4) + 4] = std_vel * std_vel
|
||||
|
||||
# §4.4：x ← F·x；P ← F·P·Fᵀ + Q
|
||||
self._x = _F @ self._x
|
||||
self._p = _F @ self._p @ _F.T + q
|
||||
return BoxCXCYWH(float(self._x[0]), float(self._x[1]),
|
||||
float(self._x[2]), float(self._x[3]))
|
||||
|
||||
def update(self, z: BoxCXCYWH) -> None:
|
||||
# §4.5：未初始化直接返回
|
||||
if not self._initialized:
|
||||
return
|
||||
|
||||
# §4.2 动态观测噪声：R 用 abs(w)/abs(h) 缩放并额外乘 R_SCALE
|
||||
w = abs(self._x[2])
|
||||
h = abs(self._x[3])
|
||||
rw = R_SCALE * (w * STD_POS) * (w * STD_POS)
|
||||
rh = R_SCALE * (h * STD_POS) * (h * STD_POS)
|
||||
r = np.diag([rw, rh, rw, rh])
|
||||
|
||||
# §4.5：S = H·P·Hᵀ + R。H = [I4|0]，故 H·P·Hᵀ 即 P 左上 4×4 子块
|
||||
s = self._p[:4, :4] + r
|
||||
try:
|
||||
s_inv = np.linalg.inv(s + _S_REG * np.eye(4))
|
||||
except np.linalg.LinAlgError:
|
||||
return # 求逆失败：防御性跳过本次更新（C++ 版同样跳过）
|
||||
|
||||
# §4.5：K = P·Hᵀ·S⁻¹，P·Hᵀ 即 P 的前 4 列（8×4）
|
||||
k = self._p[:, _H_COLS] @ s_inv
|
||||
|
||||
# 残差 y = z − H·x（H·x 即状态前 4 维）
|
||||
y = np.array([z.cx, z.cy, z.w, z.h]) - self._x[:4]
|
||||
|
||||
# x ← x + K·y
|
||||
self._x = self._x + k @ y
|
||||
|
||||
# P ← (I8 − K·H)·P：K·H 是 K 的前 4 列铺在前 4 列上
|
||||
kh = np.zeros((8, 8), dtype=float)
|
||||
kh[:, :4] = k
|
||||
self._p = (np.eye(8) - kh) @ self._p
|
||||
@@ -1,269 +0,0 @@
|
||||
"""多目标追踪过滤器（TargetTracker）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/target_tracker/src/target_tracker.cpp
|
||||
（规格：docs/视觉追踪技术路线.md §5 关联 / §6 生命周期与锁定 / §7 六步主流程）。
|
||||
|
||||
线程模型（§9.5）：update() 由单线程（推理主循环）调用；仅对 last_result
|
||||
缓存加锁，last_result() 返回受保护拷贝。
|
||||
"""
|
||||
|
||||
import math
|
||||
import threading
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional, Sequence
|
||||
|
||||
from .kalman_box_filter import BoxCXCYWH
|
||||
from .tracklet import BoxXYXY, Tracklet, compute_iou, to_center_size
|
||||
|
||||
|
||||
@dataclass
|
||||
class Detection:
|
||||
"""单个检测框（§2.2 输入元素：[x1,y1,x2,y2,conf,cls_id]）。"""
|
||||
|
||||
x1: float = 0.0
|
||||
y1: float = 0.0
|
||||
x2: float = 0.0
|
||||
y2: float = 0.0
|
||||
conf: float = 0.0
|
||||
cls_id: int = 0
|
||||
|
||||
|
||||
@dataclass
|
||||
class FrameShape:
|
||||
"""画面尺寸（height, width）：仅 Coast 兜底选举时用于取画面中心。"""
|
||||
|
||||
height: int = 0
|
||||
width: int = 0
|
||||
|
||||
|
||||
@dataclass
|
||||
class TrackResult:
|
||||
"""每帧追踪输出（§2.3 九字段）。"""
|
||||
|
||||
tracked: bool = False
|
||||
primary_id: Optional[int] = None
|
||||
center: tuple[float, float] = (0.0, 0.0)
|
||||
box: Optional[BoxXYXY] = None
|
||||
confidence: float = 0.0
|
||||
is_predicted: bool = False
|
||||
lost_frames: int = 0
|
||||
n_active: int = 0
|
||||
raw: Optional[BoxXYXY] = None
|
||||
|
||||
|
||||
@dataclass
|
||||
class TrackerConfig:
|
||||
"""追踪器配置参数（§8.1 默认值取自规格）。"""
|
||||
|
||||
max_lost_frames: int = 8
|
||||
max_association_dist: float = 200.0
|
||||
min_hits: int = 3
|
||||
dist_weight: float = 0.5
|
||||
iou_weight: float = 0.5
|
||||
|
||||
|
||||
@dataclass
|
||||
class _AssociationResult:
|
||||
matches: list[tuple[int, int]] = field(default_factory=list)
|
||||
unmatched_track_ids: list[int] = field(default_factory=list)
|
||||
unmatched_det_indices: list[int] = field(default_factory=list)
|
||||
|
||||
|
||||
class TargetTracker:
|
||||
def __init__(self, config: Optional[TrackerConfig] = None) -> None:
|
||||
self._config = config if config is not None else TrackerConfig()
|
||||
self._next_id = 0
|
||||
self._tracks: dict[int, Tracklet] = {}
|
||||
self._frame_count = 0
|
||||
self._locked_primary_id: Optional[int] = None
|
||||
self._last_result = TrackResult()
|
||||
self._last_result_mutex = threading.Lock()
|
||||
|
||||
@property
|
||||
def config(self) -> TrackerConfig:
|
||||
return self._config
|
||||
|
||||
def get_config(self) -> TrackerConfig:
|
||||
return self._config
|
||||
|
||||
def update(self, detections: Sequence[Detection],
|
||||
frame_shape: Optional[FrameShape] = None) -> TrackResult:
|
||||
# §7 六步主流程
|
||||
self._frame_count += 1
|
||||
|
||||
# 入口防御：过滤坐标或置信度含 NaN/Inf 的检测（非有限值一旦进入
|
||||
# 卡尔曼状态将永久污染轨道并传导到主目标输出）
|
||||
valid_detections = [
|
||||
det for det in detections
|
||||
if all(math.isfinite(v) for v in (det.x1, det.y1, det.x2, det.y2, det.conf))
|
||||
]
|
||||
|
||||
# 步骤 1：所有存活轨道先行预测（推进一帧）
|
||||
for track in self._tracks.values():
|
||||
track.predict()
|
||||
|
||||
# 步骤 2：贪婪关联（无检测时全部轨道进入 Coast 计数）
|
||||
if valid_detections:
|
||||
assoc = self._associate_detections(valid_detections)
|
||||
else:
|
||||
assoc = _AssociationResult(unmatched_track_ids=list(self._tracks.keys()))
|
||||
|
||||
# 步骤 3：用匹配到的检测更新轨道
|
||||
for track_id, det_idx in assoc.matches:
|
||||
det = valid_detections[det_idx]
|
||||
raw_box = BoxXYXY(det.x1, det.y1, det.x2, det.y2)
|
||||
self._tracks[track_id].update(raw_box, det.conf, det.cls_id)
|
||||
|
||||
# 步骤 4：未匹配轨道计丢失，死亡者回收（锁定唯一解除途径之一）
|
||||
for track_id in assoc.unmatched_track_ids:
|
||||
track = self._tracks.get(track_id)
|
||||
if track is None:
|
||||
continue
|
||||
track.mark_miss()
|
||||
if track.is_dead:
|
||||
if self._locked_primary_id == track_id:
|
||||
self._locked_primary_id = None
|
||||
del self._tracks[track_id]
|
||||
|
||||
# 步骤 5：未匹配检测 → 创建新轨道（next_id 从 0 递增永不复用）
|
||||
for det_idx in assoc.unmatched_det_indices:
|
||||
det = valid_detections[det_idx]
|
||||
measurement = to_center_size(BoxXYXY(det.x1, det.y1, det.x2, det.y2))
|
||||
self._tracks[self._next_id] = Tracklet(
|
||||
self._next_id, measurement, det.conf, det.cls_id,
|
||||
self._config.max_lost_frames)
|
||||
self._next_id += 1
|
||||
|
||||
# 步骤 6：选取/延续主目标，组装输出并缓存
|
||||
result = self._select_primary(frame_shape)
|
||||
with self._last_result_mutex:
|
||||
self._last_result = result
|
||||
return result
|
||||
|
||||
def last_result(self) -> TrackResult:
|
||||
with self._last_result_mutex:
|
||||
return self._last_result
|
||||
|
||||
def reset(self) -> None:
|
||||
# §2.1：清空所有轨道、ID 计数器、锁定状态与缓存结果（next_id 归零）
|
||||
self._tracks.clear()
|
||||
self._next_id = 0
|
||||
self._frame_count = 0
|
||||
self._locked_primary_id = None
|
||||
with self._last_result_mutex:
|
||||
self._last_result = TrackResult()
|
||||
|
||||
def active_count(self) -> int:
|
||||
return len(self._tracks)
|
||||
|
||||
def active_tracks(self) -> dict[int, Tracklet]:
|
||||
return dict(self._tracks)
|
||||
|
||||
def _associate_detections(self, detections: Sequence[Detection]) -> _AssociationResult:
|
||||
result = _AssociationResult()
|
||||
|
||||
# §5.3：按轨道 confidence 降序（平局按 ID 升序保证确定性）。
|
||||
# 非有限置信度按 −∞ 处理（排最后），避免 NaN 破坏全序。
|
||||
def sort_key(track_id: int) -> tuple[float, int]:
|
||||
raw = self._tracks[track_id].confidence
|
||||
conf = raw if math.isfinite(raw) else -math.inf
|
||||
return (-conf, track_id)
|
||||
|
||||
sorted_ids = sorted(self._tracks.keys(), key=sort_key)
|
||||
det_assigned = [False] * len(detections)
|
||||
|
||||
for track_id in sorted_ids:
|
||||
track = self._tracks[track_id]
|
||||
# §5.3：最优代价初始 1.0，仅 cost 严格小于才匹配（cost ≥ 1.0 判为不可匹配）
|
||||
best_cost = 1.0
|
||||
best_det = -1
|
||||
for d, det in enumerate(detections):
|
||||
if det_assigned[d]:
|
||||
continue
|
||||
det_cx = (det.x1 + det.x2) / 2.0
|
||||
det_cy = (det.y1 + det.y2) / 2.0
|
||||
dist = math.hypot(track.last_center[0] - det_cx,
|
||||
track.last_center[1] - det_cy)
|
||||
norm_dist = dist / max(self._config.max_association_dist, 1e-3)
|
||||
iou = (compute_iou(track.last_box,
|
||||
BoxXYXY(det.x1, det.y1, det.x2, det.y2))
|
||||
if track.last_box is not None else 0.0)
|
||||
cost = (self._config.dist_weight * norm_dist +
|
||||
self._config.iou_weight * (1.0 - iou))
|
||||
if cost < best_cost:
|
||||
best_cost = cost
|
||||
best_det = d
|
||||
if best_det >= 0:
|
||||
result.matches.append((track_id, best_det))
|
||||
det_assigned[best_det] = True
|
||||
else:
|
||||
result.unmatched_track_ids.append(track_id)
|
||||
|
||||
result.unmatched_det_indices = [
|
||||
d for d, assigned in enumerate(det_assigned) if not assigned
|
||||
]
|
||||
return result
|
||||
|
||||
def _select_primary(self, frame_shape: Optional[FrameShape]) -> TrackResult:
|
||||
result = TrackResult(n_active=len(self._tracks))
|
||||
|
||||
# §6.3：轨道表为空 → 清除锁定，返回 tracked=false
|
||||
if not self._tracks:
|
||||
self._locked_primary_id = None
|
||||
return result
|
||||
|
||||
# §6.3 ① 锁定延续：锁定轨道存活则直接输出（Coast 中照样输出，
|
||||
# 不被高置信度目标抢占）
|
||||
chosen: Optional[Tracklet] = None
|
||||
is_predicted = False
|
||||
if self._locked_primary_id is not None:
|
||||
track = self._tracks.get(self._locked_primary_id)
|
||||
if track is not None:
|
||||
chosen = track
|
||||
is_predicted = track.is_coasting
|
||||
|
||||
# §6.3 ② 重新选举（无锁定 / 锁定目标已死亡被回收）
|
||||
if chosen is None:
|
||||
measured = [t for t in self._tracks.values() if not t.is_coasting]
|
||||
coasting = [t for t in self._tracks.values() if t.is_coasting]
|
||||
|
||||
if measured:
|
||||
# confirmed ← measured 中 hits ≥ min_hits 者；为空则退回 measured
|
||||
# （min_hits 只影响候选池优先级，不阻止输出）
|
||||
pool = [t for t in measured if t.hits >= self._config.min_hits]
|
||||
if not pool:
|
||||
pool = measured
|
||||
chosen = pool[0]
|
||||
for t in pool:
|
||||
if t.confidence > chosen.confidence:
|
||||
chosen = t
|
||||
is_predicted = False
|
||||
elif coasting:
|
||||
# 无实测轨道：选离画面中心最近者（缺省按 640×480 中心 (320,240)）
|
||||
center_x = frame_shape.width / 2.0 if frame_shape and frame_shape.width > 0 else 320.0
|
||||
center_y = frame_shape.height / 2.0 if frame_shape and frame_shape.height > 0 else 240.0
|
||||
best_dist_sq = math.inf
|
||||
for t in coasting:
|
||||
dx = t.last_center[0] - center_x
|
||||
dy = t.last_center[1] - center_y
|
||||
dist_sq = dx * dx + dy * dy
|
||||
if dist_sq < best_dist_sq:
|
||||
best_dist_sq = dist_sq
|
||||
chosen = t
|
||||
is_predicted = True
|
||||
else:
|
||||
# 理论上不可达（轨道非空则 measured/coasting 必居其一）；防御清锁
|
||||
self._locked_primary_id = None
|
||||
return result
|
||||
self._locked_primary_id = chosen.track_id # 选举后锁定
|
||||
|
||||
# 组装输出（§2.3 九字段）；raw 仅实测时有值，Coast 时为空
|
||||
result.tracked = True
|
||||
result.primary_id = chosen.track_id
|
||||
result.center = chosen.last_center
|
||||
result.box = chosen.last_box
|
||||
result.confidence = chosen.confidence
|
||||
result.is_predicted = is_predicted
|
||||
result.lost_frames = chosen.lost_count
|
||||
result.raw = None if is_predicted else chosen.last_raw_box
|
||||
return result
|
||||
@@ -1,110 +0,0 @@
|
||||
"""轨道（Tracklet）与基础几何工具。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/target_tracker/include/target_tracker/tracklet.h
|
||||
（规格：docs/视觉追踪技术路线.md §3.2 / §5.2 / §7）。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
from .kalman_box_filter import BoxCXCYWH, KalmanBoxFilter
|
||||
|
||||
|
||||
@dataclass
|
||||
class BoxXYXY:
|
||||
"""轴对齐边界框（像素坐标，左上/右下角表示）。"""
|
||||
|
||||
x1: float = 0.0
|
||||
y1: float = 0.0
|
||||
x2: float = 0.0
|
||||
y2: float = 0.0
|
||||
|
||||
|
||||
def to_center_size(box: BoxXYXY) -> BoxCXCYWH:
|
||||
# §7 TRACKLET_UPDATE：cx=(x1+x2)/2；cy=(y1+y2)/2；w=x2−x1；h=y2−y1
|
||||
return BoxCXCYWH((box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0,
|
||||
box.x2 - box.x1, box.y2 - box.y1)
|
||||
|
||||
|
||||
def to_xyxy(box: BoxCXCYWH) -> BoxXYXY:
|
||||
# §7 TRACKLET_PREDICT 的 center_size 逆变换
|
||||
return BoxXYXY(box.cx - box.w / 2.0, box.cy - box.h / 2.0,
|
||||
box.cx + box.w / 2.0, box.cy + box.h / 2.0)
|
||||
|
||||
|
||||
def compute_iou(a: BoxXYXY, b: BoxXYXY) -> float:
|
||||
"""标准轴对齐 IoU（§5.2）：任一框为空、交集 ≤ 0 或并集 ≤ 0 时返回 0。"""
|
||||
area_a = (a.x2 - a.x1) * (a.y2 - a.y1)
|
||||
area_b = (b.x2 - b.x1) * (b.y2 - b.y1)
|
||||
if area_a <= 0.0 or area_b <= 0.0:
|
||||
return 0.0
|
||||
|
||||
inter_w = min(a.x2, b.x2) - max(a.x1, b.x1)
|
||||
inter_h = min(a.y2, b.y2) - max(a.y1, b.y1)
|
||||
if inter_w <= 0.0 or inter_h <= 0.0:
|
||||
return 0.0
|
||||
|
||||
inter_area = inter_w * inter_h
|
||||
union_area = area_a + area_b - inter_area
|
||||
if union_area <= 0.0:
|
||||
return 0.0
|
||||
return inter_area / union_area
|
||||
|
||||
|
||||
class Tracklet:
|
||||
"""单条轨道：生命周期计数 + 辅助状态 + 卡尔曼滤波器（§3.2）。
|
||||
|
||||
构造即创建轨道（§7 步骤 5）：用首次观测初始化滤波器，age=1、hits=1、
|
||||
lost_count=0，last_center / last_box 立即由观测重建（创建当帧即可参与输出）。
|
||||
"""
|
||||
|
||||
def __init__(self, track_id: int, measurement: BoxCXCYWH, conf: float,
|
||||
cls_id: int, max_lost_frames: int) -> None:
|
||||
self.track_id = track_id
|
||||
self.kf = KalmanBoxFilter()
|
||||
self.lost_count = 0
|
||||
self.max_lost = max_lost_frames
|
||||
self.confidence = conf
|
||||
self.cls_id = cls_id
|
||||
self.age = 1
|
||||
self.hits = 1
|
||||
self.last_center: tuple[float, float] = (measurement.cx, measurement.cy)
|
||||
self.last_box: Optional[BoxXYXY] = to_xyxy(measurement)
|
||||
# 创建即源于实测检测：同步记录原始检测框（Coast 期间保留不更新）
|
||||
self.last_raw_box: Optional[BoxXYXY] = self.last_box
|
||||
self.kf.initiate(measurement)
|
||||
|
||||
@property
|
||||
def is_dead(self) -> bool:
|
||||
# §6.1 注记 2：lost_count > max_lost（严格大于）
|
||||
return self.lost_count > self.max_lost
|
||||
|
||||
@property
|
||||
def is_coasting(self) -> bool:
|
||||
# §3.2：lost_count > 0 即处于纯预测状态
|
||||
return self.lost_count > 0
|
||||
|
||||
def predict(self) -> None:
|
||||
# §7 TRACKLET_PREDICT：预测成功时更新 last_center / last_box 且 age+1
|
||||
pred = self.kf.predict()
|
||||
if pred is not None:
|
||||
self.last_center = (pred.cx, pred.cy)
|
||||
self.last_box = to_xyxy(pred)
|
||||
self.age += 1
|
||||
|
||||
def update(self, box: BoxXYXY, conf: float, cls_id: int) -> None:
|
||||
# §7 TRACKLET_UPDATE：框 → [cx,cy,w,h] → kf.update
|
||||
self.kf.update(to_center_size(box))
|
||||
self.last_center = ((box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0)
|
||||
self.last_box = box
|
||||
self.last_raw_box = box
|
||||
self.confidence = conf
|
||||
self.cls_id = cls_id
|
||||
self.lost_count = 0
|
||||
self.hits += 1
|
||||
self.age += 1
|
||||
|
||||
def mark_miss(self) -> None:
|
||||
# §7 步骤 4：未匹配计丢失
|
||||
self.lost_count += 1
|
||||
self.age += 1
|
||||
@@ -1,9 +1,20 @@
|
||||
"""task_runner —— 任务编排器（预定流程.md 的天空侧任务级状态机）。
|
||||
|
||||
把已通的 L1（航向伺服 TrackingControlLaw）与 L2（地理引导 GuidanceLaw）按任务阶段
|
||||
编排成一条可运行的全流程：WAIT → ARM_TAKEOFF → GOTO → TRACK → APPROACH → RTL。
|
||||
编排成一条可运行的全流程：
|
||||
|
||||
WAIT → ARM_TAKEOFF → GOTO → SEARCH → TRACK → RTL
|
||||
|
||||
* GOTO ：L2 地理引导飞向**参考点**（地面给的坐标是搜索起点，不是终点）；
|
||||
* SEARCH ：到达参考点后原地偏航扫视，靠吊舱/视觉找目标；超时未发现 → RTL；
|
||||
* TRACK ：L1 角度直入 + 无距离降级（slow_approach 缓慢靠近）；超时 → RTL；
|
||||
* RTL ：自定义返航（回起飞点 → 恢复初始航向 → 垂直降落），不爬高；
|
||||
异常/未起飞等情形兜底退回 PX4 原生 RTL。
|
||||
|
||||
模块划分：
|
||||
* state_machine.py 纯逻辑任务状态机（无 IO，可单测）
|
||||
* runner.py 编排器（MAVSDK 飞行编排 + 状态机驱动 + PodBridge 闭环，A3）
|
||||
* runner.py 编排器（MAVSDK 飞行编排 + 状态机驱动 + PodBridge 闭环）
|
||||
* return_home.py 自定义返航纯逻辑（HomingController：转场/航向/高度）
|
||||
* pod_control.py 真机吊舱识别/锁定指令下发（0x07/0x0F/0x0E/0x08）
|
||||
* log.py 结构化任务日志（事件 JSONL + 遥测 CSV + 简报）
|
||||
"""
|
||||
|
||||
+126
-5
@@ -6,15 +6,21 @@
|
||||
* 事件日志（JSONL）：状态机转移、飞行编排事件（解锁/起飞/返航）、异常/紧急。
|
||||
低频、带语义、结构化，便于 grep / jq 定位「什么时候发生了什么」。
|
||||
* 周期遥测日志（CSV）：每控制周期的位姿 / GPS / 目标 / 控制输出，便于回放与画图。
|
||||
* 简洁任务简报（task_brief.log，纯文本）：一行一事件（阶段切换/地面站命令/
|
||||
飞控告警）+ 运行结束汇总（时长/最高/最近距目标/状态链），供人工阅读与归档；
|
||||
机器日志（JSONL/CSV）格式不变。
|
||||
|
||||
时间戳：单调毫秒（now_ms，与运行时代码同源）+ 相对时间 t_s（自首个 now_ms 起）。
|
||||
文件 IO 集中在 open / close，热路径只追加一行，不做 flush 决策。
|
||||
文件 IO 集中在 open / close；热路径只追加（并立即 flush 落盘——缓冲期间文件不可见，
|
||||
地面站页面运行中需要读到最新状态与遥测，2026-10-14 实测）。
|
||||
"""
|
||||
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
@@ -60,13 +66,26 @@ class TaskLogConfig:
|
||||
dir: str = "logs" # 输出目录（相对工作目录）
|
||||
event_file: str = "task_events.jsonl" # 事件日志
|
||||
telemetry_file: str = "task_telemetry.csv" # 周期遥测日志
|
||||
brief_file: str = "task_brief.log" # 人类可读任务简报（追加）
|
||||
telemetry_every_n: int = 1 # 每 N 周期记一次遥测（1 = 每周期）
|
||||
telemetry_flush_ms: int = 1000 # 遥测落盘间隔（毫秒）；0 = 每行立即 flush
|
||||
|
||||
|
||||
def _monotonic_ms() -> int:
|
||||
return int(time.monotonic() * 1000)
|
||||
|
||||
|
||||
_STATE_CN = {"wait": "待命", "arm_takeoff": "起飞", "goto": "坐标引导",
|
||||
"search": "搜索", "track": "追踪靠近", "approach": "接近悬停",
|
||||
"rtl": "返航"}
|
||||
|
||||
|
||||
def _fmt_t_s(t_s: float) -> str:
|
||||
"""相对时间 → mm:ss.s（简报用）。"""
|
||||
m, s = divmod(max(0.0, float(t_s or 0.0)), 60.0)
|
||||
return f"{int(m):02d}:{s:04.1f}"
|
||||
|
||||
|
||||
class TaskLogger:
|
||||
"""结构化任务日志。用法：
|
||||
|
||||
@@ -86,12 +105,20 @@ class TaskLogger:
|
||||
self._event_fp = None
|
||||
self._tel_fp = None
|
||||
self._tel_writer = None
|
||||
self._brief_fp = None
|
||||
self._t0_ms: Optional[int] = None
|
||||
self._tick = 0
|
||||
self._last_tel_flush_ms: Optional[int] = None # 遥测上次落盘时刻
|
||||
# 简报汇总统计（随遥测行/事件更新，close 时写一行汇总）
|
||||
self._n_events = 0
|
||||
self._last_t_s = 0.0
|
||||
self._max_rel_alt_m: Optional[float] = None
|
||||
self._min_dist_m: Optional[float] = None
|
||||
self._state_chain: list = []
|
||||
|
||||
# --- 生命周期 -----------------------------------------------------------
|
||||
def open(self) -> None:
|
||||
"""创建输出目录并打开两个日志文件；disabled 时跳过。"""
|
||||
"""创建输出目录并打开日志文件；disabled 时跳过。"""
|
||||
if not self._cfg.enabled:
|
||||
return
|
||||
out_dir = Path(self._cfg.dir)
|
||||
@@ -103,11 +130,22 @@ class TaskLogger:
|
||||
self._tel_writer = csv.writer(self._tel_fp)
|
||||
if (out_dir / self._cfg.telemetry_file).stat().st_size == 0:
|
||||
self._tel_writer.writerow(_TELEMETRY_FIELDS)
|
||||
self._brief_fp = (out_dir / self._cfg.brief_file).open(
|
||||
"a", encoding="utf-8")
|
||||
self._brief_fp.write(
|
||||
f"===== 任务开始 {datetime.now().strftime('%Y-%m-%d %H:%M:%S')} =====\n")
|
||||
self._last_tel_flush_ms = None
|
||||
self._event_fp.flush()
|
||||
self._tel_fp.flush()
|
||||
self._brief_fp.flush()
|
||||
|
||||
def close(self) -> None:
|
||||
"""flush 并关闭两个日志文件。"""
|
||||
"""写简报汇总并关闭全部日志文件。"""
|
||||
if self._brief_fp is not None:
|
||||
self._brief_fp.write(self._brief_summary() + "\n\n")
|
||||
self._brief_fp.flush()
|
||||
self._brief_fp.close()
|
||||
self._brief_fp = None
|
||||
if self._event_fp is not None:
|
||||
self._event_fp.flush()
|
||||
self._event_fp.close()
|
||||
@@ -120,16 +158,32 @@ class TaskLogger:
|
||||
|
||||
# --- 记录 ---------------------------------------------------------------
|
||||
def log_event(self, now_ms: int, event_type: str, **fields) -> None:
|
||||
"""追加一条 JSONL 事件。"""
|
||||
"""追加一条 JSONL 事件并**立即落盘**（事件低频且语义关键：地面站要实时看阶段）。
|
||||
|
||||
注意：遥测行不在这里 flush——见 log_telemetry 的节流落盘说明。
|
||||
"""
|
||||
if self._event_fp is None:
|
||||
return
|
||||
record = {"type": event_type, "ts_ms": now_ms,
|
||||
"t_s": self._rel_s(now_ms)}
|
||||
record.update(fields)
|
||||
self._event_fp.write(json.dumps(record, ensure_ascii=False) + "\n")
|
||||
self._event_fp.flush()
|
||||
self._n_events += 1
|
||||
if self._brief_fp is not None:
|
||||
self._brief_fp.write(self._brief_text(record) + "\n")
|
||||
self._brief_fp.flush()
|
||||
|
||||
def log_telemetry(self, now_ms: int, rec: TelemetryRecord) -> None:
|
||||
"""追加一行遥测（按 telemetry_every_n 节流）。"""
|
||||
"""追加一行遥测（按 telemetry_every_n 节流）。
|
||||
|
||||
落盘策略：**写入靠 Python 缓冲、按 telemetry_flush_ms 定期 flush**，
|
||||
而不是每行 fsync 式 flush。原因：本方法在主控制循环里以 20Hz 调用，
|
||||
每行都 flush 等于每秒 20 次同步磁盘 IO，在树莓派 SD 卡上会阻塞事件
|
||||
循环（实测 MAVSDK 回调队列因此积压告警）；事件行仍即时落盘，故页面
|
||||
的阶段显示不受影响，只有图表数据最多滞后 telemetry_flush_ms。
|
||||
close() 会做最后一次 flush，进程正常退出不丢数据。
|
||||
"""
|
||||
if self._tel_writer is None:
|
||||
return
|
||||
self._tick += 1
|
||||
@@ -137,8 +191,75 @@ class TaskLogger:
|
||||
return
|
||||
rec.t_s = self._rel_s(now_ms)
|
||||
self._tel_writer.writerow([getattr(rec, f) for f in _TELEMETRY_FIELDS])
|
||||
self._flush_telemetry(now_ms)
|
||||
# 简报汇总统计（只随真正落盘的行更新）
|
||||
self._last_t_s = rec.t_s
|
||||
if math.isfinite(rec.relative_altitude_m):
|
||||
if (self._max_rel_alt_m is None
|
||||
or rec.relative_altitude_m > self._max_rel_alt_m):
|
||||
self._max_rel_alt_m = rec.relative_altitude_m
|
||||
dist = rec.distance_to_target_m
|
||||
if math.isfinite(dist) and dist > 0:
|
||||
if self._min_dist_m is None or dist < self._min_dist_m:
|
||||
self._min_dist_m = dist
|
||||
if rec.state and (not self._state_chain
|
||||
or self._state_chain[-1] != rec.state):
|
||||
if len(self._state_chain) < 32:
|
||||
self._state_chain.append(rec.state)
|
||||
|
||||
# --- 简报（人类可读，一行一事件）----------------------------------------
|
||||
@staticmethod
|
||||
def _brief_text(record: dict) -> str:
|
||||
t = _fmt_t_s(record.get("t_s", 0.0))
|
||||
etype = record.get("type", "?")
|
||||
if etype == "state":
|
||||
to = _STATE_CN.get(record.get("to_state"),
|
||||
record.get("to_state", "?"))
|
||||
if record.get("from_state") == record.get("to_state"):
|
||||
return f"[{t}] 任务启动（当前: {to}）"
|
||||
frm = _STATE_CN.get(record.get("from_state"),
|
||||
record.get("from_state", "?"))
|
||||
return f"[{t}] 阶段: {frm} → {to}（{record.get('reason', '')}）"
|
||||
if etype == "command":
|
||||
return f"[{t}] 地面站命令: {record.get('name', '')}"
|
||||
if etype == "fc_status":
|
||||
return (f"[{t}] 飞控 {record.get('severity', '')}: "
|
||||
f"{str(record.get('text', '')).strip()}")
|
||||
if etype == "takeoff":
|
||||
return f"[{t}] 起飞至 {record.get('altitude_m', '?')} m"
|
||||
if etype == "takeoff_failed":
|
||||
return f"[{t}] 起飞失败: {record.get('error', '')}"
|
||||
if etype == "rtl":
|
||||
return f"[{t}] 开始返航"
|
||||
fields = [f"{k}={v}" for k, v in record.items()
|
||||
if k not in ("type", "ts_ms", "t_s")]
|
||||
return f"[{t}] {etype}: {', '.join(fields)}"
|
||||
|
||||
def _brief_summary(self) -> str:
|
||||
parts = [f"时长 {self._last_t_s:.1f}s"]
|
||||
if self._max_rel_alt_m is not None:
|
||||
parts.append(f"最高 {self._max_rel_alt_m:.1f}m")
|
||||
if self._min_dist_m is not None:
|
||||
parts.append(f"最近距目标 {self._min_dist_m:.1f}m")
|
||||
parts.append(f"事件 {self._n_events} 条")
|
||||
if self._state_chain:
|
||||
parts.append("状态链 " + "→".join(
|
||||
_STATE_CN.get(s, s) for s in self._state_chain))
|
||||
return "----- 本次运行汇总: " + " · ".join(parts) + " -----"
|
||||
|
||||
# --- 内部 ---------------------------------------------------------------
|
||||
def _flush_telemetry(self, now_ms: int) -> None:
|
||||
"""按 telemetry_flush_ms 节流落盘遥测（0 = 每行立即落盘）。"""
|
||||
interval = self._cfg.telemetry_flush_ms
|
||||
if interval <= 0:
|
||||
self._tel_fp.flush()
|
||||
self._last_tel_flush_ms = now_ms
|
||||
return
|
||||
if (self._last_tel_flush_ms is None
|
||||
or now_ms - self._last_tel_flush_ms >= interval):
|
||||
self._tel_fp.flush()
|
||||
self._last_tel_flush_ms = now_ms
|
||||
|
||||
def _rel_s(self, now_ms: int) -> float:
|
||||
if self._t0_ms is None:
|
||||
self._t0_ms = now_ms
|
||||
|
||||
@@ -8,7 +8,8 @@
|
||||
* 0x08 目标识别关（真正退出识别）
|
||||
|
||||
本模块把这些指令序列封装成 PodController，供 TaskRunner 在 GOTO / RTL 阶段调用。
|
||||
SITL 用 VirtualPod（直接合成锁定态），不注入 PodController。
|
||||
另含地面站手动命令（stop_track / auto_lock / center），与 pod_console.py 按键
|
||||
u / a / c 语义对齐。SITL 用 VirtualPod（直接合成锁定态），不注入 PodController。
|
||||
"""
|
||||
|
||||
from pod_bridge.protocol import ControlCode, ControlCommand
|
||||
@@ -37,3 +38,18 @@ class PodController:
|
||||
"""停止跟踪 + 关闭目标识别（RTL 前调用）。"""
|
||||
self._bridge.send_control(ControlCommand(code=ControlCode.STOP_TRACK))
|
||||
self._bridge.send_control(ControlCommand(code=ControlCode.TARGET_DETECT_OFF))
|
||||
|
||||
def stop_track(self) -> None:
|
||||
"""停止跟踪（只停当前这一次，0x0E；0x0F 常驻自动锁定仍生效）。"""
|
||||
self._bridge.send_control(ControlCommand(code=ControlCode.STOP_TRACK))
|
||||
|
||||
def auto_lock(self) -> None:
|
||||
"""发现目标自动锁定 + 识别转跟踪（0x0F + 0x10）。"""
|
||||
self._bridge.send_control(ControlCommand(
|
||||
code=ControlCode.AUTO_LOCK_ON_FOUND))
|
||||
self._bridge.send_control(ControlCommand(
|
||||
code=ControlCode.DETECT_TO_TRACK))
|
||||
|
||||
def center(self) -> None:
|
||||
"""手动归中（0x2B）。"""
|
||||
self._bridge.send_control(ControlCommand(code=ControlCode.CENTER))
|
||||
|
||||
@@ -0,0 +1,130 @@
|
||||
"""return_home.py —— 自定义返航（回起飞点 + 恢复初始航向 + 就地降落）的纯逻辑控制。
|
||||
|
||||
背景：PX4 自带的 RTL 会先爬到 `RTL_RETURN_ALT`（默认数十米）再飞回、再下降到
|
||||
`RTL_DESCEND_ALT` 后落地——即"飞很高再降落"。本模块给出替代方案：
|
||||
|
||||
RETURN 阶段：保持在**转场高度**（取进入返航时的高度与起飞高度的较大者，
|
||||
绝不比任务高度更高）飞向起飞点，机头朝目标方向；
|
||||
YAW 阶段 ：到位后原地把机头转回**起飞时刻的航向**；
|
||||
LAND 阶段 ：交给飞控 `land()` 垂直降落（不爬升）。
|
||||
|
||||
设计约束（与项目铁律一致）：
|
||||
* 纯逻辑：输入为载机位置/航向/相对高度，输出速度与角速度意图，不碰 IO，
|
||||
可确定性单测；实际下发由 runner 负责。
|
||||
* NED 语义：vz 下正。目标高度高于当前 → vz 为负（爬升）。
|
||||
* 只做"航位推算"级别的控制：无距离传感器时不需要它（返航点是已知识别坐标），
|
||||
因此这里用比例控制 + 限幅即可，不引入积分项（避免返航中积分饱和）。
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
from pod_bridge.guidance import haversine_distance_m, initial_bearing_deg
|
||||
from visual_track_control.rate_limiter import clamp
|
||||
|
||||
|
||||
def wrap_deg_180(deg: float) -> float:
|
||||
"""角度回绕到 [-180, 180)。"""
|
||||
d = math.fmod(deg + 180.0, 360.0)
|
||||
if d < 0.0:
|
||||
d += 360.0
|
||||
return d - 180.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class ReturnConfig:
|
||||
"""自定义返航参数（全外置；数值为占位，须按机体/场地标定）。"""
|
||||
|
||||
enabled: bool = True # False → 退回 PX4 原生 RTL
|
||||
speed_mps: float = 2.0 # 转场水平速度上限
|
||||
arrival_radius_m: float = 3.0 # 距起飞点该半径内视为"已回到起飞点"
|
||||
timeout_s: float = 120.0 # 转场阶段超时（超时 → 退回 PX4 RTL 兜底）
|
||||
yaw_kp: float = 1.0 # 航向误差 → 偏航角速度（dps/deg）
|
||||
yaw_rate_limit_dps: float = 20.0 # 偏航角速度上限
|
||||
align_kp: float = 1.0 # 恢复初始航向阶段的比例增益
|
||||
align_rate_limit_dps: float = 15.0
|
||||
yaw_align_tolerance_deg: float = 5.0 # 航向对齐容差
|
||||
yaw_align_timeout_s: float = 15.0 # 航向对齐超时（超时也进入降落）
|
||||
alt_kp: float = 0.5 # 高度误差 → vz（m/s per m）
|
||||
alt_limit_mps: float = 0.5 # 垂直速度上限（与 L1 vertical 限速同量级）
|
||||
|
||||
|
||||
@dataclass
|
||||
class HomingOutput:
|
||||
"""单周期归航意图（机体系前向 + 垂直 NED + 偏航角速度）。"""
|
||||
|
||||
vx_mps: float = 0.0
|
||||
vz_mps: float = 0.0
|
||||
yaw_rate_dps: float = 0.0
|
||||
distance_m: float = math.inf
|
||||
bearing_err_deg: float = 0.0
|
||||
arrived: bool = False
|
||||
|
||||
|
||||
class HomingController:
|
||||
"""把载机当前位置/航向/高度 → 飞回起飞点的速度与航向意图。
|
||||
|
||||
用法：进入返航时用起飞点经纬度、起飞时刻航向、转场高度构造一次；
|
||||
之后每周期 `update(lat, lon, yaw_rad, rel_alt_m)`。
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: ReturnConfig, home_lat_deg: float,
|
||||
home_lon_deg: float, initial_yaw_rad: float,
|
||||
transit_alt_m: float) -> None:
|
||||
self._cfg = cfg
|
||||
self._home_lat = float(home_lat_deg)
|
||||
self._home_lon = float(home_lon_deg)
|
||||
self._initial_yaw_rad = float(initial_yaw_rad)
|
||||
self._transit_alt_m = float(transit_alt_m)
|
||||
|
||||
@property
|
||||
def home(self) -> tuple[float, float]:
|
||||
return self._home_lat, self._home_lon
|
||||
|
||||
@property
|
||||
def initial_yaw_rad(self) -> float:
|
||||
return self._initial_yaw_rad
|
||||
|
||||
def update(self, lat_deg: float, lon_deg: float, yaw_rad: float,
|
||||
rel_alt_m: float) -> HomingOutput:
|
||||
"""转场阶段单周期意图：朝起飞点飞，同时保持转场高度。"""
|
||||
cfg = self._cfg
|
||||
dist = haversine_distance_m(lat_deg, lon_deg,
|
||||
self._home_lat, self._home_lon)
|
||||
bearing = initial_bearing_deg(lat_deg, lon_deg,
|
||||
self._home_lat, self._home_lon)
|
||||
err = wrap_deg_180(bearing - math.degrees(yaw_rad))
|
||||
|
||||
yaw_rate = clamp(cfg.yaw_kp * err,
|
||||
-cfg.yaw_rate_limit_dps, cfg.yaw_rate_limit_dps)
|
||||
arrived = dist <= cfg.arrival_radius_m
|
||||
# 对准门控：没对准先转，避免斜着"蹭"过去（与 L2 引导同思路）
|
||||
alignment = max(0.0, math.cos(math.radians(err)))
|
||||
vx = 0.0 if arrived else cfg.speed_mps * alignment
|
||||
return HomingOutput(
|
||||
vx_mps=vx,
|
||||
vz_mps=self.alt_hold_vz(rel_alt_m),
|
||||
yaw_rate_dps=yaw_rate,
|
||||
distance_m=dist,
|
||||
bearing_err_deg=err,
|
||||
arrived=arrived,
|
||||
)
|
||||
|
||||
def alt_hold_vz(self, rel_alt_m: float) -> float:
|
||||
"""保持转场高度（NED 下正）：低于目标 → 负值（爬升），高于 → 正值（下降）。"""
|
||||
cfg = self._cfg
|
||||
return clamp(cfg.alt_kp * (rel_alt_m - self._transit_alt_m),
|
||||
-cfg.alt_limit_mps, cfg.alt_limit_mps)
|
||||
|
||||
def yaw_align(self, yaw_rad: float) -> tuple[float, float]:
|
||||
"""恢复初始航向：返回 (航向误差 deg, 偏航角速度 dps)。"""
|
||||
cfg = self._cfg
|
||||
err = wrap_deg_180(math.degrees(self._initial_yaw_rad)
|
||||
- math.degrees(yaw_rad))
|
||||
rate = clamp(cfg.align_kp * err,
|
||||
-cfg.align_rate_limit_dps, cfg.align_rate_limit_dps)
|
||||
return err, rate
|
||||
|
||||
def yaw_aligned(self, yaw_rad: float) -> bool:
|
||||
err, _ = self.yaw_align(yaw_rad)
|
||||
return abs(err) <= self._cfg.yaw_align_tolerance_deg
|
||||
+424
-39
@@ -7,6 +7,8 @@ L2（地理引导 GuidanceLaw）按任务阶段编排，配合地面坐标注入
|
||||
职责边界：
|
||||
* 状态机（TaskStateMachine）只做「状态 + 转移判据」决策；
|
||||
* 本编排器负责「采集快照 → 喂状态机 → 按状态执行动作」；
|
||||
* 地面站命令（RTL/紧急/吊舱手动指令，经 GroundLink 队列）主循环逐拍分派；
|
||||
* 飞控状态文本（STATUSTEXT，警告/异常）逐拍落任务日志（fc_status 事件）；
|
||||
* 闭环控制复用 PodBridge（L1/L2 运行时切换走 set_heading_servo_enabled）；
|
||||
* 飞行编排（解锁/起飞/返航）通过 duck-typed `drone`（MAVSDK System）完成，
|
||||
便于 mock 单测；真实 SITL 装配见仓库根 task_runner_sitl.py。
|
||||
@@ -23,13 +25,38 @@ from typing import Callable, Optional
|
||||
|
||||
from pod_bridge.guidance import haversine_distance_m
|
||||
from visual_track_control.staleness_checker import is_stale
|
||||
from visual_track_control.types import ControlMode, ControlOutput
|
||||
|
||||
from .log import TaskLogger, TelemetryRecord
|
||||
from .return_home import HomingController, ReturnConfig
|
||||
from .state_machine import (TaskInputs, TaskState, TaskStateConfig,
|
||||
TaskStateMachine)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# 自定义返航的阶段
|
||||
_RTL_RETURN = "return" # 转场：飞回起飞点，保持转场高度
|
||||
_RTL_YAW = "yaw" # 到位：把机头转回初始航向
|
||||
_RTL_LAND = "land" # 降落：交给飞控垂直降落
|
||||
_RTL_FALLBACK = "fallback" # 兜底：退回 PX4 原生 RTL（会爬升，仅异常时使用）
|
||||
_RTL_IDLE = "idle"
|
||||
|
||||
# 地面站吊舱命令 → PodController 方法（与 ground_station/app.py 白名单对齐）
|
||||
_POD_COMMAND_METHODS = {
|
||||
"pod_detect_on": "arm_detection",
|
||||
"pod_auto_lock": "auto_lock",
|
||||
"pod_stop_track": "stop_track",
|
||||
"pod_detect_off": "stop_tracking",
|
||||
"pod_center": "center",
|
||||
}
|
||||
|
||||
# 搜索扫视换向周期计数中「向右」的半周期序号（0 → 右，1 → 左，交替）
|
||||
_SWEEP_RIGHT, _SWEEP_LEFT = 0, 1
|
||||
|
||||
# RTL 落地判定：单次等待 in_air 遥测的上限（秒）。MAVSDK 遥测流挂起时
|
||||
# `async for` 会永久阻塞并卡死主循环，故必须有兜底（超时 → 下一拍重试）。
|
||||
_IN_AIR_TIMEOUT_S = 5.0
|
||||
|
||||
|
||||
def _monotonic_ms() -> int:
|
||||
return int(time.monotonic() * 1000)
|
||||
@@ -45,14 +72,14 @@ class TaskRunner:
|
||||
sm TaskStateMachine —— update()
|
||||
ground GroundLink —— latest()（地面注入坐标）
|
||||
drone MAVSDK System —— action.arm/takeoff/return_to_launch/land/disarm
|
||||
vision_fn callable->bool —— 我方视觉第二判据（可选，require_vision 时用）
|
||||
"""
|
||||
|
||||
def __init__(self, cfg, sm_cfg: TaskStateConfig, px4, bridge, sm: TaskStateMachine,
|
||||
ground, drone, vision_fn: Optional[Callable[[], bool]] = None,
|
||||
ground, drone,
|
||||
now_ms_fn: Callable[[], int] = _monotonic_ms,
|
||||
logger: Optional[TaskLogger] = None,
|
||||
pod_control=None) -> None:
|
||||
pod_control=None, land_instead_of_rtl: bool = False,
|
||||
return_cfg: Optional[ReturnConfig] = None) -> None:
|
||||
self._cfg = cfg
|
||||
self._sm_cfg = sm_cfg
|
||||
self._px4 = px4
|
||||
@@ -60,14 +87,32 @@ class TaskRunner:
|
||||
self._sm = sm
|
||||
self._ground = ground
|
||||
self._drone = drone
|
||||
self._vision_fn = vision_fn
|
||||
self._now = now_ms_fn
|
||||
self._logger = logger # 结构化任务日志（可选，B 阶段）
|
||||
self._pod_control = pod_control # 吊舱识别/锁定控制（可选，真机）
|
||||
self._land_instead_of_rtl = land_instead_of_rtl # 收尾改用就地降落
|
||||
self._return_cfg = return_cfg or ReturnConfig()
|
||||
self._centered_since_ms: Optional[int] = None
|
||||
self._rtl_requested = False
|
||||
self._emergency = False
|
||||
self._rtl_done = False
|
||||
# SEARCH 扫视状态：当前扫向（右/左）与本次扫向的起始时刻
|
||||
self._sweep_dir = _SWEEP_RIGHT
|
||||
self._sweep_since_ms: Optional[int] = None
|
||||
self._pod_detection_on = False # 吊舱识别/自动锁定只开一次（防重发 0x07）
|
||||
self._takeoff_done = False # 解锁/起飞只执行一次（防空中重复起飞）
|
||||
self._in_air_stream = None # in_air 遥测流复用（避免每拍重新订阅）
|
||||
self._rtl_entered_ms: Optional[int] = None # 进入 RTL 时刻（落地等待上限用）
|
||||
# --- 自定义返航（回起飞点 → 恢复初始航向 → 降落）---
|
||||
self._home_lat: Optional[float] = None # 起飞点纬度（解锁时刻记录）
|
||||
self._home_lon: Optional[float] = None
|
||||
self._initial_yaw_rad: Optional[float] = None # 起飞时刻航向
|
||||
self._bridge_started = False # 是否已进入 offboard（未起飞则不能自主返航）
|
||||
self._rtl_phase = _RTL_IDLE
|
||||
self._return_ctrl: Optional[HomingController] = None
|
||||
self._return_started_ms: Optional[int] = None
|
||||
self._yaw_started_ms: Optional[int] = None
|
||||
self._land_cmd_sent = False
|
||||
|
||||
# --- 外部控制 -----------------------------------------------------------
|
||||
def request_rtl(self) -> None:
|
||||
@@ -78,6 +123,43 @@ class TaskRunner:
|
||||
"""外部触发紧急终止（丢控 / RC 接管 / 失控）。"""
|
||||
self._emergency = True
|
||||
|
||||
# --- 地面站命令分派 -----------------------------------------------------
|
||||
def _apply_ground_commands(self) -> None:
|
||||
"""排空地面站命令队列并逐条分派（主循环每拍调用）。"""
|
||||
for name in self._ground.drain_commands():
|
||||
self._apply_command(name)
|
||||
|
||||
def _apply_command(self, name: str) -> None:
|
||||
"""分派一条地面站命令；未知命令名忽略并警告。"""
|
||||
if name == "rtl":
|
||||
self.request_rtl()
|
||||
elif name == "emergency":
|
||||
self.trigger_emergency()
|
||||
elif name in _POD_COMMAND_METHODS:
|
||||
if self._pod_control is None:
|
||||
logger.warning("[Task] 地面站命令 %s 被忽略：未配置吊舱控制（SITL）",
|
||||
name)
|
||||
return
|
||||
try:
|
||||
getattr(self._pod_control, _POD_COMMAND_METHODS[name])()
|
||||
except Exception as exc:
|
||||
logger.warning("[Task] 吊舱命令 %s 执行失败: %s", name, exc)
|
||||
return
|
||||
else:
|
||||
logger.warning("[Task] 未知地面站命令: %s", name)
|
||||
return
|
||||
logger.info("[Task] 地面站命令: %s", name)
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(self._now(), "command", name=name)
|
||||
|
||||
def _log_fc_messages(self) -> None:
|
||||
"""把飞控状态文本（STATUSTEXT）落任务日志并输出控制台（每拍调用）。"""
|
||||
for sev, text in self._px4.take_fc_messages():
|
||||
logger.info("[FC] %s: %s", sev, text)
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(self._now(), "fc_status",
|
||||
severity=sev, text=text)
|
||||
|
||||
@property
|
||||
def state(self) -> TaskState:
|
||||
return self._sm.state
|
||||
@@ -92,24 +174,29 @@ class TaskRunner:
|
||||
await self._px4.start_telemetry() # 幂等：先拿遥测，供起飞到位/距离计算
|
||||
if self._logger is not None:
|
||||
self._logger.open()
|
||||
# 记初始状态：页面「任务状态」取日志最后一个 state 事件，缺此行会显示上次旧状态
|
||||
self._logger.log_event(self._now(), "state",
|
||||
from_state=self._sm.state.value,
|
||||
to_state=self._sm.state.value,
|
||||
reason="任务启动")
|
||||
period_s = 1.0 / self._sm_cfg.loop_frequency_hz
|
||||
try:
|
||||
while not stop_event.is_set():
|
||||
now_ms = self._now()
|
||||
inputs = self._collect_inputs(now_ms)
|
||||
tr = self._sm.update(inputs, now_ms)
|
||||
if tr.occurred:
|
||||
logger.info("[Task] %s -> %s (%s)",
|
||||
tr.from_state.value, tr.to_state.value, tr.reason)
|
||||
try:
|
||||
await self._iteration(now_ms)
|
||||
except asyncio.CancelledError:
|
||||
raise
|
||||
except Exception as exc:
|
||||
# 单周期异常不得让任务带着 offboard 指令静默停摆：转紧急 → 下一拍
|
||||
# 状态机转 RTL（与 bridge.run() 的按周期护栏同思路，但这里必须
|
||||
# 升级为返航，因为再没有别的循环会接管飞机）。
|
||||
logger.error("[Task] 任务周期异常，转紧急返航: %s", exc)
|
||||
self._emergency = True
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(
|
||||
now_ms, "state", from_state=tr.from_state.value,
|
||||
to_state=tr.to_state.value, reason=tr.reason)
|
||||
await self._on_enter(tr.to_state, now_ms)
|
||||
out = await self._tick(now_ms)
|
||||
if self._logger is not None:
|
||||
self._logger.log_telemetry(
|
||||
now_ms, self._build_telemetry_record(now_ms, inputs, out))
|
||||
self._logger.log_event(self._now(), "iteration_error",
|
||||
error=f"{type(exc).__name__}: {exc}")
|
||||
await self._safe_abort()
|
||||
if self._finished():
|
||||
break
|
||||
await asyncio.sleep(period_s)
|
||||
@@ -118,10 +205,52 @@ class TaskRunner:
|
||||
if self._logger is not None:
|
||||
self._logger.close()
|
||||
|
||||
async def _iteration(self, now_ms: int) -> None:
|
||||
"""单周期：命令 → 快照 → 状态机 → 进入动作 → 持续动作 → 日志。"""
|
||||
self._apply_ground_commands() # 地面站命令先于快照采集生效
|
||||
self._log_fc_messages()
|
||||
inputs = self._collect_inputs(now_ms)
|
||||
tr = self._sm.update(inputs, now_ms)
|
||||
if tr.occurred:
|
||||
logger.info("[Task] %s -> %s (%s)",
|
||||
tr.from_state.value, tr.to_state.value, tr.reason)
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(
|
||||
now_ms, "state", from_state=tr.from_state.value,
|
||||
to_state=tr.to_state.value, reason=tr.reason)
|
||||
await self._on_enter(tr.to_state, now_ms)
|
||||
out = await self._tick(now_ms)
|
||||
if self._logger is not None:
|
||||
self._logger.log_telemetry(
|
||||
now_ms, self._build_telemetry_record(now_ms, inputs, out))
|
||||
|
||||
async def _safe_abort(self) -> None:
|
||||
"""异常兜底：直接进入 RTL 流程（不依赖状态机下一拍转移）。
|
||||
|
||||
异常可能来自采集/控制律/日志任意环节，下一拍未必能正常跑完转移；因此在
|
||||
兜底里显式走一次 RTL 进入动作（停机 + 返航），保证飞机不会停在 offboard
|
||||
速度指令上。
|
||||
"""
|
||||
if self._sm.state is TaskState.RTL:
|
||||
return
|
||||
self._sm.force_state(TaskState.RTL, self._now())
|
||||
self._rtl_entered_ms = self._now()
|
||||
if self._pod_control is not None:
|
||||
try:
|
||||
self._pod_control.stop_tracking()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
# 兜底不走自定义返航：异常后不再信任自身状态/定位，直接交给 PX4 原生 RTL
|
||||
await self._begin_rtl(self._now(), force_fallback=True)
|
||||
except Exception as exc:
|
||||
logger.error("[Task] 异常兜底返航失败: %s", exc)
|
||||
|
||||
# --- 输入采集 -----------------------------------------------------------
|
||||
def _collect_inputs(self, now_ms: int) -> TaskInputs:
|
||||
"""聚合 PX4 / 注入坐标 / 吊舱 / 视觉为一份 TaskInputs 快照。"""
|
||||
own = self._px4.own_state()
|
||||
self._maybe_record_home(own) # 起飞阶段定位可能晚到：每拍补记一次
|
||||
|
||||
injected = self._ground.latest()
|
||||
target_injected = bool(
|
||||
@@ -135,10 +264,12 @@ class TaskRunner:
|
||||
own.latitude_deg, own.longitude_deg,
|
||||
injected.latitude_deg, injected.longitude_deg)
|
||||
|
||||
# 起飞到达判据带容差：悬停/高度估计通常略低于设定目标（实测 SITL 4.99m vs 5.0m），
|
||||
# 无容差会差厘米误判"起飞超时"→ 真机上更明显
|
||||
takeoff_reached = (own.relative_altitude_m
|
||||
>= self._sm_cfg.takeoff_altitude_m)
|
||||
>= self._sm_cfg.takeoff_altitude_m
|
||||
- self._sm_cfg.takeoff_reached_tolerance_m)
|
||||
target_locked = self._bridge.pod_locked(now_ms)
|
||||
vision = bool(self._vision_fn and self._vision_fn())
|
||||
centered, centered_s = self._update_centered(now_ms)
|
||||
|
||||
return TaskInputs(
|
||||
@@ -146,7 +277,6 @@ class TaskRunner:
|
||||
distance_to_target_m=distance,
|
||||
takeoff_reached=takeoff_reached,
|
||||
target_locked=target_locked,
|
||||
vision_has_target=vision,
|
||||
target_centered=centered,
|
||||
centered_duration_s=centered_s,
|
||||
rtl_requested=self._rtl_requested,
|
||||
@@ -174,6 +304,12 @@ class TaskRunner:
|
||||
|
||||
# --- 状态进入动作 -------------------------------------------------------
|
||||
async def _on_enter(self, state: TaskState, now_ms: int) -> None:
|
||||
"""进入状态的**一次性**动作。
|
||||
|
||||
注意：这些动作只在「首次」进入时执行——任务中途可能需要多次经过 GOTO/
|
||||
SEARCH（丢目标回搜），重复 `offboard.start()` / `arm_detection()`
|
||||
（0x07 目标识别开会打断正在进行的跟踪）会造成真机自激，故以标志位守卫。
|
||||
"""
|
||||
if state is TaskState.ARM_TAKEOFF:
|
||||
await self._arm_takeoff()
|
||||
elif state is TaskState.GOTO:
|
||||
@@ -181,17 +317,50 @@ class TaskRunner:
|
||||
logger.error("[Task] offboard 启动失败，紧急终止")
|
||||
self._emergency = True
|
||||
return
|
||||
self._bridge_started = True
|
||||
self._bridge.set_heading_servo_enabled(False) # L2 地理引导
|
||||
if self._pod_control is not None:
|
||||
self._pod_control.arm_detection() # 真机：开启目标识别 + 自动锁定
|
||||
self._start_pod_detection() # 首次：开识别 + 自动锁定
|
||||
elif state is TaskState.SEARCH:
|
||||
self._bridge.set_heading_servo_enabled(False) # 搜索不用控制律
|
||||
self._sweep_dir = _SWEEP_RIGHT # 每次进入从右向起扫
|
||||
self._sweep_since_ms = now_ms
|
||||
elif state is TaskState.TRACK:
|
||||
self._bridge.set_heading_servo_enabled(True) # L1 航向伺服
|
||||
self._bridge.set_heading_servo_enabled(True) # L1 视觉追踪 + 缓慢靠近
|
||||
elif state is TaskState.APPROACH:
|
||||
pass # 悬停由 _tick 持续发零速
|
||||
elif state is TaskState.RTL:
|
||||
self._rtl_entered_ms = now_ms
|
||||
if self._pod_control is not None:
|
||||
self._pod_control.stop_tracking() # 真机：停止跟踪 + 识别关（bridge 停机前）
|
||||
await self._start_rtl()
|
||||
self._pod_control.stop_tracking() # 真机：停止跟踪 + 识别关
|
||||
await self._begin_rtl(now_ms)
|
||||
|
||||
def _start_pod_detection(self) -> None:
|
||||
"""首次到达引导/搜索阶段时开启吊舱目标识别 + 自动锁定（只发一次）。"""
|
||||
if self._pod_control is None or self._pod_detection_on:
|
||||
return
|
||||
self._pod_detection_on = True
|
||||
self._pod_control.arm_detection()
|
||||
|
||||
# --- SEARCH 扫视 --------------------------------------------------------
|
||||
def _search_yaw_rate(self, now_ms: int) -> float:
|
||||
"""SEARCH 扫视角速度：按 sweep_reverse_s 交替左右，0 = 单向扫视。
|
||||
|
||||
吊舱相机刚性固连机体（无电机、不独立转动），故只能靠机头旋转改变观察
|
||||
方向；扫视期间零线速度，飞机保持悬停在参考点附近。
|
||||
"""
|
||||
rate = self._sm_cfg.sweep_rate_dps
|
||||
if rate == 0.0:
|
||||
return 0.0
|
||||
period_s = self._sm_cfg.sweep_reverse_s
|
||||
if period_s > 0.0:
|
||||
if self._sweep_since_ms is None:
|
||||
self._sweep_since_ms = now_ms
|
||||
elif (now_ms - self._sweep_since_ms) >= period_s * 1000.0:
|
||||
self._sweep_since_ms = now_ms
|
||||
self._sweep_dir = (_SWEEP_LEFT if self._sweep_dir == _SWEEP_RIGHT
|
||||
else _SWEEP_RIGHT)
|
||||
sign = 1.0 if self._sweep_dir == _SWEEP_RIGHT else -1.0
|
||||
return sign * rate
|
||||
|
||||
# --- 每周期持续动作 -----------------------------------------------------
|
||||
async def _tick(self, now_ms: int):
|
||||
@@ -199,10 +368,13 @@ class TaskRunner:
|
||||
state = self._sm.state
|
||||
if state in (TaskState.GOTO, TaskState.TRACK):
|
||||
return await self._bridge.step(now_ms)
|
||||
elif state is TaskState.SEARCH:
|
||||
# 原地偏航扫视（同时 poll 吊舱，保遥测新鲜，否则搜到也切不进 TRACK）
|
||||
await self._bridge.search_step(now_ms, self._search_yaw_rate(now_ms))
|
||||
elif state is TaskState.APPROACH:
|
||||
await self._px4.send_brake_hover(now_ms) # 悬停目标上空（保 offboard 心跳）
|
||||
elif state is TaskState.RTL:
|
||||
await self._check_landed()
|
||||
await self._tick_rtl(now_ms)
|
||||
return None
|
||||
# WAIT / ARM_TAKEOFF：无持续动作（待命 / 等起飞高度）
|
||||
|
||||
@@ -238,6 +410,17 @@ class TaskRunner:
|
||||
|
||||
# --- 飞行编排 -----------------------------------------------------------
|
||||
async def _arm_takeoff(self) -> None:
|
||||
"""解锁 + 起飞到悬停高度（每次任务只执行一次）。
|
||||
|
||||
重复执行会导致：机已在空中时再次 arm/takeoff → PX4 拒绝甚至触发保护，
|
||||
且日志看起来像"重新起飞"。故用标志位守卫，重复进入只告警不改动飞机。
|
||||
"""
|
||||
if self._takeoff_done:
|
||||
logger.warning("[Task] 已在飞行中，忽略重复的解锁/起飞请求")
|
||||
return
|
||||
self._takeoff_done = True
|
||||
# 解锁前记录起飞点与初始航向（自定义返航要用；PX4 的 home 此时已固定）
|
||||
self._record_home()
|
||||
try:
|
||||
await self._drone.action.arm()
|
||||
await self._drone.action.set_takeoff_altitude(
|
||||
@@ -254,9 +437,175 @@ class TaskRunner:
|
||||
self._logger.log_event(self._now(), "takeoff_failed",
|
||||
error=str(exc))
|
||||
|
||||
async def _start_rtl(self) -> None:
|
||||
def _record_home(self) -> bool:
|
||||
"""尝试记录起飞点与初始航向；成功返回 True（可重复调用，只记一次）。"""
|
||||
if self._home_lat is not None:
|
||||
return True
|
||||
try:
|
||||
await self._bridge.stop() # 先零速刹车 + 停 offboard/遥测/串口
|
||||
own = self._px4.own_state()
|
||||
except Exception as exc: # 遥测不可用时不阻塞起飞
|
||||
logger.warning("[Task] 读取起飞点失败: %s", exc)
|
||||
return False
|
||||
return self._maybe_record_home(own)
|
||||
|
||||
def _maybe_record_home(self, own) -> bool:
|
||||
"""在**起飞前/起飞中**用首个有效定位记下起飞点与初始航向。
|
||||
|
||||
⚠ SITL 实测踩到的坑：解锁发生在地面站坐标到达后约 0.3s，此时 `gps_info`
|
||||
（2Hz）可能还没到，`position_valid` 仍为 False——若只在解锁那一刻记一次，
|
||||
起飞点就会丢失、自定义返航只能退回 PX4 RTL。故起飞阶段每拍补记。
|
||||
只接受 WAIT / ARM_TAKEOFF 阶段的定位：进入 GOTO 后飞机已在空中，
|
||||
那时的位置不该再当作"起飞点"。
|
||||
"""
|
||||
if self._home_lat is not None:
|
||||
return True
|
||||
if not own.position_valid:
|
||||
return False
|
||||
if self._sm.state not in (TaskState.WAIT, TaskState.ARM_TAKEOFF):
|
||||
return False
|
||||
self._home_lat = own.latitude_deg
|
||||
self._home_lon = own.longitude_deg
|
||||
self._initial_yaw_rad = own.yaw_rad
|
||||
logger.info("[Task] 起飞点已记录: (%.7f, %.7f) 初始航向 %.1f°",
|
||||
self._home_lat, self._home_lon,
|
||||
math.degrees(self._initial_yaw_rad))
|
||||
return True
|
||||
|
||||
# --- 返航（自定义：回起飞点 → 恢复初始航向 → 降落）---------------------
|
||||
def _custom_return_possible(self) -> bool:
|
||||
"""能否走自定义返航（已起飞 + 有起飞点 + 位置有效）。"""
|
||||
if not self._return_cfg.enabled:
|
||||
return False
|
||||
if not self._bridge_started:
|
||||
return False # 没进过 offboard：还在待命/起飞段
|
||||
if self._home_lat is None or self._home_lon is None:
|
||||
return False
|
||||
if self._initial_yaw_rad is None:
|
||||
return False
|
||||
try:
|
||||
return bool(self._px4.own_state().position_valid)
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
async def _begin_rtl(self, now_ms: int, force_fallback: bool = False) -> None:
|
||||
"""进入返航：选择「自定义返航」或「就地降落」或「PX4 RTL 兜底」。"""
|
||||
if self._land_instead_of_rtl:
|
||||
self._rtl_phase = _RTL_LAND
|
||||
await self._start_land()
|
||||
return
|
||||
if force_fallback or not self._custom_return_possible():
|
||||
reason = "异常兜底" if force_fallback else "未起飞/无起飞点"
|
||||
logger.info("[Task] 使用 PX4 原生 RTL（%s）", reason)
|
||||
await self._fallback_rtl()
|
||||
return
|
||||
own = self._px4.own_state()
|
||||
# 转场高度：取"当前高度"与"起飞高度"的较大者 —— 不低于任务高度，
|
||||
# 也不会为了返航额外爬高（这正是原生 RTL 会做的事）。
|
||||
transit_alt = max(own.relative_altitude_m,
|
||||
self._sm_cfg.takeoff_altitude_m)
|
||||
self._return_ctrl = HomingController(
|
||||
self._return_cfg, self._home_lat, self._home_lon,
|
||||
self._initial_yaw_rad, transit_alt)
|
||||
self._return_started_ms = now_ms
|
||||
self._rtl_phase = _RTL_RETURN
|
||||
logger.info("[Task] 自定义返航：回起飞点 (%.7f, %.7f)，转场高度 %.1fm，"
|
||||
"到位后恢复航向 %.1f° 并降落",
|
||||
self._home_lat, self._home_lon, transit_alt,
|
||||
math.degrees(self._initial_yaw_rad))
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(
|
||||
now_ms, "return_home", home_lat=self._home_lat,
|
||||
home_lon=self._home_lon, transit_alt_m=transit_alt,
|
||||
initial_yaw_deg=math.degrees(self._initial_yaw_rad))
|
||||
|
||||
async def _return_step(self, now_ms: int) -> None:
|
||||
"""转场阶段：朝起飞点飞，保持转场高度。"""
|
||||
cfg = self._return_cfg
|
||||
if (self._return_started_ms is not None
|
||||
and now_ms - self._return_started_ms >= cfg.timeout_s * 1000.0):
|
||||
logger.warning("[Task] 返航转场超时 %.0fs，退回 PX4 RTL", cfg.timeout_s)
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(now_ms, "return_timeout",
|
||||
timeout_s=cfg.timeout_s)
|
||||
await self._fallback_rtl()
|
||||
return
|
||||
own = self._px4.own_state()
|
||||
if not own.position_valid:
|
||||
# 定位丢失：不能自主返航，先刹停等恢复（不盲目飞）
|
||||
logger.warning("[Task] 返航中定位失效，刹停等待")
|
||||
await self._px4.send_brake_hover(now_ms)
|
||||
return
|
||||
cmd = self._return_ctrl.update(own.latitude_deg, own.longitude_deg,
|
||||
own.yaw_rad, own.relative_altitude_m)
|
||||
out = ControlOutput(mode=ControlMode.VELOCITY_HEADING, valid=True,
|
||||
vx_body_mps=cmd.vx_mps, vz_mps=cmd.vz_mps,
|
||||
yaw_rate_dps=cmd.yaw_rate_dps)
|
||||
await self._px4.send_velocity_heading(out, now_ms)
|
||||
if cmd.arrived:
|
||||
logger.info("[Task] 已回到起飞点（距 %.1fm），开始恢复初始航向",
|
||||
cmd.distance_m)
|
||||
self._rtl_phase = _RTL_YAW
|
||||
self._yaw_started_ms = now_ms
|
||||
|
||||
async def _yaw_align_step(self, now_ms: int) -> None:
|
||||
"""恢复初始航向阶段：原地转回起飞时刻的航向。"""
|
||||
cfg = self._return_cfg
|
||||
own = self._px4.own_state()
|
||||
err, rate = self._return_ctrl.yaw_align(own.yaw_rad)
|
||||
# 保持转场高度（不额外爬升）
|
||||
out = ControlOutput(mode=ControlMode.VELOCITY_HEADING, valid=True,
|
||||
vx_body_mps=0.0,
|
||||
vz_mps=self._return_ctrl.alt_hold_vz(
|
||||
own.relative_altitude_m),
|
||||
yaw_rate_dps=rate)
|
||||
await self._px4.send_velocity_heading(out, now_ms)
|
||||
|
||||
timed_out = (self._yaw_started_ms is not None
|
||||
and now_ms - self._yaw_started_ms
|
||||
>= cfg.yaw_align_timeout_s * 1000.0)
|
||||
if abs(err) <= cfg.yaw_align_tolerance_deg:
|
||||
logger.info("[Task] 航向已恢复（误差 %.1f°），开始降落", err)
|
||||
elif timed_out:
|
||||
logger.warning("[Task] 航向对齐超时（误差 %.1f°），仍进入降落", err)
|
||||
else:
|
||||
return
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(now_ms, "yaw_restored",
|
||||
err_deg=err, timed_out=timed_out)
|
||||
self._rtl_phase = _RTL_LAND
|
||||
await self._start_land()
|
||||
|
||||
async def _start_land(self) -> None:
|
||||
"""退出 offboard 并垂直降落（不爬升）。"""
|
||||
if self._land_cmd_sent:
|
||||
return
|
||||
self._land_cmd_sent = True
|
||||
# 只退 offboard、保留遥测：落地判定与高度曲线都依赖遥测（SITL 实测过：
|
||||
# 若在这里就全停，日志会冻结在降落前的高度，看不出下降）。
|
||||
stop_for_landing = getattr(self._bridge, "landing_stop", None)
|
||||
try:
|
||||
if stop_for_landing is not None:
|
||||
await stop_for_landing()
|
||||
else:
|
||||
await self._bridge.stop()
|
||||
except Exception as exc:
|
||||
logger.warning("[Task] 停机异常: %s", exc)
|
||||
try:
|
||||
await self._drone.action.land()
|
||||
except Exception as exc:
|
||||
logger.warning("[Task] 降落指令失败，尝试 PX4 RTL 兜底: %s", exc)
|
||||
try:
|
||||
await self._drone.action.return_to_launch()
|
||||
except Exception:
|
||||
pass
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(self._now(), "rtl", action="land")
|
||||
|
||||
async def _fallback_rtl(self) -> None:
|
||||
"""兜底：停机后交给 PX4 原生 RTL（会先爬升到 RTL_RETURN_ALT）。"""
|
||||
self._rtl_phase = _RTL_FALLBACK
|
||||
try:
|
||||
await self._bridge.stop()
|
||||
except Exception as exc:
|
||||
logger.warning("[Task] 停机异常: %s", exc)
|
||||
try:
|
||||
@@ -268,21 +617,57 @@ class TaskRunner:
|
||||
except Exception:
|
||||
pass
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(self._now(), "rtl")
|
||||
self._logger.log_event(self._now(), "rtl", action="rtl")
|
||||
|
||||
async def _tick_rtl(self, now_ms: int) -> None:
|
||||
"""RTL 状态机：按阶段推进（转场 → 恢复航向 → 降落 / 兜底）。"""
|
||||
phase = self._rtl_phase
|
||||
if phase == _RTL_RETURN:
|
||||
await self._return_step(now_ms)
|
||||
elif phase == _RTL_YAW:
|
||||
await self._yaw_align_step(now_ms)
|
||||
elif phase == _RTL_LAND:
|
||||
await self._check_landed()
|
||||
elif phase == _RTL_FALLBACK:
|
||||
await self._check_landed()
|
||||
# 整体落地等待上限：任何阶段都不得无限等
|
||||
if (not self._rtl_done and self._rtl_entered_ms is not None
|
||||
and now_ms - self._rtl_entered_ms
|
||||
>= self._sm_cfg.rtl_timeout_s * 1000.0):
|
||||
logger.warning("[Task] RTL 等待落地超过 %.0fs，结束任务（无法确认落地）",
|
||||
self._sm_cfg.rtl_timeout_s)
|
||||
self._rtl_done = True
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(now_ms, "rtl_timeout",
|
||||
timeout_s=self._sm_cfg.rtl_timeout_s)
|
||||
|
||||
async def _check_landed(self) -> None:
|
||||
"""RTL 状态每周期取一帧 in_air 遥测；落地后 disarm 并标记完成。"""
|
||||
"""RTL 状态每周期取一帧 in_air 遥测；落地后 disarm 并标记完成。
|
||||
|
||||
两个要点：
|
||||
* `in_air()` 是 MAVSDK 遥测流：流挂起时 `async for` 会**永久阻塞**，
|
||||
主循环随之卡死（rtl_done 永不置位）。故加超时兜底，超时下拍重试。
|
||||
* 生成器**复用**（`_in_air_stream`）：每拍新建一个 `in_air()` 会在
|
||||
mavsdk_server 侧反复订阅/退订同一条流（20Hz × 整个 RTL 段），是
|
||||
回调队列的无谓负担；复用后只订阅一次。
|
||||
"""
|
||||
try:
|
||||
async for is_in_air in self._drone.telemetry.in_air():
|
||||
if not is_in_air and not self._rtl_done:
|
||||
self._rtl_done = True
|
||||
try:
|
||||
await self._drone.action.disarm()
|
||||
except Exception:
|
||||
pass
|
||||
break # 只取最新一帧
|
||||
if self._in_air_stream is None:
|
||||
self._in_air_stream = self._drone.telemetry.in_air()
|
||||
async with asyncio.timeout(_IN_AIR_TIMEOUT_S):
|
||||
async for is_in_air in self._in_air_stream:
|
||||
if not is_in_air and not self._rtl_done:
|
||||
self._rtl_done = True
|
||||
try:
|
||||
await self._drone.action.disarm()
|
||||
except Exception:
|
||||
pass
|
||||
break # 只取最新一帧
|
||||
except (TimeoutError, asyncio.TimeoutError):
|
||||
logger.debug("[Task] in_air 遥测超时 %.1fs，下拍重试", _IN_AIR_TIMEOUT_S)
|
||||
except Exception:
|
||||
pass
|
||||
# 建流或消费失败都算流不可用：丢弃缓存，下拍重建（不永久卡在坏流上）
|
||||
self._in_air_stream = None
|
||||
|
||||
# --- 收尾 ---------------------------------------------------------------
|
||||
def _finished(self) -> bool:
|
||||
|
||||
@@ -5,23 +5,29 @@ L2（地理引导 GuidanceLaw）按任务阶段串成一条全流程。本模块
|
||||
+ 守卫」，不驱动 MAVSDK/offboard —— 那些由 runner.py（编排器）负责。每周期吃一份
|
||||
TaskInputs 快照，返回是否转移 + 原因。
|
||||
|
||||
状态链：
|
||||
WAIT ──收到有效坐标──▶ ARM_TAKEOFF ──到起飞高度──▶ GOTO（L2 坐标引导）
|
||||
│ 距目标 < switch_radius
|
||||
│ 且锁定(+可选视觉)
|
||||
状态链（2026-10-15 按实机作业预期重构）：
|
||||
WAIT ──收到有效坐标──▶ ARM_TAKEOFF ──到起飞高度──▶ GOTO（L2 飞到参考点）
|
||||
│ 距参考点 < arrival_radius
|
||||
▼
|
||||
APPROACH ◀──居中稳定── TRACK（L1 航向伺服）
|
||||
│
|
||||
任意状态 ──超时 / 返航 / 紧急──▶ RTL
|
||||
SEARCH（原地偏航扫视找目标）──锁定(+可选视觉)──▶ TRACK
|
||||
│ 搜索超时 （L1 视觉追踪 +
|
||||
│ slow_approach 缓慢靠近）
|
||||
▼ │ 追踪超时
|
||||
RTL ◀────────────────────────────────┘
|
||||
|
||||
设计要点（铁律对齐）：
|
||||
* 纯逻辑：输入/输出都是 dataclass，时间用外部传入的 now_ms（单调毫秒），
|
||||
不碰时钟/IO/网络，可确定性单测。
|
||||
* 全局最高优先级：emergency（丢控/RC 接管/失控）与 rtl_requested（人工返航）
|
||||
→ 任何非 RTL 状态立即转 RTL（安全第一）。
|
||||
* GOTO→TRACK 守卫复用 L1 方案 §2.3：目标锁定 bit2 +（可选）我方视觉第二判据，
|
||||
避免 bit2 粘滞导致误入追踪。
|
||||
* TRACK 丢目标：若注入坐标仍有效 → 退回 GOTO 重新引导；否则 → RTL。
|
||||
* 「参考点」语义：地面给的坐标是**搜索起点**，不是终点——单点坐标精度不足以
|
||||
直接覆盖目标，故进入到达半径即转 SEARCH 扫视，由吊舱/视觉去发现目标。
|
||||
* GOTO/SEARCH→TRACK 守卫复用 L1 方案 §2.3：目标锁定 bit2 +（可选）我方视觉
|
||||
第二判据，避免 bit2 粘滞导致误入追踪。
|
||||
* TRACK 丢目标 → 回 SEARCH 重新扫视（**不回 GOTO**：飞机本就在参考点附近，
|
||||
回 GOTO 只会反复重跑进入动作，而参考点坐标本身不精确）；坐标同时失效 → RTL。
|
||||
* TRACK 不因「目标居中」退出：居中正是吊舱对准目标、应当继续缓慢靠近的时刻。
|
||||
接近的收尾只由超时 / 人工返航 / 紧急终止（无距离输入，判不出"已到目标"）。
|
||||
* 阈值/超时全外置（TaskStateConfig），代码不写死数值。
|
||||
"""
|
||||
|
||||
@@ -35,9 +41,10 @@ class TaskState(Enum):
|
||||
|
||||
WAIT = "wait" # 待命：等地面坐标
|
||||
ARM_TAKEOFF = "arm_takeoff" # 解锁 + 起飞到悬停高度
|
||||
GOTO = "goto" # L2 坐标引导（飞向注入坐标）
|
||||
TRACK = "track" # L1 航向伺服（吊舱角度追踪）
|
||||
APPROACH = "approach" # 接近/悬停目标上空
|
||||
GOTO = "goto" # L2 地理引导：飞向参考点（地面给的搜索起点）
|
||||
SEARCH = "search" # 到点后原地偏航扫视搜索目标（吊舱/视觉）
|
||||
TRACK = "track" # L1 视觉追踪 + 缓慢靠近（slow_approach 保守速度）
|
||||
APPROACH = "approach" # 到点悬停（真机可选收尾；SITL/常规流程不用）
|
||||
RTL = "rtl" # 返航 + 落地 + 上锁（终态，reset() 才回 WAIT）
|
||||
|
||||
|
||||
@@ -45,15 +52,19 @@ class TaskState(Enum):
|
||||
class TaskStateConfig:
|
||||
"""状态机配置（全外置；数值均为占位，须台架/实飞标定后回填）。"""
|
||||
|
||||
switch_radius_m: float = 30.0 # GOTO→TRACK 切换半径（米）
|
||||
require_vision: bool = False # GOTO→TRACK 是否需视觉第二判据（S3 起开）
|
||||
arrival_radius_m: float = 30.0 # GOTO→SEARCH 到达参考点半径（米）
|
||||
arm_takeoff_timeout_s: float = 60.0 # 起飞阶段超时
|
||||
goto_timeout_s: float = 120.0 # 坐标引导超时
|
||||
track_timeout_s: float = 120.0 # 追踪超时
|
||||
center_hold_s: float = 3.0 # TRACK→APPROACH 居中稳定时长（秒）
|
||||
approach_hover_s: float = 10.0 # APPROACH 悬停时长（到点 → RTL）
|
||||
goto_timeout_s: float = 120.0 # 飞向参考点超时
|
||||
search_timeout_s: float = 20.0 # 搜索超时（未发现目标 → 返航）
|
||||
track_timeout_s: float = 20.0 # 追踪/靠近段上限（到点 → 返航）
|
||||
approach_hover_s: float = 10.0 # APPROACH 悬停时长（到点 → RTL；已非主流程）
|
||||
takeoff_altitude_m: float = 5.0 # 起飞目标高度（runner 起飞 + takeoff_reached 判据）
|
||||
takeoff_reached_tolerance_m: float = 0.5 # 起飞到达判据容差（低于目标不超过此值也算到达；
|
||||
# 悬停/高度估计误差下避免差厘米误判"起飞超时"）
|
||||
center_threshold_deg: float = 1.0 # 脱靶量居中阈值（|off_target| < 此值判居中）
|
||||
sweep_rate_dps: float = 15.0 # SEARCH 原地扫视角速度（度/秒）
|
||||
sweep_reverse_s: float = 4.0 # SEARCH 扫视换向周期（秒）；0 = 单向扫视
|
||||
rtl_timeout_s: float = 180.0 # RTL 等待落地上限（秒）；超时即结束任务，防无限等待
|
||||
loop_frequency_hz: float = 20.0 # runner 主循环频率（bridge.step 节拍）
|
||||
|
||||
|
||||
@@ -66,12 +77,11 @@ class TaskInputs:
|
||||
"""
|
||||
|
||||
target_injected: bool = False # 地面坐标有效注入（新鲜、未超龄）
|
||||
distance_to_target_m: float = math.inf # 到注入目标的水平距离（GOTO 用）
|
||||
distance_to_target_m: float = math.inf # 到参考点的水平距离（GOTO 用）
|
||||
takeoff_reached: bool = False # 已到达起飞目标高度
|
||||
target_locked: bool = False # 吊舱锁定（bit2 + 遥测新鲜）
|
||||
vision_has_target: bool = False # 我方视觉第二判据（可选，require_vision 时必看）
|
||||
target_centered: bool = False # 目标居中（脱靶量长期≈0）
|
||||
centered_duration_s: float = 0.0 # 居中持续时长（外部累加，秒）
|
||||
target_locked: bool = False # 目标有效（吊舱 bit2 + 遥测新鲜 + LOS 一致性，见 bridge）
|
||||
target_centered: bool = False # 目标居中（脱靶量≈0）——仅供日志/诊断
|
||||
centered_duration_s: float = 0.0 # 居中持续时长（外部累加，秒）——不再作转移判据
|
||||
rtl_requested: bool = False # 人工返航命令
|
||||
emergency: bool = False # 紧急（丢控 / RC 接管 / 失控）
|
||||
|
||||
@@ -103,6 +113,15 @@ class TaskStateMachine:
|
||||
self._state = TaskState.WAIT
|
||||
self._entered_ms = 0
|
||||
|
||||
def force_state(self, state: TaskState, now_ms: int) -> None:
|
||||
"""强制落到某状态（仅供编排器异常兜底；不产生 TaskTransition）。
|
||||
|
||||
正常转移一律走 update()。此入口用于「主循环抛异常、必须立刻进 RTL」这类
|
||||
非判据驱动的安全路径——此时再喂一拍快照不可靠。
|
||||
"""
|
||||
self._state = state
|
||||
self._entered_ms = now_ms
|
||||
|
||||
def update(self, inputs: TaskInputs, now_ms: int) -> TaskTransition:
|
||||
"""吃一份快照，返回转移结果；无转移时 occurred=False、to_state 保持原状态。"""
|
||||
state = self._state
|
||||
@@ -127,23 +146,31 @@ class TaskStateMachine:
|
||||
elif state is TaskState.GOTO:
|
||||
if not inputs.target_injected:
|
||||
return self._goto(TaskState.RTL, "注入坐标失效", now_ms)
|
||||
if (inputs.distance_to_target_m <= self._cfg.switch_radius_m
|
||||
and self._lock_ok(inputs)):
|
||||
return self._goto(TaskState.TRACK, "进入追踪半径且目标锁定", now_ms)
|
||||
# 到参考点即结束地理引导 → 转扫视搜索（参考点是搜索起点，不是终点）
|
||||
if inputs.distance_to_target_m <= self._cfg.arrival_radius_m:
|
||||
return self._goto(TaskState.SEARCH, "已到参考点，开始搜索", now_ms)
|
||||
if self._elapsed_s(now_ms) >= self._cfg.goto_timeout_s:
|
||||
return self._goto(TaskState.RTL, "坐标引导超时", now_ms)
|
||||
return self._goto(TaskState.RTL, "飞向参考点超时", now_ms)
|
||||
|
||||
elif state is TaskState.SEARCH:
|
||||
# 扫视中一旦发现并锁定目标 → 进追踪（缓慢靠近）
|
||||
if self._lock_ok(inputs):
|
||||
return self._goto(TaskState.TRACK, "搜索发现目标并锁定", now_ms)
|
||||
if not inputs.target_injected:
|
||||
return self._goto(TaskState.RTL, "注入坐标失效", now_ms)
|
||||
if self._elapsed_s(now_ms) >= self._cfg.search_timeout_s:
|
||||
return self._goto(TaskState.RTL, "搜索超时未发现目标", now_ms)
|
||||
|
||||
elif state is TaskState.TRACK:
|
||||
if not inputs.target_locked:
|
||||
# 丢目标：坐标仍有效 → 退回 GOTO 重新引导；否则终止返航。
|
||||
# 丢目标：坐标仍有效 → 回参考点附近重新扫视；否则终止返航。
|
||||
if inputs.target_injected:
|
||||
return self._goto(TaskState.GOTO, "目标丢失，退回坐标引导", now_ms)
|
||||
return self._goto(TaskState.SEARCH, "目标丢失，重新搜索", now_ms)
|
||||
return self._goto(TaskState.RTL, "目标丢失且坐标失效", now_ms)
|
||||
if (inputs.target_centered
|
||||
and inputs.centered_duration_s >= self._cfg.center_hold_s):
|
||||
return self._goto(TaskState.APPROACH, "目标居中稳定", now_ms)
|
||||
# 注意：不再有「居中稳定 → APPROACH」分支。居中表示吊舱已对准目标，
|
||||
# 正是应当继续按 slow_approach 缓慢靠近的时刻，不是终止条件。
|
||||
if self._elapsed_s(now_ms) >= self._cfg.track_timeout_s:
|
||||
return self._goto(TaskState.RTL, "追踪超时", now_ms)
|
||||
return self._goto(TaskState.RTL, "追踪/靠近超时", now_ms)
|
||||
|
||||
elif state is TaskState.APPROACH:
|
||||
if self._elapsed_s(now_ms) >= self._cfg.approach_hover_s:
|
||||
@@ -154,15 +181,13 @@ class TaskStateMachine:
|
||||
|
||||
# --- 内部辅助 -----------------------------------------------------------
|
||||
def _lock_ok(self, inputs: TaskInputs) -> bool:
|
||||
"""GOTO→TRACK 的目标锁定判据（复用 L1 方案 §2.3）。
|
||||
"""进 TRACK 的目标判据（GOTO/SEARCH→TRACK）。
|
||||
|
||||
需吊舱锁定 bit2；若 require_vision 还需我方视觉第二判据（bit2 粘滞兜底）。
|
||||
`target_locked` 已由编排层合成：吊舱 bit2 + 遥测新鲜 + **LOS 运动学一致性**
|
||||
（见 bridge.pod_locked / los_consistency）。状态机不再重复做视觉判据 ——
|
||||
吊舱已取代双目+YOLO 成为唯一目标源，"我方视觉"不复存在。
|
||||
"""
|
||||
if not inputs.target_locked:
|
||||
return False
|
||||
if self._cfg.require_vision and not inputs.vision_has_target:
|
||||
return False
|
||||
return True
|
||||
return inputs.target_locked
|
||||
|
||||
def _elapsed_s(self, now_ms: int) -> float:
|
||||
"""进入当前状态的时长（秒）。_entered_ms=0（未打戳）时返回 0。"""
|
||||
|
||||
@@ -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.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user