first commit
This commit is contained in:
@@ -0,0 +1,58 @@
|
||||
# utils —— reference-c++ 两个子项目的 Python 移植
|
||||
|
||||
将 `reference-c++/` 下的 `target_tracker`（多目标追踪过滤器）与
|
||||
`visual_track_control`（视觉跟踪控制律）移植为 Python，**两包互不依赖**
|
||||
（唯一的数据转换点在胶水层 demo）。
|
||||
|
||||
## 目录结构
|
||||
|
||||
```
|
||||
utils/
|
||||
├── target_tracker/ # 包 1：卡尔曼滤波多目标追踪（无第三方硬依赖，仅 numpy）
|
||||
│ └── README.md # 接口清单 + C++ 差异
|
||||
├── visual_track_control/ # 包 2：三通道控制律 + MAVSDK offboard 网关
|
||||
│ └── README.md # 接口清单 + C++ 差异
|
||||
├── demo_track_control.py # 胶水层集成演示（模拟检测 → 追踪 → 控制律 → 发送）
|
||||
├── config/track_control.json # 控制律配置（拷贝自 reference-c++）
|
||||
├── tests/ # pytest 测试（对应 C++ 全部 13 个测试文件）
|
||||
│ ├── target_tracker/ # 27 条
|
||||
│ └── visual_track_control/ # 78 条
|
||||
├── conftest.py # 保证可从任意工作目录导入两包
|
||||
└── pytest.ini # 收集 *_test.py 命名（与 C++ 测试文件名对齐）
|
||||
```
|
||||
|
||||
## 环境
|
||||
|
||||
conda env `pytorch`（`D:\program file\conda\envs\pytorch`），Python 3.12，
|
||||
依赖：numpy、mavsdk、pytest。
|
||||
|
||||
## 运行测试
|
||||
|
||||
```bash
|
||||
cd utils
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
|
||||
```
|
||||
|
||||
105 条测试全部通过（含 C++ 验收用例的逐条移植：§9.4 六条硬验收、A1/A2/A4
|
||||
回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）。
|
||||
|
||||
## 运行演示
|
||||
|
||||
```bash
|
||||
# mock 传输（本地记录指令，无实机）
|
||||
python utils/demo_track_control.py --speedup 10
|
||||
|
||||
# MAVSDK 实发（需飞控/SITL 在 udp://:14540）
|
||||
python utils/demo_track_control.py --transport mavsdk
|
||||
```
|
||||
|
||||
时间线 13 s（20 Hz）：正常跟踪 6 s（vx 从零渐变 → 逼近限速 3 m/s）→
|
||||
目标消失 4 s（Coast 预测，60 帧后轨道判亡）→ 判零速刹车 3 s。
|
||||
|
||||
## 移植约定
|
||||
|
||||
- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支
|
||||
映射保留 C++ A2 回归语义，详见包 2 README。
|
||||
- 测试逐条移植 C++ gtest 断言（`EXPECT_DOUBLE_EQ` → 精确 `==`，
|
||||
`EXPECT_NEAR` → `pytest.approx`）；MAVSDK 部分因无 SITL/实机，仅 mock 验证。
|
||||
- 行为修复项（C1/C2/C4、A1/A2/A4、S6）在测试文件头注释中标注对应章节。
|
||||
Binary file not shown.
@@ -0,0 +1,48 @@
|
||||
{
|
||||
"_comment": "视觉跟踪控制律配置。全部数值为占位示例（对应 docs/物理追踪思路.md 第 8 章待定项），键名后缀标注单位；标定完成前仅限回放验证，不得用于实飞。",
|
||||
|
||||
"control": {
|
||||
"frequency_hz": 20.0,
|
||||
"visual_stale_ms": 200,
|
||||
"radar_stale_ms": 500,
|
||||
"attitude_stale_ms": 300
|
||||
},
|
||||
|
||||
"camera": {
|
||||
"image_width": 1920,
|
||||
"image_height": 1080,
|
||||
"fov_h_deg": 60.0,
|
||||
"fov_v_deg": 35.0
|
||||
},
|
||||
|
||||
"heading": {
|
||||
"mode": "rate",
|
||||
"gain": 1.0,
|
||||
"yaw_rate_limit_dps": 30.0,
|
||||
"yaw_slew_limit_dps2": 60.0
|
||||
},
|
||||
|
||||
"vertical": {
|
||||
"vz_gain_mps_per_deg": 0.1,
|
||||
"vz_limit_mps": 2.0
|
||||
},
|
||||
|
||||
"distance": {
|
||||
"d_exp_m": 10.0,
|
||||
"kp": 0.5,
|
||||
"ki": 0.05,
|
||||
"kd": 0.1,
|
||||
"integral_limit": 2.0,
|
||||
"derivative_filter_coef": 0.2,
|
||||
"approach_velocity_limit_mps": 3.0,
|
||||
"retreat_velocity_limit_mps": 1.0,
|
||||
"no_distance_action": "hold",
|
||||
"no_distance_approach_limit_mps": 0.5
|
||||
},
|
||||
|
||||
"accel_limit": {
|
||||
"ax_mps2": 2.0,
|
||||
"ay_mps2": 2.0,
|
||||
"az_mps2": 1.5
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
"""pytest 根配置：保证无论从哪个工作目录启动 pytest，均可导入 utils/ 下的两个包。"""
|
||||
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
_UTILS_DIR = Path(__file__).resolve().parent
|
||||
if str(_UTILS_DIR) not in sys.path:
|
||||
sys.path.insert(0, str(_UTILS_DIR))
|
||||
@@ -0,0 +1,207 @@
|
||||
"""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_radar(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)
|
||||
radar_m, associated = simulate_radar(tick, normal_ticks, period_s)
|
||||
|
||||
result = tracker.update(detections)
|
||||
snap = ControlSnapshot(
|
||||
track=track_result_to_state(result),
|
||||
track_time_ms=now_ms, # 每拍取样即打戳（视觉始终新鲜）
|
||||
radar_distance_m=radar_m,
|
||||
associated_to_target=associated,
|
||||
radar_time_ms=now_ms if associated else 0,
|
||||
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()
|
||||
@@ -0,0 +1,3 @@
|
||||
[pytest]
|
||||
testpaths = tests
|
||||
python_files = *_test.py test_*.py
|
||||
@@ -0,0 +1,29 @@
|
||||
# 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 条移植测试锁定。
|
||||
@@ -0,0 +1,15 @@
|
||||
"""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.
@@ -0,0 +1,111 @@
|
||||
"""单目标 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
|
||||
@@ -0,0 +1,269 @@
|
||||
"""多目标追踪过滤器（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
|
||||
@@ -0,0 +1,110 @@
|
||||
"""轨道（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
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,107 @@
|
||||
"""kalman_box_filter_test.py —— KalmanBoxFilter 单元测试。
|
||||
|
||||
移植自 reference-c++/target_tracker/tests/kalman_box_filter_test.cpp。
|
||||
覆盖（视觉追踪技术路线 §4）：初始化状态、静止目标收敛、匀速目标速度收敛
|
||||
且预测超前一个 dt、未初始化 predict 返回 None。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from target_tracker.kalman_box_filter import BoxCXCYWH, KalmanBoxFilter
|
||||
|
||||
|
||||
def test_uninitialized_predict_returns_none():
|
||||
kf = KalmanBoxFilter()
|
||||
assert not kf.initialized
|
||||
assert kf.predict() is None
|
||||
|
||||
# 未初始化时 update 无副作用
|
||||
kf.update(BoxCXCYWH(100.0, 100.0, 20.0, 20.0))
|
||||
assert not kf.initialized
|
||||
assert kf.predict() is None
|
||||
for v in kf.state:
|
||||
assert v == 0.0
|
||||
|
||||
|
||||
def test_initiate_sets_state_correctly():
|
||||
kf = KalmanBoxFilter()
|
||||
kf.initiate(BoxCXCYWH(320.0, 240.0, 40.0, 30.0))
|
||||
assert kf.initialized
|
||||
|
||||
expected = [320.0, 240.0, 40.0, 30.0, 0.0, 0.0, 0.0, 0.0]
|
||||
state = kf.state
|
||||
for i, want in enumerate(expected):
|
||||
assert state[i] == want, f"index={i}"
|
||||
|
||||
# 初始化后 predict 返回状态前 4 维（速度为零 → 位置不变，§4.4）
|
||||
pred = kf.predict()
|
||||
assert pred is not None
|
||||
assert pred.cx == pytest.approx(320.0, abs=1e-9)
|
||||
assert pred.cy == pytest.approx(240.0, abs=1e-9)
|
||||
assert pred.w == pytest.approx(40.0, abs=1e-9)
|
||||
assert pred.h == pytest.approx(30.0, abs=1e-9)
|
||||
|
||||
|
||||
def test_static_target_converges_to_observation():
|
||||
kf = KalmanBoxFilter()
|
||||
z = BoxCXCYWH(100.0, 150.0, 40.0, 60.0)
|
||||
kf.initiate(BoxCXCYWH(104.0, 156.0, 42.0, 62.0)) # 带初始偏差，验证收敛能力
|
||||
|
||||
for _ in range(50):
|
||||
kf.predict()
|
||||
kf.update(z)
|
||||
|
||||
state = kf.state
|
||||
assert state[0] == pytest.approx(z.cx, abs=0.5) # cx
|
||||
assert state[1] == pytest.approx(z.cy, abs=0.5) # cy
|
||||
assert state[2] == pytest.approx(z.w, abs=0.5) # w
|
||||
assert state[3] == pytest.approx(z.h, abs=0.5) # h
|
||||
# 静止目标速度应收敛到约 0
|
||||
assert state[4] == pytest.approx(0.0, abs=0.1)
|
||||
assert state[5] == pytest.approx(0.0, abs=0.1)
|
||||
|
||||
|
||||
def test_constant_velocity_converges_and_predict_leads_one_dt():
|
||||
kf = KalmanBoxFilter()
|
||||
vx = 3.0 # 像素/帧
|
||||
vy = 2.0 # 像素/帧
|
||||
|
||||
# 目标从 (100, 100) 出发匀速运动，每帧观测一次（predict → update）
|
||||
z = BoxCXCYWH(100.0, 100.0, 30.0, 30.0)
|
||||
kf.initiate(z)
|
||||
for _ in range(60):
|
||||
kf.predict()
|
||||
z.cx += vx
|
||||
z.cy += vy
|
||||
kf.update(z)
|
||||
|
||||
state = kf.state
|
||||
# 速度收敛到真实速度（像素/帧）
|
||||
assert state[4] == pytest.approx(vx, abs=0.3), "vx 未收敛"
|
||||
assert state[5] == pytest.approx(vy, abs=0.3), "vy 未收敛"
|
||||
# 位置贴合当前观测
|
||||
assert state[0] == pytest.approx(z.cx, abs=1.5)
|
||||
assert state[1] == pytest.approx(z.cy, abs=1.5)
|
||||
|
||||
# 再预测一帧：应超前当前观测一个 dt（≈ 速度 × 1 帧）
|
||||
pred = kf.predict()
|
||||
assert pred is not None
|
||||
assert pred.cx == pytest.approx(z.cx + vx, abs=1.0), "预测未超前一个 dt"
|
||||
assert pred.cy == pytest.approx(z.cy + vy, abs=1.0), "预测未超前一个 dt"
|
||||
|
||||
|
||||
def test_small_target_noise_scaling_stable():
|
||||
# 动态噪声不退化：小尺寸目标多帧观测仍稳定收敛（§4.2 噪声与尺寸成比例）
|
||||
kf = KalmanBoxFilter()
|
||||
z = BoxCXCYWH(50.0, 60.0, 4.0, 4.0) # 小目标（4×4 像素）
|
||||
kf.initiate(z)
|
||||
for _ in range(30):
|
||||
kf.predict()
|
||||
kf.update(z)
|
||||
state = kf.state
|
||||
assert state[0] == pytest.approx(z.cx, abs=0.5)
|
||||
assert state[1] == pytest.approx(z.cy, abs=0.5)
|
||||
assert math.isfinite(state[2])
|
||||
assert math.isfinite(state[3])
|
||||
@@ -0,0 +1,364 @@
|
||||
"""target_tracker_test.py —— TargetTracker 单元测试。
|
||||
|
||||
移植自 reference-c++/target_tracker/tests/target_tracker_test.cpp。
|
||||
逐条覆盖 docs/视觉追踪技术路线.md §9.4 行为一致性验收用例（硬验收标准）：
|
||||
1. 空检测帧序列：Coast 持续 max_lost_frames 帧，第 max_lost_frames+1 帧判丢
|
||||
2. ID 严格递增不复用（含 Reset 后归零）
|
||||
3. 锁定不被抢占；锁定目标死亡后切换到新目标
|
||||
4. min_hits 回退：无已确认实测轨道时新轨道也可当选主目标
|
||||
5. cost=1.0 恰好不构成匹配（严格小于）
|
||||
6. 置信度优先：两轨道竞争同一检测，置信度高者获胜
|
||||
另含 NaN/Inf 防御与 §2.3/§6.3/§9.5 行为测试。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from target_tracker.target_tracker import (Detection, FrameShape, TargetTracker,
|
||||
TrackerConfig)
|
||||
|
||||
|
||||
def make_det(x1, y1, x2, y2, conf, cls=0):
|
||||
return Detection(x1, y1, x2, y2, conf, cls)
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 1（§9.4-1）：空检测帧序列
|
||||
# ============================================================================
|
||||
def test_empty_frames_coast_then_lost():
|
||||
tracker = TargetTracker() # 默认配置 max_lost_frames = 8
|
||||
assert tracker.get_config().max_lost_frames == 8
|
||||
|
||||
# 第 1 帧：给出一个检测，建立轨道并锁定为主目标
|
||||
r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.9)])
|
||||
assert r.tracked
|
||||
assert r.primary_id is not None
|
||||
assert not r.is_predicted # 当帧有实测
|
||||
assert r.lost_frames == 0
|
||||
assert r.n_active == 1
|
||||
assert r.box is not None
|
||||
assert r.raw is not None # 当帧关联到原始检测框
|
||||
|
||||
# 后续空检测帧：应持续 Coast 共 max_lost_frames 帧（is_predicted=True）
|
||||
for i in range(1, 9):
|
||||
r = tracker.update([])
|
||||
assert r.tracked, f"第 {i} 帧 Coast 期间不应判丢"
|
||||
assert r.is_predicted, f"第 {i} 帧应为纯预测输出"
|
||||
assert r.lost_frames == i
|
||||
assert r.primary_id is not None
|
||||
assert r.box is not None # Coast 期间继续输出预测框
|
||||
assert r.raw is None # Coast 时 raw 为空（§2.3）
|
||||
assert r.confidence == pytest.approx(0.9) # 继承最近实测置信度（§2.3）
|
||||
|
||||
# 第 max_lost_frames + 1 帧：轨道死亡回收 → tracked=False
|
||||
r = tracker.update([])
|
||||
assert not r.tracked
|
||||
assert r.primary_id is None
|
||||
assert r.box is None
|
||||
assert r.lost_frames == 0 # 丢失时 lost_frames=0（§2.3）
|
||||
assert r.n_active == 0
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 2（§9.4-2）：ID 单调性
|
||||
# ============================================================================
|
||||
def test_id_strictly_increasing_no_reuse():
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 1 # 缩短 Coast 窗口，加速轨道死亡
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# 目标 A 出现 → id 0
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9)])
|
||||
assert r.primary_id is not None
|
||||
id_a = r.primary_id
|
||||
assert id_a == 0
|
||||
|
||||
# 目标 A 消失至死亡（max_lost=1：第 2 帧 Coast、第 3 帧死亡回收）
|
||||
tracker.update([])
|
||||
r = tracker.update([])
|
||||
assert not r.tracked
|
||||
assert tracker.active_count() == 0
|
||||
|
||||
# 目标 B 出现（同一位置）：ID 不复用，严格递增 → id 1
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9)])
|
||||
assert r.primary_id is not None
|
||||
id_b = r.primary_id
|
||||
assert id_b == 1
|
||||
assert id_b > id_a
|
||||
|
||||
# 目标 C 同帧新建 → id 2（同帧多目标亦严格递增）
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9),
|
||||
make_det(400.0, 300.0, 430.0, 330.0, 0.8)])
|
||||
assert tracker.active_count() == 2
|
||||
max_id = max(tracker.active_tracks().keys())
|
||||
assert max_id == 2
|
||||
|
||||
# Reset 后计数器归零（§6.2）：新轨道重新从 id 0 开始
|
||||
tracker.reset()
|
||||
assert tracker.active_count() == 0
|
||||
assert not tracker.last_result().tracked
|
||||
r = tracker.update([make_det(50.0, 50.0, 70.0, 70.0, 0.9)])
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == 0
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 3（§9.4-3）：锁定不被抢占
|
||||
# ============================================================================
|
||||
def test_lock_not_stolen_and_switch_after_death():
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 3
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# 低置信度目标 A 建立并锁定
|
||||
r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.4)])
|
||||
assert r.primary_id is not None
|
||||
id_a = r.primary_id
|
||||
|
||||
# A 消失进入 Coast；期间出现更高置信度新目标 B（远离 A，不会被误关联）
|
||||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == id_a, "Coast 期间锁定不应被高置信度目标抢占"
|
||||
assert r.is_predicted
|
||||
assert tracker.active_count() == 2 # A（Coast）与 B 并存
|
||||
|
||||
# 继续 Coast，锁定依旧延续
|
||||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||||
assert r.primary_id == id_a
|
||||
assert r.is_predicted
|
||||
|
||||
# A 继续丢失直至死亡（max_lost=3：lost 达 4 帧时回收）
|
||||
for _ in range(2):
|
||||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||||
# A 已死亡删除 → 锁定解除，重新选举切换到 B（此时 B 已确认且实测）
|
||||
assert r.tracked
|
||||
assert r.primary_id != id_a, "锁定目标死亡后应切换到新目标"
|
||||
assert not r.is_predicted
|
||||
|
||||
# 后续 B 持续作为主目标
|
||||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id != id_a
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 4（§9.4-4）：min_hits 回退
|
||||
# ============================================================================
|
||||
def test_min_hits_fallback_allows_new_track():
|
||||
cfg = TrackerConfig()
|
||||
cfg.min_hits = 5 # 抬高确认门槛，保证测试期间轨道始终未确认
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# 首帧新建轨道 hits=1 < min_hits：候选池 confirmed 为空 → 退回 measured
|
||||
r = tracker.update([make_det(200.0, 150.0, 240.0, 190.0, 0.6)])
|
||||
assert r.tracked, "无已确认轨道时新轨道应能当选主目标"
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == 0
|
||||
assert not r.is_predicted
|
||||
|
||||
# 再跟几帧（hits 增长但仍未达 min_hits=5），依旧当选
|
||||
for _ in range(3):
|
||||
r = tracker.update([make_det(200.0, 150.0, 240.0, 190.0, 0.6)])
|
||||
assert r.tracked
|
||||
assert r.primary_id == 0
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 5（§9.4-5）：cost 边界（cost = 1.0 恰好不匹配）
|
||||
# ============================================================================
|
||||
def test_cost_exactly_one_does_not_match():
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 1
|
||||
cfg.max_association_dist = 200.0
|
||||
cfg.dist_weight = 1.0 # 纯距离项，IoU 项权重为 0 且两框零交集
|
||||
cfg.iou_weight = 0.0
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# 建立轨道：中心 (100,100)
|
||||
r = tracker.update([make_det(90.0, 90.0, 110.0, 110.0, 0.9)])
|
||||
assert r.primary_id is not None
|
||||
id_a = r.primary_id
|
||||
|
||||
# 下一帧检测中心 (100,300)：距离 = 200 = Dmax → cost = 1.0，恰好不匹配
|
||||
r = tracker.update([make_det(90.0, 290.0, 110.0, 310.0, 0.9)])
|
||||
# 未匹配 → 原轨道 Coast、检测新建轨道；两个目标并存
|
||||
assert tracker.active_count() == 2, "cost=1.0 不应匹配，应新建轨道"
|
||||
# 原轨道（锁定）进入 Coast：lost_frames=1
|
||||
assert r.tracked
|
||||
assert r.is_predicted
|
||||
assert r.primary_id == id_a
|
||||
assert r.lost_frames == 1
|
||||
|
||||
# 对照：距离略小于阈值（199px → cost=0.995 < 1.0）应匹配成功
|
||||
tracker2 = TargetTracker(cfg)
|
||||
r = tracker2.update([make_det(90.0, 90.0, 110.0, 110.0, 0.9)])
|
||||
id_b = r.primary_id
|
||||
r = tracker2.update([make_det(90.0, 289.0, 110.0, 309.0, 0.9)])
|
||||
assert tracker2.active_count() == 1, "cost<1.0 应匹配成功，不应新建轨道"
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == id_b
|
||||
assert not r.is_predicted # 实测命中
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 6（§9.4-6）：置信度优先
|
||||
# ============================================================================
|
||||
def test_higher_confidence_track_wins_detection():
|
||||
tracker = TargetTracker()
|
||||
|
||||
# 同帧建立两条轨道：A 置信度 0.5、B 置信度 0.9，相距 80px（不互相重叠）
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.5),
|
||||
make_det(180.0, 100.0, 200.0, 120.0, 0.9)])
|
||||
assert tracker.active_count() == 2
|
||||
|
||||
# 找出两条轨道 ID（按置信度区分）
|
||||
id_low = None
|
||||
id_high = None
|
||||
for track_id, track in tracker.active_tracks().items():
|
||||
if track.confidence > 0.8:
|
||||
id_high = track_id
|
||||
else:
|
||||
id_low = track_id
|
||||
assert id_low is not None
|
||||
assert id_high is not None
|
||||
|
||||
# 下一帧只给一个检测，放在两轨道预测中心之间（各距 40px，对称）：
|
||||
# 距离项对两者相同，贪婪按置信度降序 → 高置信度轨道 B 优先匹配
|
||||
r = tracker.update([make_det(140.0, 100.0, 160.0, 120.0, 0.7)])
|
||||
|
||||
tracks = tracker.active_tracks()
|
||||
assert len(tracks) == 2
|
||||
# 获胜者：命中（lost=0、hits=2、置信度更新）
|
||||
assert tracks[id_high].lost_count == 0, "高置信度轨道应获胜"
|
||||
assert tracks[id_high].hits == 2
|
||||
assert tracks[id_high].confidence == pytest.approx(0.7)
|
||||
# 失败者：未匹配进入 Coast（lost=1、hits 不变）
|
||||
assert tracks[id_low].lost_count == 1, "低置信度轨道应落选"
|
||||
assert tracks[id_low].hits == 1
|
||||
assert tracks[id_low].confidence == pytest.approx(0.5)
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# NaN/Inf 防御（评审修复项）
|
||||
# ============================================================================
|
||||
def test_non_finite_detections_are_discarded():
|
||||
tracker = TargetTracker()
|
||||
nan = math.nan
|
||||
inf = math.inf
|
||||
|
||||
# 第 1 帧：建立正常轨道
|
||||
r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.9)])
|
||||
assert r.tracked
|
||||
|
||||
# 第 2 帧：混入 NaN 坐标 / Inf 坐标 / NaN 置信度检测与一个正常检测：
|
||||
# 非有限检测全部被丢弃，仅正常检测参与关联，不新建轨道
|
||||
r = tracker.update([make_det(nan, 100.0, 140.0, 140.0, 0.9),
|
||||
make_det(100.0, -inf, 140.0, 140.0, 0.9),
|
||||
make_det(100.0, 100.0, nan, 140.0, 0.9),
|
||||
make_det(102.0, 102.0, 142.0, 142.0, nan),
|
||||
make_det(101.0, 101.0, 141.0, 141.0, 0.9)])
|
||||
assert tracker.active_count() == 1, "非有限检测不应新建轨道"
|
||||
assert r.tracked
|
||||
assert not r.is_predicted # 正常检测命中，实测输出
|
||||
# 主目标输出未被 NaN 污染
|
||||
assert math.isfinite(r.center[0])
|
||||
assert math.isfinite(r.center[1])
|
||||
assert r.box is not None
|
||||
assert math.isfinite(r.box.x1) and math.isfinite(r.box.y1)
|
||||
assert math.isfinite(r.box.x2) and math.isfinite(r.box.y2)
|
||||
assert math.isfinite(r.confidence)
|
||||
assert r.raw is not None # 实测帧 raw 有值
|
||||
|
||||
# 第 3 帧：仅含 NaN 检测 → 等价空帧，轨道 Coast 不崩溃
|
||||
r = tracker.update([make_det(nan, nan, nan, nan, nan)])
|
||||
assert r.tracked
|
||||
assert r.is_predicted
|
||||
assert r.lost_frames == 1
|
||||
assert r.raw is None # Coast 时 raw 为空（§2.3）
|
||||
assert math.isfinite(r.center[0])
|
||||
|
||||
# 持续 NaN 检测直至轨道死亡回收，全程不崩溃
|
||||
for _ in range(10):
|
||||
r = tracker.update([make_det(nan, 0.0, 10.0, 10.0, 0.9)])
|
||||
assert not r.tracked
|
||||
assert tracker.active_count() == 0
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 补充行为测试（§2.3 / §6.3 / §9.5）
|
||||
# ============================================================================
|
||||
|
||||
def test_lost_result_fields_are_empty():
|
||||
tracker = TargetTracker()
|
||||
r = tracker.update([]) # 空帧序列起点：无任何轨道
|
||||
assert not r.tracked
|
||||
assert r.primary_id is None
|
||||
assert r.box is None
|
||||
assert r.raw is None
|
||||
assert r.lost_frames == 0
|
||||
assert r.n_active == 0
|
||||
assert r.confidence == 0.0
|
||||
|
||||
# LastResult 返回同一快照（§9.5 跨线程读取入口）
|
||||
cached = tracker.last_result()
|
||||
assert not cached.tracked
|
||||
|
||||
|
||||
def test_coasting_fallback_chooses_nearest_to_frame_center():
|
||||
# Coast 兜底选举：无实测轨道时选离画面中心最近的 Coast 轨道（§6.3）。
|
||||
# 锁定延续优先：需先让锁定轨道死亡解除锁定，才会进入重新选举分支。
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 2
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# F1：角落目标（高置信度）先建立并被锁定
|
||||
tracker.update([make_det(10.0, 10.0, 30.0, 30.0, 0.95)])
|
||||
# F2：画面中心附近目标建立；角落目标丢失(1)
|
||||
tracker.update([make_det(300.0, 220.0, 340.0, 260.0, 0.9)])
|
||||
# F3：远处目标建立；角落丢失(2)、中心目标丢失(1)
|
||||
tracker.update([make_det(600.0, 450.0, 620.0, 470.0, 0.85)])
|
||||
# F4：全部丢失：角落丢失(3) > max_lost 死亡解除锁定；
|
||||
# 其余两条 Coast 存活 → 重新选举选离画面中心最近者
|
||||
r = tracker.update([], FrameShape(480, 640))
|
||||
assert r.tracked
|
||||
assert r.is_predicted
|
||||
assert r.center[0] == pytest.approx(320.0, abs=25.0) # 靠近画面中心 (320,240)
|
||||
assert r.center[1] == pytest.approx(240.0, abs=25.0)
|
||||
assert r.n_active == 2 # 角落轨道已回收
|
||||
|
||||
|
||||
def test_locked_primary_kept_while_measured():
|
||||
# 锁定延续优先于置信度选举（§6.3 ①）
|
||||
tracker = TargetTracker()
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.4)])
|
||||
assert r.primary_id is not None
|
||||
locked_id = r.primary_id
|
||||
|
||||
# 高置信度新目标出现并持续，锁定目标也持续实测 → primary_id 不变
|
||||
for i in range(5):
|
||||
r = tracker.update([make_det(100.0 + i, 100.0, 120.0 + i, 120.0, 0.4),
|
||||
make_det(400.0, 300.0, 430.0, 330.0, 0.95)])
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == locked_id
|
||||
assert not r.is_predicted
|
||||
|
||||
|
||||
def test_default_frame_center_is_320x240():
|
||||
# frame_shape 缺省按 640×480 画面中心 (320,240) 计算（§8.2 / §2.2）
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 2
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# F1：角落目标先建立并被锁定（高置信度）
|
||||
tracker.update([make_det(0.0, 0.0, 20.0, 20.0, 0.95)])
|
||||
# F2：左侧目标（中心 (280,240)，距默认中心 40px）建立
|
||||
tracker.update([make_det(270.0, 230.0, 290.0, 250.0, 0.9)])
|
||||
# F3：右侧目标（中心 (500,240)，距默认中心 180px）建立
|
||||
tracker.update([make_det(490.0, 230.0, 510.0, 250.0, 0.85)])
|
||||
# F4：全部丢失：锁定轨道死亡解除锁定，两条 Coast 轨道兜底选举
|
||||
r = tracker.update([])
|
||||
assert r.tracked
|
||||
assert r.is_predicted
|
||||
assert r.center[0] == pytest.approx(280.0, abs=30.0) # 左侧更近，当选
|
||||
assert r.center[1] == pytest.approx(240.0, abs=30.0)
|
||||
@@ -0,0 +1,159 @@
|
||||
"""tracklet_test.py —— Tracklet 与几何工具单元测试。
|
||||
|
||||
移植自 reference-c++/target_tracker/tests/tracklet_test.cpp。
|
||||
覆盖（视觉追踪技术路线 §3.2/§5.2/§6.1/§7）：框互转、IoU 边界、
|
||||
IsDead/IsCoasting 判定（严格大于）、Predict/Update/MarkMiss 计数语义。
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from target_tracker.tracklet import (BoxXYXY, BoxCXCYWH, Tracklet, compute_iou,
|
||||
to_center_size, to_xyxy)
|
||||
|
||||
|
||||
# ---- 框表示互转（§7） ----
|
||||
|
||||
def test_round_trip_preserves_values():
|
||||
box = BoxXYXY(10.0, 20.0, 50.0, 80.0)
|
||||
c = to_center_size(box)
|
||||
assert c.cx == 30.0 # (10+50)/2
|
||||
assert c.cy == 50.0 # (20+80)/2
|
||||
assert c.w == 40.0 # 50−10
|
||||
assert c.h == 60.0 # 80−20
|
||||
|
||||
back = to_xyxy(c)
|
||||
assert back.x1 == box.x1
|
||||
assert back.y1 == box.y1
|
||||
assert back.x2 == box.x2
|
||||
assert back.y2 == box.y2
|
||||
|
||||
|
||||
# ---- IoU 边界（§5.2） ----
|
||||
|
||||
def test_identical_boxes_return_one():
|
||||
a = BoxXYXY(10.0, 10.0, 30.0, 30.0)
|
||||
assert compute_iou(a, a) == pytest.approx(1.0, abs=1e-12) # 完全重合 = 1
|
||||
|
||||
|
||||
def test_disjoint_boxes_return_zero():
|
||||
a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
|
||||
b = BoxXYXY(20.0, 20.0, 30.0, 30.0)
|
||||
assert compute_iou(a, b) == 0.0 # 零交集
|
||||
|
||||
|
||||
def test_touching_edges_return_zero():
|
||||
# 仅边/角相接：交集宽高为 0 → 交集面积 ≤ 0 返回 0（§5.2）
|
||||
a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
|
||||
b = BoxXYXY(10.0, 0.0, 20.0, 10.0) # 右边缘相接
|
||||
assert compute_iou(a, b) == 0.0
|
||||
c = BoxXYXY(10.0, 10.0, 20.0, 20.0) # 角点相接
|
||||
assert compute_iou(a, c) == 0.0
|
||||
|
||||
|
||||
def test_empty_box_returns_zero():
|
||||
normal = BoxXYXY(0.0, 0.0, 10.0, 10.0)
|
||||
zero_area = BoxXYXY(5.0, 5.0, 5.0, 5.0) # 零面积
|
||||
inverted = BoxXYXY(10.0, 10.0, 0.0, 0.0) # 反向（负宽高）
|
||||
assert compute_iou(normal, zero_area) == 0.0
|
||||
assert compute_iou(zero_area, normal) == 0.0
|
||||
assert compute_iou(normal, inverted) == 0.0
|
||||
assert compute_iou(zero_area, zero_area) == 0.0
|
||||
|
||||
|
||||
def test_partial_overlap_is_correct():
|
||||
# A = [0,0,10,10]（面积 100），B = [5,5,15,15]（面积 100），交集 25
|
||||
a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
|
||||
b = BoxXYXY(5.0, 5.0, 15.0, 15.0)
|
||||
assert compute_iou(a, b) == pytest.approx(25.0 / 175.0, abs=1e-12)
|
||||
assert compute_iou(b, a) == pytest.approx(25.0 / 175.0, abs=1e-12) # 对称性
|
||||
|
||||
|
||||
# ---- 构造与派生状态（§3.2 / §6.1） ----
|
||||
|
||||
def test_construction_initial_fields():
|
||||
t = Tracklet(7, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 1, 8)
|
||||
|
||||
assert t.track_id == 7
|
||||
assert t.lost_count == 0
|
||||
assert t.max_lost == 8
|
||||
assert t.confidence == 0.9
|
||||
assert t.cls_id == 1
|
||||
assert t.age == 1 # 创建时为 1（§3.2）
|
||||
assert t.hits == 1 # 创建时为 1（§3.2）
|
||||
assert t.last_center == (100.0, 100.0)
|
||||
assert t.last_box is not None # 创建当帧即赋 last_box（§7 步骤 5）
|
||||
assert t.last_box.x1 == 90.0
|
||||
assert t.last_box.y1 == 90.0
|
||||
assert t.last_box.x2 == 110.0
|
||||
assert t.last_box.y2 == 110.0
|
||||
assert not t.is_dead
|
||||
assert not t.is_coasting
|
||||
|
||||
|
||||
def test_is_dead_uses_strictly_greater():
|
||||
# IsDead 使用严格大于：lost_count == max_lost 仍存活（§6.1 注记 2）
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
for _ in range(8):
|
||||
t.mark_miss()
|
||||
assert not t.is_dead, f"lost_count={t.lost_count} 未超过 max_lost 不应判死"
|
||||
assert t.lost_count == 8
|
||||
t.mark_miss() # lost_count = 9 > 8
|
||||
assert t.is_dead
|
||||
|
||||
|
||||
def test_is_coasting_when_lost_positive():
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
assert not t.is_coasting
|
||||
t.mark_miss()
|
||||
assert t.is_coasting # lost_count > 0 即 Coast（§3.2）
|
||||
|
||||
|
||||
# ---- 生命周期方法计数语义（§7） ----
|
||||
|
||||
def test_predict_advances_age_and_updates_last_state():
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
age_before = t.age
|
||||
t.predict() # 速度为零：位置不变，但 age+1、last_box 更新为预测框
|
||||
assert t.age == age_before + 1
|
||||
assert t.last_box is not None
|
||||
assert t.last_center[0] == pytest.approx(100.0, abs=1e-9)
|
||||
assert t.last_center[1] == pytest.approx(100.0, abs=1e-9)
|
||||
# hits / lost_count / confidence 不受预测影响
|
||||
assert t.hits == 1
|
||||
assert t.lost_count == 0
|
||||
assert t.confidence == 0.9
|
||||
|
||||
|
||||
def test_update_resets_lost_and_increments_hits():
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
t.mark_miss()
|
||||
t.mark_miss()
|
||||
assert t.lost_count == 2
|
||||
assert t.is_coasting
|
||||
|
||||
age_before = t.age
|
||||
box = BoxXYXY(150.0, 160.0, 170.0, 180.0)
|
||||
t.update(box, 0.75, 2)
|
||||
|
||||
assert t.lost_count == 0 # 匹配成功清零（§7）
|
||||
assert t.hits == 2 # hits+1
|
||||
assert t.age == age_before + 1 # age+1
|
||||
assert t.confidence == 0.75
|
||||
assert t.cls_id == 2
|
||||
assert t.last_center == (160.0, 170.0) # ((150+170)/2, (160+180)/2)
|
||||
assert t.last_box is not None
|
||||
assert t.last_box.x1 == box.x1
|
||||
assert t.last_raw_box is not None # 实测框同步记录
|
||||
assert t.last_raw_box.x2 == box.x2
|
||||
assert not t.is_coasting
|
||||
|
||||
|
||||
def test_mark_miss_increments_lost_and_age():
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
age_before = t.age
|
||||
t.mark_miss()
|
||||
assert t.lost_count == 1
|
||||
assert t.age == age_before + 1
|
||||
# MarkMiss 不影响 hits / confidence
|
||||
assert t.hits == 1
|
||||
assert t.confidence == 0.9
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
@@ -0,0 +1,43 @@
|
||||
"""frame_transform_test.py —— rotate_body_to_ned 单元测试。
|
||||
|
||||
移植自 reference-c++/visual_track_control/tests/frame_transform_test.cpp。
|
||||
覆盖（物理追踪思路 §2.3，A4）：零航向恒等、朝东时机体前向 → 东向、
|
||||
垂直分量不变、模长守恒。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from visual_track_control.frame_transform import rotate_body_to_ned
|
||||
|
||||
|
||||
def test_zero_yaw_is_identity():
|
||||
north, east, down = rotate_body_to_ned(1.0, 0.5, -2.0, 0.0)
|
||||
assert north == pytest.approx(1.0, abs=1e-12) # 北 = 前
|
||||
assert east == pytest.approx(0.5, abs=1e-12) # 东 = 右
|
||||
assert down == pytest.approx(-2.0, abs=1e-12)
|
||||
|
||||
|
||||
def test_yaw_east_90_maps_forward_to_east():
|
||||
north, east, _ = rotate_body_to_ned(2.0, 0.0, 0.0, math.pi / 2.0) # 机头朝东
|
||||
assert north == pytest.approx(0.0, abs=1e-9) # 北向 ≈ 0
|
||||
assert east == pytest.approx(2.0, abs=1e-9) # 东向 = 前向速度
|
||||
|
||||
|
||||
def test_right_is_east_when_heading_north():
|
||||
north, east, _ = rotate_body_to_ned(0.0, 1.0, 0.0, 0.0)
|
||||
assert north == pytest.approx(0.0, abs=1e-12)
|
||||
assert east == pytest.approx(1.0, abs=1e-12)
|
||||
|
||||
|
||||
def test_down_component_unchanged():
|
||||
_, _, down = rotate_body_to_ned(1.0, 1.0, 3.5, math.pi / 3.0)
|
||||
assert down == 3.5
|
||||
|
||||
|
||||
def test_preserves_horizontal_norm():
|
||||
# 旋转为正交变换：水平合速度不变（限幅约束不被破坏的前提）
|
||||
north, east, _ = rotate_body_to_ned(3.0, -4.0, 1.0, 0.7)
|
||||
assert math.hypot(north, east) == pytest.approx(math.hypot(3.0, -4.0),
|
||||
abs=1e-9)
|
||||
@@ -0,0 +1,63 @@
|
||||
"""line_of_sight_test.py —— PixelToAngle 单元测试。
|
||||
|
||||
移植自 reference-c++/visual_track_control/tests/line_of_sight_test.cpp。
|
||||
覆盖（物理追踪思路 §4.1/§5.1，C2）：零偏移、画面边缘偏移 = 半视场角、
|
||||
奇对称、单调性、与线性近似的偏离（全程针孔模型）、构造期非法参数抛异常。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from visual_track_control.line_of_sight import PixelToAngle
|
||||
|
||||
|
||||
def test_zero_offset_gives_zero_angle():
|
||||
conv = PixelToAngle(1000.0, math.radians(90.0))
|
||||
assert conv.convert(0.0) == 0.0
|
||||
|
||||
|
||||
def test_half_size_offset_gives_half_fov():
|
||||
# 针孔模型：偏移 = size/2 时 angle = atan(tan(θ/2)) = θ/2
|
||||
conv = PixelToAngle(1000.0, math.radians(90.0))
|
||||
assert conv.convert(500.0) == pytest.approx(math.radians(45.0), abs=1e-12)
|
||||
assert conv.convert(-500.0) == pytest.approx(-math.radians(45.0), abs=1e-12)
|
||||
|
||||
|
||||
def test_odd_symmetry():
|
||||
conv = PixelToAngle(1920.0, math.radians(60.0))
|
||||
for offset in (1.0, 100.0, 730.5):
|
||||
assert conv.convert(-offset) == pytest.approx(-conv.convert(offset),
|
||||
abs=1e-15)
|
||||
|
||||
|
||||
def test_monotonic_increasing():
|
||||
conv = PixelToAngle(1080.0, math.radians(35.0))
|
||||
prev = conv.convert(-600.0)
|
||||
for x in range(-500, 601, 100):
|
||||
cur = conv.convert(float(x))
|
||||
assert cur > prev
|
||||
prev = cur
|
||||
|
||||
|
||||
def test_deviates_from_linear_approximation():
|
||||
# C2：全程针孔模型。θ=90°、偏移比例 0.25 处线性近似误差约 4°，
|
||||
# 针孔结果必须与之明显偏离（证明未走线性支路）。
|
||||
conv = PixelToAngle(1000.0, math.radians(90.0))
|
||||
pinhole_deg = math.degrees(conv.convert(250.0)) # atan(250·2·tan45°/1000)
|
||||
linear_deg = 250.0 * (90.0 / 1000.0) # θ·offset/size = 22.5°
|
||||
assert pinhole_deg == pytest.approx(math.degrees(math.atan(0.5)), abs=1e-9)
|
||||
assert abs(pinhole_deg - linear_deg) > 2.0
|
||||
|
||||
|
||||
def test_constructor_rejects_invalid_args():
|
||||
with pytest.raises(ValueError):
|
||||
PixelToAngle(0.0, 1.0)
|
||||
with pytest.raises(ValueError):
|
||||
PixelToAngle(-10.0, 1.0)
|
||||
with pytest.raises(ValueError):
|
||||
PixelToAngle(1000.0, 0.0)
|
||||
with pytest.raises(ValueError):
|
||||
PixelToAngle(1000.0, math.pi)
|
||||
with pytest.raises(ValueError):
|
||||
PixelToAngle(1000.0, math.pi + 0.1)
|
||||
@@ -0,0 +1,176 @@
|
||||
"""mavsdk_gateway_test.py —— MavsdkCommandGateway 单元测试（mock offboard）。
|
||||
|
||||
移植自 reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp。
|
||||
C++ 版校验 MAVLink ID 84 线格式字节流；Python 版采用 MAVSDK 语义映射，
|
||||
改为以 FakeOffboard 捕获设定点对象，校验三分支映射（物理追踪思路 §2.2、
|
||||
A2 掩码三分支的语义等价）：
|
||||
|
||||
角速率非零 → 机体速度设定点 + 非零 yawspeed；
|
||||
航向角非零（角速率为零）→ NED 速度设定点 + 绝对 yaw；
|
||||
两者均零 → 机体速度设定点 + 零 yawspeed（保持航向，A2 回归语义）；
|
||||
刹车 → 全零机体速度设定点。
|
||||
传输失败（异常）→ 返回 False 且不向外抛。
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
|
||||
import pytest
|
||||
|
||||
from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
|
||||
from visual_track_control.types import ControlOutput
|
||||
|
||||
|
||||
class FakeOffboard:
|
||||
"""捕获式 offboard mock：记录每次调用的设定点，可注入失败。"""
|
||||
|
||||
def __init__(self):
|
||||
self.body_setpoints = []
|
||||
self.ned_setpoints = []
|
||||
self.attempts = 0
|
||||
self.start_count = 0
|
||||
self.stop_count = 0
|
||||
self.succeed = True
|
||||
|
||||
async def set_velocity_body(self, setpoint):
|
||||
self.attempts += 1 # 与 C++ CaptureTransport 一致：失败也计入发送次数
|
||||
if not self.succeed:
|
||||
raise RuntimeError("mock transport failure")
|
||||
self.body_setpoints.append(setpoint)
|
||||
|
||||
async def set_velocity_ned(self, setpoint):
|
||||
self.attempts += 1
|
||||
if not self.succeed:
|
||||
raise RuntimeError("mock transport failure")
|
||||
self.ned_setpoints.append(setpoint)
|
||||
|
||||
async def start(self):
|
||||
if not self.succeed:
|
||||
raise RuntimeError("mock start failure")
|
||||
self.start_count += 1
|
||||
|
||||
async def stop(self):
|
||||
self.stop_count += 1
|
||||
|
||||
@property
|
||||
def send_count(self):
|
||||
return self.attempts
|
||||
|
||||
|
||||
class FakeDrone:
|
||||
def __init__(self):
|
||||
self.offboard = FakeOffboard()
|
||||
|
||||
|
||||
def run(coro):
|
||||
return asyncio.run(coro)
|
||||
|
||||
|
||||
def make_gateway():
|
||||
drone = FakeDrone()
|
||||
return MavsdkCommandGateway(drone), drone.offboard
|
||||
|
||||
|
||||
def test_start_sends_initial_setpoint_then_starts():
|
||||
gateway, offboard = make_gateway()
|
||||
assert run(gateway.start())
|
||||
assert offboard.start_count == 1
|
||||
# PX4 要求先流式发送首条设定点（全零）再请求启动
|
||||
assert len(offboard.body_setpoints) == 1
|
||||
sp = offboard.body_setpoints[0]
|
||||
assert sp.forward_m_s == 0.0
|
||||
assert sp.right_m_s == 0.0
|
||||
assert sp.down_m_s == 0.0
|
||||
assert sp.yawspeed_deg_s == 0.0
|
||||
|
||||
|
||||
def test_start_failure_returns_false():
|
||||
gateway, offboard = make_gateway()
|
||||
offboard.succeed = False
|
||||
assert not run(gateway.start())
|
||||
|
||||
|
||||
def _make_velocity_heading_output():
|
||||
from visual_track_control.types import ControlMode
|
||||
out = ControlOutput()
|
||||
out.mode = ControlMode.VELOCITY_HEADING
|
||||
return out
|
||||
|
||||
|
||||
def test_yaw_rate_branch_uses_body_frame_with_yawspeed():
|
||||
from visual_track_control.types import ControlMode
|
||||
gateway, offboard = make_gateway()
|
||||
out = _make_velocity_heading_output()
|
||||
out.vx_body_mps = 1.5
|
||||
out.vz_mps = 0.25
|
||||
out.yaw_rate_dps = 5.0
|
||||
assert run(gateway.send_velocity_heading(out, 12345))
|
||||
assert offboard.send_count == 1
|
||||
assert len(offboard.body_setpoints) == 1
|
||||
sp = offboard.body_setpoints[0]
|
||||
assert sp.forward_m_s == pytest.approx(1.5)
|
||||
assert sp.right_m_s == 0.0
|
||||
assert sp.down_m_s == pytest.approx(0.25)
|
||||
assert sp.yawspeed_deg_s == pytest.approx(5.0)
|
||||
assert offboard.ned_setpoints == []
|
||||
|
||||
|
||||
def test_yaw_only_branch_uses_ned_frame_with_absolute_yaw():
|
||||
# 航向角非零且角速率为零 → 航向掩码语义：NED 速度 + 绝对航向
|
||||
gateway, offboard = make_gateway()
|
||||
out = _make_velocity_heading_output()
|
||||
out.vx_mps = 2.0
|
||||
out.vy_mps = -1.0
|
||||
out.vz_mps = 0.5
|
||||
out.yaw_deg = 10.0
|
||||
out.yaw_rate_dps = 0.0
|
||||
assert run(gateway.send_velocity_heading(out, 1))
|
||||
assert offboard.send_count == 1
|
||||
assert len(offboard.ned_setpoints) == 1
|
||||
sp = offboard.ned_setpoints[0]
|
||||
assert sp.north_m_s == pytest.approx(2.0)
|
||||
assert sp.east_m_s == pytest.approx(-1.0)
|
||||
assert sp.down_m_s == pytest.approx(0.5)
|
||||
assert sp.yaw_deg == pytest.approx(10.0)
|
||||
assert offboard.body_setpoints == []
|
||||
|
||||
|
||||
def test_both_zero_keeps_heading_via_body_zero_yawspeed():
|
||||
# A2 回归：两者均零时机体速度设定点配零角速率 = 保持当前航向，
|
||||
# 不得误发绝对航向 0（正北）。
|
||||
gateway, offboard = make_gateway()
|
||||
out = _make_velocity_heading_output()
|
||||
out.vx_body_mps = 1.0
|
||||
out.yaw_rate_dps = 0.0
|
||||
out.yaw_deg = 0.0
|
||||
assert run(gateway.send_velocity_heading(out, 1))
|
||||
assert len(offboard.body_setpoints) == 1
|
||||
sp = offboard.body_setpoints[0]
|
||||
assert sp.forward_m_s == pytest.approx(1.0)
|
||||
assert sp.yawspeed_deg_s == 0.0
|
||||
assert offboard.ned_setpoints == [] # 不得走绝对航向支路
|
||||
|
||||
|
||||
def test_brake_hover_sends_zero_velocity_body():
|
||||
gateway, offboard = make_gateway()
|
||||
assert run(gateway.send_brake_hover(777))
|
||||
assert offboard.send_count == 1
|
||||
sp = offboard.body_setpoints[0]
|
||||
assert sp.forward_m_s == 0.0
|
||||
assert sp.right_m_s == 0.0
|
||||
assert sp.down_m_s == 0.0
|
||||
assert sp.yawspeed_deg_s == 0.0
|
||||
|
||||
|
||||
def test_transport_failure_propagates_as_false():
|
||||
gateway, offboard = make_gateway()
|
||||
offboard.succeed = False
|
||||
out = _make_velocity_heading_output()
|
||||
assert not run(gateway.send_velocity_heading(out, 1))
|
||||
assert not run(gateway.send_brake_hover(1))
|
||||
assert offboard.send_count == 2
|
||||
|
||||
|
||||
def test_stop_calls_offboard_stop():
|
||||
gateway, offboard = make_gateway()
|
||||
run(gateway.stop())
|
||||
assert offboard.stop_count == 1
|
||||
@@ -0,0 +1,103 @@
|
||||
"""pid_controller_test.py —— PidController 单元测试。
|
||||
|
||||
移植自 reference-c++/visual_track_control/tests/pid_controller_test.cpp。
|
||||
覆盖（物理追踪思路 §6.2/§6.4）：位置式 P/I/D 三项、积分钳位、输出限幅、
|
||||
异常节拍保护、anti-windup 条件积分（C1 修复回归）、ResetIntegral / Reset。
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from visual_track_control.pid_controller import PidController, PidParams
|
||||
|
||||
|
||||
def make_params(kp, ki, kd, integral_limit, output_limit, filter_coef=0.0):
|
||||
return PidParams(kp=kp, ki=ki, kd=kd, integral_limit=integral_limit,
|
||||
derivative_filter_coef=filter_coef, output_limit=output_limit)
|
||||
|
||||
|
||||
def test_proportional_only():
|
||||
pid = PidController(make_params(2.0, 0.0, 0.0, 0.0, 0.0))
|
||||
assert pid.update(3.0, 0.1) == 6.0
|
||||
assert pid.update(-1.5, 0.1) == -3.0
|
||||
|
||||
|
||||
def test_integral_accumulates():
|
||||
pid = PidController(make_params(0.0, 1.0, 0.0, 0.0, 0.0))
|
||||
# 两拍 error=2、dt=0.5：积分累计 1.0 → 2.0
|
||||
assert pid.update(2.0, 0.5) == 1.0
|
||||
assert pid.update(2.0, 0.5) == 2.0
|
||||
|
||||
|
||||
def test_integral_clamped_to_limit():
|
||||
pid = PidController(make_params(0.0, 1.0, 0.0, integral_limit=1.0, output_limit=0.0))
|
||||
for _ in range(5):
|
||||
pid.update(10.0, 1.0)
|
||||
# 积分候选值被钳位在 1.0 → 输出恒为 1.0
|
||||
assert pid.last_output == 1.0
|
||||
|
||||
|
||||
def test_output_clamped_to_limit():
|
||||
pid = PidController(make_params(10.0, 0.0, 0.0, 0.0, output_limit=3.0))
|
||||
assert pid.update(1.0, 0.1) == 3.0
|
||||
assert pid.update(-1.0, 0.1) == -3.0
|
||||
|
||||
|
||||
def test_anti_windup_freeze_while_saturated_same_direction():
|
||||
# kp=0、ki=2、output_limit=5：error=+1 持续饱和时积分冻结，
|
||||
# 输出恒钉在 +5（10 拍累计也不会让积分继续增长）。
|
||||
pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0))
|
||||
for _ in range(10):
|
||||
assert pid.update(10.0, 1.0) == 5.0
|
||||
|
||||
|
||||
def test_anti_windup_unwinds_after_error_flips():
|
||||
# C1 回归：误差反号后积分必须立即回卷（旧判据"积分与输出同向"会
|
||||
# 冻结回卷、输出一直钉在 +5）。
|
||||
# kp=0、ki=2、limit=5：+10 饱和 3 拍、积分冻结在 0；
|
||||
# error=-1 首拍 → 输出 2×(-1) = -2（立即回卷）；
|
||||
# 持续负误差使输出到达负饱和 -5（积分冻结在 -2，输出 = clamp(-6)）。
|
||||
pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0))
|
||||
for _ in range(3):
|
||||
assert pid.update(10.0, 1.0) == 5.0
|
||||
assert pid.update(-1.0, 1.0) == -2.0 # 首拍立即回卷
|
||||
out = -2.0
|
||||
for _ in range(11):
|
||||
out = pid.update(-1.0, 1.0)
|
||||
assert out == pytest.approx(-5.0, abs=1e-9) # 旧判据此处仍为 +5.0
|
||||
|
||||
|
||||
def test_derivative_first_step_is_zero():
|
||||
pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0))
|
||||
# 首拍无微分历史：微分项为 0
|
||||
assert pid.update(5.0, 1.0) == 0.0
|
||||
|
||||
|
||||
def test_derivative_with_low_pass_filter():
|
||||
pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0, filter_coef=0.5))
|
||||
pid.update(0.0, 1.0) # 建立微分历史（导数 0）
|
||||
# 第二拍原始导数 1.0，低通后 0.5×0 + 0.5×1.0 = 0.5
|
||||
assert pid.update(1.0, 1.0) == 0.5
|
||||
|
||||
|
||||
def test_zero_or_negative_dt_holds_last_output():
|
||||
pid = PidController(make_params(1.0, 0.0, 0.0, 0.0, 0.0))
|
||||
assert pid.update(2.0, 0.1) == 2.0
|
||||
assert pid.update(99.0, 0.0) == 2.0
|
||||
assert pid.update(99.0, -1.0) == 2.0
|
||||
|
||||
|
||||
def test_reset_integral_clears_only_integral():
|
||||
pid = PidController(make_params(0.0, 1.0, 1.0, 0.0, 0.0))
|
||||
pid.update(2.0, 1.0) # 积分 = 2
|
||||
pid.reset_integral()
|
||||
# 积分清零、微分历史保留：输出应为 kd×(0−2)/1 = −2
|
||||
assert pid.update(0.0, 1.0) == -2.0
|
||||
|
||||
|
||||
def test_reset_clears_everything():
|
||||
pid = PidController(make_params(1.0, 1.0, 1.0, 0.0, 0.0))
|
||||
pid.update(5.0, 1.0)
|
||||
pid.reset()
|
||||
assert pid.last_output == 0.0
|
||||
# 全清后等同全新控制器：首拍无微分历史
|
||||
assert pid.update(3.0, 1.0) == 3.0 + 3.0 # kp·e + ki·∫e
|
||||
@@ -0,0 +1,68 @@
|
||||
"""rate_limiter_test.py —— Clamp / SlewRateLimiter 单元测试。
|
||||
|
||||
移植自 reference-c++/visual_track_control/tests/rate_limiter_test.cpp。
|
||||
覆盖（物理追踪思路 §8、状态机设计 S6）：静态限幅、变化率限幅步进、
|
||||
首条指令从零渐变、异常节拍保护、Reset 归零。
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from visual_track_control.rate_limiter import SlewRateLimiter, clamp
|
||||
|
||||
|
||||
def test_clamp_clamps_value():
|
||||
assert clamp(5.0, -2.0, 2.0) == 2.0
|
||||
assert clamp(-5.0, -2.0, 2.0) == -2.0
|
||||
assert clamp(1.5, -2.0, 2.0) == 1.5
|
||||
# 边界值：落在区间内
|
||||
assert clamp(2.0, -2.0, 2.0) == 2.0
|
||||
assert clamp(-2.0, -2.0, 2.0) == -2.0
|
||||
|
||||
|
||||
def test_first_command_ramps_from_zero():
|
||||
# S6：初始输出为 0，首条大指令只能按变化率渐变
|
||||
lim = SlewRateLimiter(2.0)
|
||||
assert lim.limit(10.0, 0.5) == 1.0 # 0 + 2×0.5
|
||||
assert lim.limit(10.0, 0.5) == 2.0 # 继续渐变
|
||||
assert lim.limit(10.0, 10.0) == 10.0 # 步长足够 → 到达目标
|
||||
|
||||
|
||||
def test_tracks_target_within_step():
|
||||
lim = SlewRateLimiter(10.0)
|
||||
# 步长 10×1=10，目标 3 在步长内 → 直接到达
|
||||
assert lim.limit(3.0, 1.0) == 3.0
|
||||
assert lim.limit(3.0, 1.0) == 3.0 # 已在目标：保持
|
||||
|
||||
|
||||
def test_negative_direction():
|
||||
lim = SlewRateLimiter(2.0)
|
||||
assert lim.limit(-10.0, 0.5) == -1.0
|
||||
assert lim.limit(-10.0, 0.5) == -2.0
|
||||
|
||||
|
||||
def test_zero_or_negative_dt_holds():
|
||||
lim = SlewRateLimiter(2.0)
|
||||
lim.limit(10.0, 1.0) # 输出 2.0
|
||||
assert lim.limit(10.0, 0.0) == 2.0
|
||||
assert lim.limit(10.0, -1.0) == 2.0
|
||||
|
||||
|
||||
def test_zero_rate_limit_freezes_output():
|
||||
lim = SlewRateLimiter(0.0)
|
||||
assert lim.limit(5.0, 1.0) == 0.0
|
||||
|
||||
|
||||
def test_reset_to_value():
|
||||
lim = SlewRateLimiter(2.0)
|
||||
lim.limit(10.0, 1.0)
|
||||
lim.reset()
|
||||
assert lim.last_output == 0.0
|
||||
lim.reset(3.5)
|
||||
assert lim.last_output == 3.5
|
||||
# 复位后从新值继续渐变
|
||||
assert lim.limit(10.0, 1.0) == 5.5 # 3.5 + 2×1
|
||||
|
||||
|
||||
def test_custom_initial_value():
|
||||
lim = SlewRateLimiter(1.0, initial=5.0)
|
||||
assert lim.limit(10.0, 1.0) == 6.0
|
||||
@@ -0,0 +1,32 @@
|
||||
"""staleness_checker_test.py —— is_stale 单元测试。
|
||||
|
||||
移植自 reference-c++/visual_track_control/tests/staleness_checker_test.cpp。
|
||||
覆盖（物理追踪思路 §7.1）：从未取样判超龄、有效期内新鲜、超出有效期超龄、
|
||||
边界恰好在有效期上（不算超龄）、时间戳不一致判超龄（C4）。
|
||||
"""
|
||||
|
||||
from visual_track_control.staleness_checker import is_stale
|
||||
|
||||
|
||||
def test_never_sampled_is_stale():
|
||||
assert is_stale(0, 1000, 200)
|
||||
assert is_stale(-5, 1000, 200)
|
||||
|
||||
|
||||
def test_within_validity_is_fresh():
|
||||
assert not is_stale(900, 1000, 200)
|
||||
assert not is_stale(1000, 1000, 200) # 刚取样
|
||||
|
||||
|
||||
def test_beyond_validity_is_stale():
|
||||
assert is_stale(799, 1000, 200) # 201 > 200
|
||||
|
||||
|
||||
def test_exactly_at_boundary_is_fresh():
|
||||
# 超龄判定为严格大于：恰好等于有效期仍算新鲜
|
||||
assert not is_stale(800, 1000, 200)
|
||||
|
||||
|
||||
def test_inconsistent_timestamp_is_stale():
|
||||
# C4：now < sample 时间戳不一致 → 年龄不可信 → 判超龄（宁降级不轻信）
|
||||
assert is_stale(2000, 1000, 200)
|
||||
@@ -0,0 +1,338 @@
|
||||
"""tracking_control_law_test.py —— TrackingControlLaw 主控律单元测试。
|
||||
|
||||
移植自 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp。
|
||||
覆盖（物理追踪思路 §2.3/§4/§5/§6/§7/§9）：超龄降级（姿态缺失/过期、视觉
|
||||
超龄、降级优先级）、判丢零速刹车、Coast 清积分与标志、速率式/位置式航向
|
||||
通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道三种
|
||||
无距离降级动作、首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from visual_track_control.config import (HeadingMode, NoDistanceAction,
|
||||
TrackingConfig)
|
||||
from visual_track_control.tracking_control_law import TrackingControlLaw
|
||||
from visual_track_control.types import (ControlMode, ControlSnapshot,
|
||||
DegradedReason, TrackState)
|
||||
|
||||
|
||||
def make_config() -> TrackingConfig:
|
||||
"""测试基准配置：大限幅使限幅器近似透明，便于独立验证各通道数学。
|
||||
画面 1000×1000、视场角 90° → 针孔系数 2·tan(45°)/1000 = 0.002，
|
||||
偏移 100 像素 → 视线角 atan(0.2) ≈ 11.3099°。
|
||||
"""
|
||||
cfg = TrackingConfig()
|
||||
cfg.control.frequency_hz = 20.0
|
||||
cfg.control.visual_stale_ms = 200
|
||||
cfg.control.radar_stale_ms = 500
|
||||
cfg.control.attitude_stale_ms = 300
|
||||
cfg.camera.image_width = 1000
|
||||
cfg.camera.image_height = 1000
|
||||
cfg.camera.fov_h_deg = 90.0
|
||||
cfg.camera.fov_v_deg = 90.0
|
||||
cfg.heading.mode = HeadingMode.RATE
|
||||
cfg.heading.gain = 1.0
|
||||
cfg.heading.yaw_rate_limit_dps = 30.0
|
||||
cfg.heading.yaw_slew_limit_dps2 = 100000.0
|
||||
cfg.vertical.vz_gain_mps_per_deg = 0.1
|
||||
cfg.vertical.vz_limit_mps = 2.0
|
||||
cfg.distance.d_exp_m = 10.0
|
||||
cfg.distance.kp = 0.5
|
||||
cfg.distance.ki = 0.0
|
||||
cfg.distance.kd = 0.0
|
||||
cfg.distance.integral_limit = 0.0
|
||||
cfg.distance.derivative_filter_coef = 0.0
|
||||
cfg.distance.approach_velocity_limit_mps = 3.0
|
||||
cfg.distance.retreat_velocity_limit_mps = 1.0
|
||||
cfg.distance.no_distance_action = NoDistanceAction.HOLD
|
||||
cfg.distance.no_distance_approach_limit_mps = 0.5
|
||||
cfg.accel_limit.ax_mps2 = 1000.0
|
||||
cfg.accel_limit.ay_mps2 = 1000.0
|
||||
cfg.accel_limit.az_mps2 = 1000.0
|
||||
return cfg
|
||||
|
||||
|
||||
def make_snapshot(now_ms: int) -> ControlSnapshot:
|
||||
"""新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜。"""
|
||||
return ControlSnapshot(
|
||||
track=TrackState(tracked=True, is_predicted=False,
|
||||
center=(500.0, 500.0)),
|
||||
track_time_ms=now_ms,
|
||||
attitude_present=True,
|
||||
attitude_time_ms=now_ms,
|
||||
)
|
||||
|
||||
|
||||
# ---- 超龄降级（§7.1 / §9.2） ----
|
||||
|
||||
def test_attitude_missing_degrades():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.attitude_present = False
|
||||
out = law.update(snap, 1000)
|
||||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||||
assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
|
||||
assert not out.valid
|
||||
|
||||
|
||||
def test_attitude_stale_degrades():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.attitude_time_ms = 600 # 400 ms 前 > 300 ms 阈值
|
||||
out = law.update(snap, 1000)
|
||||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||||
assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
|
||||
|
||||
|
||||
def test_vision_stale_degrades():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.track_time_ms = 700 # 300 ms 前 > 200 ms 阈值
|
||||
out = law.update(snap, 1000)
|
||||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||||
assert out.degraded_reason is DegradedReason.VISION_STALE
|
||||
|
||||
|
||||
def test_never_sampled_track_degrades():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.track_time_ms = 0 # 从未取样
|
||||
out = law.update(snap, 1000)
|
||||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||||
assert out.degraded_reason is DegradedReason.VISION_STALE
|
||||
|
||||
|
||||
def test_attitude_stale_beats_vision_stale():
|
||||
# 降级优先级：姿态校验在前（§9.2 姿态过期禁止一切空间转换）
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.attitude_present = False
|
||||
snap.track_time_ms = 0
|
||||
out = law.update(snap, 1000)
|
||||
assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
|
||||
|
||||
|
||||
# ---- 判丢刹车（§6.4） ----
|
||||
|
||||
def test_lost_target_brakes():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.track.tracked = False
|
||||
out = law.update(snap, 1000)
|
||||
assert out.mode is ControlMode.BRAKE_HOVER
|
||||
assert out.valid
|
||||
assert out.vx_mps == 0.0
|
||||
assert out.vy_mps == 0.0
|
||||
assert out.vz_mps == 0.0
|
||||
|
||||
|
||||
# ---- Coast（§6.4） ----
|
||||
|
||||
def test_coast_sets_flag_and_keeps_tracking():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.track.is_predicted = True
|
||||
out = law.update(snap, 1000)
|
||||
assert out.mode is ControlMode.VELOCITY_HEADING
|
||||
assert out.coast_active
|
||||
assert out.valid
|
||||
|
||||
|
||||
# ---- 水平航向通道（§4.2，A2） ----
|
||||
|
||||
def test_rate_mode_maps_beta_to_yaw_rate():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.track.center = (600.0, 500.0) # 目标偏右 100 像素
|
||||
out = law.update(snap, 1000)
|
||||
beta_deg = math.degrees(math.atan(0.2)) # ≈ 11.31°
|
||||
assert out.yaw_rate_dps == pytest.approx(beta_deg, abs=1e-6) # gain=1，右转正
|
||||
assert out.yaw_deg == 0.0
|
||||
|
||||
|
||||
def test_rate_mode_clamps_to_yaw_rate_limit():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.track.center = (950.0, 500.0) # 大偏移：β≈43.5° > 30°/s 上限
|
||||
out = law.update(snap, 1000)
|
||||
assert out.yaw_rate_dps == pytest.approx(30.0, abs=1e-9)
|
||||
|
||||
|
||||
def test_position_mode_composes_absolute_heading():
|
||||
# A2：位置式输出为绝对 NED 航向 = 当前航向 + 限幅修正量
|
||||
cfg = make_config()
|
||||
cfg.heading.mode = HeadingMode.POSITION
|
||||
law = TrackingControlLaw(cfg)
|
||||
snap = make_snapshot(1000)
|
||||
snap.track.center = (600.0, 500.0)
|
||||
snap.yaw_rad = math.radians(45.0)
|
||||
out = law.update(snap, 1000)
|
||||
beta_deg = math.degrees(math.atan(0.2))
|
||||
assert out.yaw_deg == pytest.approx(45.0 + beta_deg, abs=1e-6)
|
||||
assert out.yaw_rate_dps == 0.0
|
||||
|
||||
|
||||
def test_position_mode_wraps_around_180():
|
||||
cfg = make_config()
|
||||
cfg.heading.mode = HeadingMode.POSITION
|
||||
law = TrackingControlLaw(cfg)
|
||||
snap = make_snapshot(1000)
|
||||
snap.track.center = (600.0, 500.0)
|
||||
snap.yaw_rad = math.radians(175.0)
|
||||
out = law.update(snap, 1000)
|
||||
beta_deg = math.degrees(math.atan(0.2))
|
||||
# 175 + 11.31 = 186.31 → 回绕到 ≈ -173.69
|
||||
assert out.yaw_deg == pytest.approx(175.0 + beta_deg - 360.0, abs=1e-6)
|
||||
assert -180.0 < out.yaw_deg < 180.0
|
||||
|
||||
|
||||
# ---- 垂直统一速度支路（§5，A1） ----
|
||||
|
||||
def test_vertical_offset_drives_vz():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.track.center = (500.0, 600.0) # 目标偏低 100 像素 → α>0 → 下降
|
||||
out = law.update(snap, 1000)
|
||||
alpha_deg = math.degrees(math.atan(0.2))
|
||||
assert out.vz_mps == pytest.approx(0.1 * alpha_deg, abs=1e-6)
|
||||
assert out.mode is ControlMode.VELOCITY_HEADING # A1：不切姿态微调模式
|
||||
|
||||
|
||||
def test_vertical_velocity_clamped():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.track.center = (500.0, 990.0) # α≈44.5° × 0.1 = 4.45 > 2 上限
|
||||
out = law.update(snap, 1000)
|
||||
assert out.vz_mps == pytest.approx(2.0, abs=1e-9)
|
||||
|
||||
|
||||
# ---- 距离通道（§6.2 / §6.3） ----
|
||||
|
||||
def test_distance_approach_when_far():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.radar_distance_m = 30.0 # Δd = +20 → kp 输出 10 → 限幅 3
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 1000
|
||||
out = law.update(snap, 1000)
|
||||
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
|
||||
|
||||
|
||||
def test_distance_retreat_clamped_when_too_close():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.radar_distance_m = 5.0 # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 1000
|
||||
out = law.update(snap, 1000)
|
||||
assert out.vx_mps == pytest.approx(-1.0, abs=1e-9)
|
||||
|
||||
|
||||
def test_no_distance_hold_zeros_vx():
|
||||
law = TrackingControlLaw(make_config()) # 缺省 kHold
|
||||
snap = make_snapshot(1000)
|
||||
snap.associated_to_target = False
|
||||
out = law.update(snap, 1000)
|
||||
assert out.mode is ControlMode.VELOCITY_HEADING
|
||||
assert out.vx_mps == 0.0
|
||||
|
||||
|
||||
def test_no_distance_slow_approach():
|
||||
cfg = make_config()
|
||||
cfg.distance.no_distance_action = NoDistanceAction.SLOW_APPROACH
|
||||
law = TrackingControlLaw(cfg)
|
||||
snap = make_snapshot(1000)
|
||||
snap.associated_to_target = False
|
||||
out = law.update(snap, 1000)
|
||||
assert out.vx_mps == pytest.approx(0.5, abs=1e-9)
|
||||
|
||||
|
||||
def test_no_distance_exit_degrades():
|
||||
cfg = make_config()
|
||||
cfg.distance.no_distance_action = NoDistanceAction.EXIT
|
||||
law = TrackingControlLaw(cfg)
|
||||
snap = make_snapshot(1000)
|
||||
snap.associated_to_target = False
|
||||
out = law.update(snap, 1000)
|
||||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||||
assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
|
||||
assert not out.valid
|
||||
|
||||
|
||||
def test_stale_radar_falls_back_to_no_distance():
|
||||
law = TrackingControlLaw(make_config()) # kHold
|
||||
snap = make_snapshot(1000)
|
||||
snap.radar_distance_m = 30.0
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 400 # 600 ms 前 > 500 ms 阈值 → 距离不可用
|
||||
out = law.update(snap, 1000)
|
||||
assert out.mode is ControlMode.VELOCITY_HEADING
|
||||
assert out.vx_mps == 0.0
|
||||
|
||||
|
||||
# ---- 首拍/限幅（§8、S6） ----
|
||||
|
||||
def test_first_tick_vx_slews_from_zero():
|
||||
# S6：首条速度指令必须经变化率限幅，从 0 渐变而非阶跃
|
||||
cfg = make_config()
|
||||
cfg.accel_limit.ax_mps2 = 2.0
|
||||
law = TrackingControlLaw(cfg)
|
||||
snap = make_snapshot(1000)
|
||||
snap.radar_distance_m = 30.0 # 通道输出本应为 3.0
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 1000
|
||||
out = law.update(snap, 1000) # 首拍：dt 按 1/20 Hz 兜底
|
||||
assert out.vx_mps == pytest.approx(2.0 * (1.0 / 20.0), abs=1e-9) # 0.1
|
||||
|
||||
|
||||
# ---- 机体系 → NED 旋转（§2.3，A4） ----
|
||||
|
||||
def test_output_rotated_to_ned_by_yaw():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.yaw_rad = math.pi / 2.0 # 机头朝东
|
||||
snap.radar_distance_m = 30.0
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 1000
|
||||
out = law.update(snap, 1000)
|
||||
# 机体前向 3.0 → 东向 3.0，北向 ≈ 0
|
||||
assert out.vx_mps == pytest.approx(0.0, abs=1e-9)
|
||||
assert out.vy_mps == pytest.approx(3.0, abs=1e-9)
|
||||
|
||||
|
||||
def test_zero_yaw_keeps_forward_as_north():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.yaw_rad = 0.0
|
||||
snap.radar_distance_m = 30.0
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 1000
|
||||
out = law.update(snap, 1000)
|
||||
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
|
||||
assert out.vy_mps == pytest.approx(0.0, abs=1e-9)
|
||||
|
||||
|
||||
# ---- Reset（§6.4） ----
|
||||
|
||||
def test_reset_restores_slew_from_zero():
|
||||
cfg = make_config()
|
||||
cfg.accel_limit.ax_mps2 = 2.0
|
||||
law = TrackingControlLaw(cfg)
|
||||
snap = make_snapshot(1000)
|
||||
snap.radar_distance_m = 30.0
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 1000
|
||||
for t in range(1000, 3000, 50):
|
||||
snap.track_time_ms = t
|
||||
snap.radar_time_ms = t
|
||||
snap.attitude_time_ms = t
|
||||
law.update(snap, t) # 若干拍后 vx 已爬升到限幅 3.0
|
||||
law.reset()
|
||||
snap.track_time_ms = 3000
|
||||
snap.radar_time_ms = 3000
|
||||
snap.attitude_time_ms = 3000
|
||||
out = law.update(snap, 3000)
|
||||
# Reset 后限幅器归零：又从 0.1 开始渐变
|
||||
assert out.vx_mps == pytest.approx(0.1, abs=1e-9)
|
||||
@@ -0,0 +1,171 @@
|
||||
"""vtc_config_test.py —— TrackingConfig::load_from_json 单元测试。
|
||||
|
||||
移植自 reference-c++/visual_track_control/tests/vtc_config_test.cpp。
|
||||
覆盖（物理追踪思路 §8、需求分析 NFR-007）：真实配置文件加载成功与取值
|
||||
正确（含 A1/A2 修改后的字段结构）、缺字段 / 缺分组 / 非法枚举 / 非法数值 /
|
||||
文件缺失均抛带字段名的 ValueError。
|
||||
|
||||
C++ 版用字符串替换构造非法变体；Python 版改为 dict 变异（更稳健），
|
||||
临时文件经 pytest 的 tmp_path 提供。
|
||||
"""
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from visual_track_control.config import (HeadingMode, NoDistanceAction,
|
||||
load_from_json)
|
||||
|
||||
_REPO_ROOT = Path(__file__).resolve().parents[3]
|
||||
REAL_CONFIG = _REPO_ROOT / "reference-c++" / "visual_track_control" / "config" / "track_control.json"
|
||||
|
||||
|
||||
def full_config() -> dict:
|
||||
"""与真实配置同构的完整合法配置（用于派生各非法变体）。"""
|
||||
return {
|
||||
"control": {
|
||||
"frequency_hz": 20.0,
|
||||
"visual_stale_ms": 200,
|
||||
"radar_stale_ms": 500,
|
||||
"attitude_stale_ms": 300,
|
||||
},
|
||||
"camera": {
|
||||
"image_width": 1920,
|
||||
"image_height": 1080,
|
||||
"fov_h_deg": 60.0,
|
||||
"fov_v_deg": 35.0,
|
||||
},
|
||||
"heading": {
|
||||
"mode": "rate",
|
||||
"gain": 1.0,
|
||||
"yaw_rate_limit_dps": 30.0,
|
||||
"yaw_slew_limit_dps2": 60.0,
|
||||
},
|
||||
"vertical": {
|
||||
"vz_gain_mps_per_deg": 0.1,
|
||||
"vz_limit_mps": 2.0,
|
||||
},
|
||||
"distance": {
|
||||
"d_exp_m": 10.0,
|
||||
"kp": 0.5,
|
||||
"ki": 0.05,
|
||||
"kd": 0.1,
|
||||
"integral_limit": 2.0,
|
||||
"derivative_filter_coef": 0.2,
|
||||
"approach_velocity_limit_mps": 3.0,
|
||||
"retreat_velocity_limit_mps": 1.0,
|
||||
"no_distance_action": "hold",
|
||||
"no_distance_approach_limit_mps": 0.5,
|
||||
},
|
||||
"accel_limit": {
|
||||
"ax_mps2": 2.0,
|
||||
"ay_mps2": 2.0,
|
||||
"az_mps2": 1.5,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def write_temp_json(tmp_path: Path, config: dict, name: str) -> str:
|
||||
path = tmp_path / name
|
||||
path.write_text(json.dumps(config), encoding="utf-8")
|
||||
return str(path)
|
||||
|
||||
|
||||
def test_load_real_config_succeeds():
|
||||
cfg = load_from_json(str(REAL_CONFIG))
|
||||
assert cfg.control.frequency_hz == 20.0
|
||||
assert cfg.control.visual_stale_ms == 200
|
||||
assert cfg.camera.image_width == 1920
|
||||
assert cfg.camera.image_height == 1080
|
||||
assert cfg.camera.fov_h_deg == 60.0
|
||||
assert cfg.heading.mode is HeadingMode.RATE # A2：默认速率式
|
||||
assert cfg.heading.yaw_rate_limit_dps == 30.0
|
||||
assert cfg.vertical.vz_gain_mps_per_deg == 0.1
|
||||
assert cfg.vertical.vz_limit_mps == 2.0
|
||||
assert cfg.distance.d_exp_m == 10.0
|
||||
assert cfg.distance.no_distance_action is NoDistanceAction.HOLD
|
||||
assert cfg.accel_limit.ax_mps2 == 2.0
|
||||
|
||||
|
||||
def test_full_json_round_trip_matches_expected(tmp_path):
|
||||
# 内嵌合法配置与真实配置文件行为一致
|
||||
path = write_temp_json(tmp_path, full_config(), "vtc_cfg_full.json")
|
||||
cfg = load_from_json(path)
|
||||
assert cfg.heading.mode is HeadingMode.RATE
|
||||
assert cfg.distance.kp == 0.5
|
||||
|
||||
|
||||
def test_position_mode_accepted(tmp_path):
|
||||
config = full_config()
|
||||
config["heading"]["mode"] = "position"
|
||||
path = write_temp_json(tmp_path, config, "vtc_cfg_position.json")
|
||||
cfg = load_from_json(path)
|
||||
assert cfg.heading.mode is HeadingMode.POSITION
|
||||
|
||||
|
||||
def test_missing_file_throws_with_path():
|
||||
with pytest.raises(ValueError) as exc_info:
|
||||
load_from_json("/nonexistent/vtc_no_such_file.json")
|
||||
assert "vtc_no_such_file.json" in str(exc_info.value)
|
||||
|
||||
|
||||
def test_malformed_json_throws(tmp_path):
|
||||
path = tmp_path / "vtc_cfg_bad.json"
|
||||
path.write_text("{ this is not json", encoding="utf-8")
|
||||
with pytest.raises(ValueError):
|
||||
load_from_json(str(path))
|
||||
|
||||
|
||||
def test_missing_field_throws_with_field_name(tmp_path):
|
||||
config = full_config()
|
||||
del config["heading"]["gain"]
|
||||
path = write_temp_json(tmp_path, config, "vtc_cfg_missing.json")
|
||||
with pytest.raises(ValueError) as exc_info:
|
||||
load_from_json(path)
|
||||
assert "heading.gain" in str(exc_info.value)
|
||||
|
||||
|
||||
def test_missing_section_throws(tmp_path):
|
||||
config = full_config()
|
||||
del config["accel_limit"]
|
||||
path = write_temp_json(tmp_path, config, "vtc_cfg_nosection.json")
|
||||
with pytest.raises(ValueError) as exc_info:
|
||||
load_from_json(path)
|
||||
assert "accel_limit" in str(exc_info.value)
|
||||
|
||||
|
||||
def test_invalid_heading_mode_throws(tmp_path):
|
||||
config = full_config()
|
||||
config["heading"]["mode"] = "magic"
|
||||
path = write_temp_json(tmp_path, config, "vtc_cfg_badmode.json")
|
||||
with pytest.raises(ValueError) as exc_info:
|
||||
load_from_json(path)
|
||||
assert "heading.mode" in str(exc_info.value)
|
||||
|
||||
|
||||
def test_invalid_no_distance_action_throws(tmp_path):
|
||||
config = full_config()
|
||||
config["distance"]["no_distance_action"] = "zoom"
|
||||
path = write_temp_json(tmp_path, config, "vtc_cfg_badaction.json")
|
||||
with pytest.raises(ValueError) as exc_info:
|
||||
load_from_json(path)
|
||||
assert "no_distance_action" in str(exc_info.value)
|
||||
|
||||
|
||||
def test_negative_frequency_throws(tmp_path):
|
||||
config = full_config()
|
||||
config["control"]["frequency_hz"] = -1.0
|
||||
path = write_temp_json(tmp_path, config, "vtc_cfg_negfreq.json")
|
||||
with pytest.raises(ValueError) as exc_info:
|
||||
load_from_json(path)
|
||||
assert "control.frequency_hz" in str(exc_info.value)
|
||||
|
||||
|
||||
def test_fov_out_of_range_throws(tmp_path):
|
||||
config = full_config()
|
||||
config["camera"]["fov_h_deg"] = 200.0
|
||||
path = write_temp_json(tmp_path, config, "vtc_cfg_badfov.json")
|
||||
with pytest.raises(ValueError) as exc_info:
|
||||
load_from_json(path)
|
||||
assert "camera.fov_h_deg" in str(exc_info.value)
|
||||
@@ -0,0 +1,41 @@
|
||||
# visual_track_control（Python 版）
|
||||
|
||||
视觉跟踪控制律：像素视线角 → 水平航向 / 垂直速度 / 距离 PID 三通道 + 统一限幅 +
|
||||
状态机（正常 / Coast / 判丢刹车 / 降级保持）+ MAVSDK offboard 指令输出。
|
||||
移植自 `reference-c++/visual_track_control/`（规格：docs/物理追踪思路.md）。
|
||||
|
||||
## 接口清单
|
||||
|
||||
| 名称 | 说明 |
|
||||
| --- | --- |
|
||||
| `TrackingConfig` / `load_from_json(path)` | 配置加载（启动期校验，错误携带字段名，抛 `ValueError`） |
|
||||
| `HeadingMode` / `NoDistanceAction` | 配置枚举（rate/position；hold/slow_approach/exit） |
|
||||
| `TrackingControlLaw(cfg)` | 主控律 |
|
||||
| `.update(snapshot, now_ms) -> ControlOutput` | 单周期八步流程（姿态/视觉超龄 → 判丢 → Coast → 三通道 → NED 旋转） |
|
||||
| `.reset()` | 停止闭环时清全部内部状态 |
|
||||
| `ControlSnapshot` | 输入快照（自带 `TrackState`，track_time_ms 由调用方打戳） |
|
||||
| `ControlOutput` | 输出：mode / valid / vx·vy·vz（NED）/ vx_body_mps（机体系前向）/ yaw_deg / yaw_rate_dps / degraded_reason / coast_active |
|
||||
| `PidController` / `PidParams` | 距离 PID（anti-windup 条件积分） |
|
||||
| `SlewRateLimiter` / `clamp` | 变化率限幅（初值 0 → 首条指令渐变，S6） |
|
||||
| `PixelToAngle` / `is_stale` / `rotate_body_to_ned` | 针孔视线角 / 超龄判定 / 机体→NED 旋转 |
|
||||
| `CommandSender`（Protocol） | 指令输出抽象（供注入 mock） |
|
||||
| `MavsdkCommandGateway(drone)` | MAVSDK offboard 网关（async：start / send_velocity_heading / send_brake_hover / stop） |
|
||||
|
||||
## 与 C++ 版的差异
|
||||
|
||||
- **MAVLink → MAVSDK 语义映射**（替代 C++ 的 ID 84 手工编码）：
|
||||
- 角速率非零 → `set_velocity_body(forward, 0, down, yawspeed)`；
|
||||
- 航向角非零（角速率为零）→ `set_velocity_ned(north, east, down, yaw)`；
|
||||
- 两者均零 → `set_velocity_body` 配零角速率（保持航向，A2 回归语义）；
|
||||
- 刹车 → `set_velocity_body(0,0,0,0)`（MAVSDK 无刹车掩码的行为等效适配）；
|
||||
- 降级保持不发送（由调用方判断模式）。
|
||||
- **异步**：MAVSDK Python 为 asyncio 接口，网关方法均为协程；传输异常捕获后返回
|
||||
`False`（与 C++ 传输失败传播一致）。
|
||||
- **解耦**：C++ 的 ControlSnapshot 内嵌 `target_tracker::TrackResult` 是唯一耦合点；
|
||||
Python 版自带字段对齐的 `TrackState`（box/raw 为 plain tuple），两包互不 import，
|
||||
转换在胶水层（`utils/demo_track_control.py` 的 `track_result_to_state`）。
|
||||
- **新增字段** `ControlOutput.vx_body_mps`：旋转前的机体系前向速度，供速率式
|
||||
机体设定点直接使用（免反旋）。
|
||||
- **日志**：spdlog → 标准 `logging`，仅状态切换点打印（热路径零日志）不变。
|
||||
- 行为语义（姿态超龄优先于视觉、判丢进刹车清限幅器、Coast 清距离积分、
|
||||
dt 兜底 1/frequency_hz 上限 0.5s、WrapDeg180 回绕）与 C++ 一致，由移植测试锁定。
|
||||
@@ -0,0 +1,55 @@
|
||||
"""视觉跟踪控制包（visual_track_control Python 版）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/。本包与 target_tracker
|
||||
包互不依赖（TrackState 为字段对齐的独立值类型，胶水层负责转换）。
|
||||
|
||||
分层：
|
||||
纯算法构件 rate_limiter / staleness_checker / line_of_sight /
|
||||
frame_transform / pid_controller
|
||||
配置 config（TrackingConfig + load_from_json）
|
||||
主控律 tracking_control_law（TrackingControlLaw）
|
||||
指令输出 command_sender（抽象 Protocol）+ mavsdk_gateway（MAVSDK offboard）
|
||||
"""
|
||||
|
||||
from .command_sender import CommandSender
|
||||
from .config import (AccelLimitConfig, CameraGroupConfig, ControlGroupConfig,
|
||||
DistanceGroupConfig, HeadingGroupConfig, HeadingMode,
|
||||
NoDistanceAction, TrackingConfig, VerticalGroupConfig,
|
||||
load_from_json)
|
||||
from .frame_transform import rotate_body_to_ned
|
||||
from .line_of_sight import PixelToAngle
|
||||
from .mavsdk_gateway import MavsdkCommandGateway
|
||||
from .pid_controller import PidController, PidParams
|
||||
from .rate_limiter import SlewRateLimiter, clamp
|
||||
from .staleness_checker import is_stale
|
||||
from .tracking_control_law import TrackingControlLaw
|
||||
from .types import (ControlMode, ControlOutput, ControlSnapshot, DegradedReason,
|
||||
TrackState)
|
||||
|
||||
__all__ = [
|
||||
"CommandSender",
|
||||
"MavsdkCommandGateway",
|
||||
"TrackingControlLaw",
|
||||
"TrackingConfig",
|
||||
"load_from_json",
|
||||
"HeadingMode",
|
||||
"NoDistanceAction",
|
||||
"ControlGroupConfig",
|
||||
"CameraGroupConfig",
|
||||
"HeadingGroupConfig",
|
||||
"VerticalGroupConfig",
|
||||
"DistanceGroupConfig",
|
||||
"AccelLimitConfig",
|
||||
"ControlMode",
|
||||
"DegradedReason",
|
||||
"TrackState",
|
||||
"ControlSnapshot",
|
||||
"ControlOutput",
|
||||
"PidController",
|
||||
"PidParams",
|
||||
"SlewRateLimiter",
|
||||
"clamp",
|
||||
"is_stale",
|
||||
"PixelToAngle",
|
||||
"rotate_body_to_ned",
|
||||
]
|
||||
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.
@@ -0,0 +1,32 @@
|
||||
"""控制指令发送抽象（语义接口，对应 C++ ICommandTransport + 网关门面）。
|
||||
|
||||
C++ 版的 ICommandTransport 是字节流传输抽象（Send(bytes)），MAVLink 编码
|
||||
在网关内完成；Python 版采用 MAVSDK 语义映射后，"传输"被抽象为设定点级
|
||||
指令发送。本 Protocol 仅声明门面接口，供胶水层依赖注入与 mock 测试。
|
||||
|
||||
异常约定：实现不得向外抛传输层异常，失败一律返回 False（网关已捕获）。
|
||||
"""
|
||||
|
||||
from typing import Optional, Protocol, runtime_checkable
|
||||
|
||||
from .types import ControlOutput
|
||||
|
||||
|
||||
@runtime_checkable
|
||||
class CommandSender(Protocol):
|
||||
async def start(self) -> bool:
|
||||
"""进入 offboard 模式。失败返回 False。"""
|
||||
...
|
||||
|
||||
async def send_velocity_heading(self, output: ControlOutput,
|
||||
time_boot_ms: Optional[int] = None) -> bool:
|
||||
"""发送速度+航向意图（ControlMode.VELOCITY_HEADING）。失败返回 False。"""
|
||||
...
|
||||
|
||||
async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool:
|
||||
"""发送零速刹车/悬停意图（ControlMode.BRAKE_HOVER）。失败返回 False。"""
|
||||
...
|
||||
|
||||
async def stop(self) -> None:
|
||||
"""退出 offboard 模式（结束时调用）。"""
|
||||
...
|
||||
@@ -0,0 +1,278 @@
|
||||
"""视觉跟踪控制律配置（TrackingConfig + load_from_json）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/src/vtc_config.cpp
|
||||
（规格：docs/物理追踪思路.md §8：全部阈值集中配置，NFR-007）。
|
||||
|
||||
启动期一次性解析：缺字段 / 类型错误 / 非法取值抛 ValueError 并携带完整
|
||||
字段名（如 "heading.gain"）；解析结果与 JSON 脱钩，热路径不做热更新。
|
||||
"""
|
||||
|
||||
import json
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum
|
||||
from typing import Any
|
||||
|
||||
from .types import ControlMode # noqa: F401 (re-export 便利性)
|
||||
|
||||
|
||||
class HeadingMode(Enum):
|
||||
"""航向通道模式（§4.2）。默认速率式：yaw_rate 天然是相对量，无坐标系歧义。"""
|
||||
|
||||
RATE = "rate" # β → 偏航角速度，输出 yaw_rate_dps
|
||||
POSITION = "position" # β → 修正量，与当前航向合成绝对航向输出 yaw_deg
|
||||
|
||||
|
||||
class NoDistanceAction(Enum):
|
||||
"""雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3）。"""
|
||||
|
||||
HOLD = "hold" # 保持：前向速度指令清零
|
||||
SLOW_APPROACH = "slow_approach" # 限速接近：按 no_distance_approach_limit_mps 缓速
|
||||
EXIT = "exit" # 退出近距操作：输出降级保持
|
||||
|
||||
|
||||
@dataclass
|
||||
class ControlGroupConfig:
|
||||
frequency_hz: float = 20.0
|
||||
visual_stale_ms: int = 200
|
||||
radar_stale_ms: int = 500
|
||||
attitude_stale_ms: int = 300
|
||||
|
||||
|
||||
@dataclass
|
||||
class CameraGroupConfig:
|
||||
image_width: int = 1920
|
||||
image_height: int = 1080
|
||||
fov_h_deg: float = 60.0
|
||||
fov_v_deg: float = 35.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class HeadingGroupConfig:
|
||||
mode: HeadingMode = HeadingMode.RATE
|
||||
gain: float = 1.0
|
||||
yaw_rate_limit_dps: float = 30.0
|
||||
yaw_slew_limit_dps2: float = 60.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class VerticalGroupConfig:
|
||||
vz_gain_mps_per_deg: float = 0.1
|
||||
vz_limit_mps: float = 2.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class DistanceGroupConfig:
|
||||
d_exp_m: float = 10.0
|
||||
kp: float = 0.0
|
||||
ki: float = 0.0
|
||||
kd: float = 0.0
|
||||
integral_limit: float = 0.0
|
||||
derivative_filter_coef: float = 0.0
|
||||
approach_velocity_limit_mps: float = 3.0
|
||||
retreat_velocity_limit_mps: float = 1.0
|
||||
no_distance_action: NoDistanceAction = NoDistanceAction.HOLD
|
||||
no_distance_approach_limit_mps: float = 0.5
|
||||
|
||||
|
||||
@dataclass
|
||||
class AccelLimitConfig:
|
||||
ax_mps2: float = 2.0
|
||||
ay_mps2: float = 2.0
|
||||
az_mps2: float = 1.5
|
||||
|
||||
|
||||
@dataclass
|
||||
class TrackingConfig:
|
||||
control: ControlGroupConfig = field(default_factory=ControlGroupConfig)
|
||||
camera: CameraGroupConfig = field(default_factory=CameraGroupConfig)
|
||||
heading: HeadingGroupConfig = field(default_factory=HeadingGroupConfig)
|
||||
vertical: VerticalGroupConfig = field(default_factory=VerticalGroupConfig)
|
||||
distance: DistanceGroupConfig = field(default_factory=DistanceGroupConfig)
|
||||
accel_limit: AccelLimitConfig = field(default_factory=AccelLimitConfig)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# JSON 解析与校验（错误消息携带完整字段名，与 C++ 版文本一致）
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def _require_section(root: dict, section: str) -> dict:
|
||||
value = root.get(section)
|
||||
if not isinstance(value, dict):
|
||||
raise ValueError(f"配置缺失分组或类型错误: {section}")
|
||||
return value
|
||||
|
||||
|
||||
def _is_number(value: Any) -> bool:
|
||||
return isinstance(value, (int, float)) and not isinstance(value, bool)
|
||||
|
||||
|
||||
def _is_integer(value: Any) -> bool:
|
||||
return isinstance(value, int) and not isinstance(value, bool)
|
||||
|
||||
|
||||
def _require_number(section: dict, section_name: str, key: str) -> float:
|
||||
value = section.get(key)
|
||||
if not _is_number(value):
|
||||
raise ValueError(f"配置缺失字段或类型错误: {section_name}.{key}")
|
||||
return float(value)
|
||||
|
||||
|
||||
def _require_integer(section: dict, section_name: str, key: str) -> int:
|
||||
value = section.get(key)
|
||||
if not _is_integer(value):
|
||||
raise ValueError(f"配置缺失字段或类型错误(需整数): {section_name}.{key}")
|
||||
return int(value)
|
||||
|
||||
|
||||
def _require_string(section: dict, section_name: str, key: str) -> str:
|
||||
value = section.get(key)
|
||||
if not isinstance(value, str):
|
||||
raise ValueError(f"配置缺失字段或类型错误(需字符串): {section_name}.{key}")
|
||||
return value
|
||||
|
||||
|
||||
def _require_positive(value: float, field_name: str) -> float:
|
||||
if not (value > 0.0):
|
||||
raise ValueError(f"配置非法(必须为正): {field_name}")
|
||||
return value
|
||||
|
||||
|
||||
def _require_non_negative(value: float, field_name: str) -> float:
|
||||
if value < 0.0:
|
||||
raise ValueError(f"配置非法(不得为负): {field_name}")
|
||||
return value
|
||||
|
||||
|
||||
def _require_range(value: float, lo: float, hi: float, field_name: str) -> float:
|
||||
if not (lo < value < hi):
|
||||
raise ValueError(f"配置非法(超出范围): {field_name}")
|
||||
return value
|
||||
|
||||
|
||||
def _load_control(root: dict) -> ControlGroupConfig:
|
||||
s = _require_section(root, "control")
|
||||
c = ControlGroupConfig()
|
||||
c.frequency_hz = _require_positive(
|
||||
_require_number(s, "control", "frequency_hz"), "control.frequency_hz")
|
||||
c.visual_stale_ms = _require_integer(s, "control", "visual_stale_ms")
|
||||
c.radar_stale_ms = _require_integer(s, "control", "radar_stale_ms")
|
||||
c.attitude_stale_ms = _require_integer(s, "control", "attitude_stale_ms")
|
||||
if c.visual_stale_ms <= 0 or c.radar_stale_ms <= 0 or c.attitude_stale_ms <= 0:
|
||||
raise ValueError("配置非法: control.*_stale_ms 必须为正")
|
||||
return c
|
||||
|
||||
|
||||
def _load_camera(root: dict) -> CameraGroupConfig:
|
||||
s = _require_section(root, "camera")
|
||||
c = CameraGroupConfig()
|
||||
c.image_width = int(_require_positive(
|
||||
float(_require_integer(s, "camera", "image_width")), "camera.image_width"))
|
||||
c.image_height = int(_require_positive(
|
||||
float(_require_integer(s, "camera", "image_height")), "camera.image_height"))
|
||||
c.fov_h_deg = _require_range(_require_number(s, "camera", "fov_h_deg"),
|
||||
0.0, 180.0, "camera.fov_h_deg")
|
||||
c.fov_v_deg = _require_range(_require_number(s, "camera", "fov_v_deg"),
|
||||
0.0, 180.0, "camera.fov_v_deg")
|
||||
return c
|
||||
|
||||
|
||||
def _load_heading(root: dict) -> HeadingGroupConfig:
|
||||
s = _require_section(root, "heading")
|
||||
h = HeadingGroupConfig()
|
||||
mode = _require_string(s, "heading", "mode")
|
||||
if mode == "position":
|
||||
h.mode = HeadingMode.POSITION
|
||||
elif mode == "rate":
|
||||
h.mode = HeadingMode.RATE
|
||||
else:
|
||||
raise ValueError("配置非法: heading.mode 仅支持 position / rate")
|
||||
h.gain = _require_non_negative(_require_number(s, "heading", "gain"), "heading.gain")
|
||||
h.yaw_rate_limit_dps = _require_positive(
|
||||
_require_number(s, "heading", "yaw_rate_limit_dps"), "heading.yaw_rate_limit_dps")
|
||||
h.yaw_slew_limit_dps2 = _require_positive(
|
||||
_require_number(s, "heading", "yaw_slew_limit_dps2"), "heading.yaw_slew_limit_dps2")
|
||||
return h
|
||||
|
||||
|
||||
def _load_vertical(root: dict) -> VerticalGroupConfig:
|
||||
s = _require_section(root, "vertical")
|
||||
v = VerticalGroupConfig()
|
||||
v.vz_gain_mps_per_deg = _require_non_negative(
|
||||
_require_number(s, "vertical", "vz_gain_mps_per_deg"),
|
||||
"vertical.vz_gain_mps_per_deg")
|
||||
v.vz_limit_mps = _require_positive(
|
||||
_require_number(s, "vertical", "vz_limit_mps"), "vertical.vz_limit_mps")
|
||||
return v
|
||||
|
||||
|
||||
def _load_distance(root: dict) -> DistanceGroupConfig:
|
||||
s = _require_section(root, "distance")
|
||||
d = DistanceGroupConfig()
|
||||
d.d_exp_m = _require_positive(_require_number(s, "distance", "d_exp_m"),
|
||||
"distance.d_exp_m")
|
||||
d.kp = _require_non_negative(_require_number(s, "distance", "kp"), "distance.kp")
|
||||
d.ki = _require_non_negative(_require_number(s, "distance", "ki"), "distance.ki")
|
||||
d.kd = _require_non_negative(_require_number(s, "distance", "kd"), "distance.kd")
|
||||
d.integral_limit = _require_non_negative(
|
||||
_require_number(s, "distance", "integral_limit"), "distance.integral_limit")
|
||||
# 容许 -0.0：范围下界略低于 0，读入后归一化为 0
|
||||
d.derivative_filter_coef = _require_range(
|
||||
_require_number(s, "distance", "derivative_filter_coef"),
|
||||
-0.000001, 1.0, "distance.derivative_filter_coef")
|
||||
if d.derivative_filter_coef < 0.0:
|
||||
d.derivative_filter_coef = 0.0
|
||||
d.approach_velocity_limit_mps = _require_positive(
|
||||
_require_number(s, "distance", "approach_velocity_limit_mps"),
|
||||
"distance.approach_velocity_limit_mps")
|
||||
d.retreat_velocity_limit_mps = _require_positive(
|
||||
_require_number(s, "distance", "retreat_velocity_limit_mps"),
|
||||
"distance.retreat_velocity_limit_mps")
|
||||
action = _require_string(s, "distance", "no_distance_action")
|
||||
if action == "hold":
|
||||
d.no_distance_action = NoDistanceAction.HOLD
|
||||
elif action == "slow_approach":
|
||||
d.no_distance_action = NoDistanceAction.SLOW_APPROACH
|
||||
elif action == "exit":
|
||||
d.no_distance_action = NoDistanceAction.EXIT
|
||||
else:
|
||||
raise ValueError(
|
||||
"配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit")
|
||||
d.no_distance_approach_limit_mps = _require_non_negative(
|
||||
_require_number(s, "distance", "no_distance_approach_limit_mps"),
|
||||
"distance.no_distance_approach_limit_mps")
|
||||
return d
|
||||
|
||||
|
||||
def _load_accel_limit(root: dict) -> AccelLimitConfig:
|
||||
s = _require_section(root, "accel_limit")
|
||||
a = AccelLimitConfig()
|
||||
a.ax_mps2 = _require_positive(_require_number(s, "accel_limit", "ax_mps2"),
|
||||
"accel_limit.ax_mps2")
|
||||
a.ay_mps2 = _require_positive(_require_number(s, "accel_limit", "ay_mps2"),
|
||||
"accel_limit.ay_mps2")
|
||||
a.az_mps2 = _require_positive(_require_number(s, "accel_limit", "az_mps2"),
|
||||
"accel_limit.az_mps2")
|
||||
return a
|
||||
|
||||
|
||||
def load_from_json(path: str) -> TrackingConfig:
|
||||
"""从 JSON 文件一次性加载配置（启动期调用，调参重启生效）。"""
|
||||
try:
|
||||
with open(path, "r", encoding="utf-8") as fp:
|
||||
root = json.load(fp)
|
||||
except OSError:
|
||||
raise ValueError(f"配置文件打开失败: {path}") from None
|
||||
except json.JSONDecodeError as exc:
|
||||
raise ValueError(f"配置文件解析失败: {path} ({exc})") from None
|
||||
|
||||
if not isinstance(root, dict):
|
||||
raise ValueError(f"配置文件解析失败: {path} (根节点必须为对象)")
|
||||
|
||||
cfg = TrackingConfig()
|
||||
cfg.control = _load_control(root)
|
||||
cfg.camera = _load_camera(root)
|
||||
cfg.heading = _load_heading(root)
|
||||
cfg.vertical = _load_vertical(root)
|
||||
cfg.distance = _load_distance(root)
|
||||
cfg.accel_limit = _load_accel_limit(root)
|
||||
return cfg
|
||||
@@ -0,0 +1,25 @@
|
||||
"""机体系（前/右/下）→ 局部 NED（北/东/地）坐标旋转。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/frame_transform.h
|
||||
（规格：docs/物理追踪思路.md §2.3）。
|
||||
|
||||
速度设定值只随航向角 ψ（绕下轴旋转）旋转：滚转/俯仰对速度指令为二阶
|
||||
小量，由飞控内环处理。旋转为正交变换：合速度大小不变，因此"限幅在机
|
||||
体系完成、旋转在输出端执行"不破坏 §8 限幅约束。
|
||||
|
||||
正方向验证（NED：x=北，y=东，z=地；航向 ψ 自北顺时针为正）：
|
||||
机体前向 (1,0) 在 ψ=90°（机头朝东）时 → (0, 1) = 东；
|
||||
机体右向 (0,1) 在 ψ=0°（机头朝北）时 → (0, 1) = 东。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
|
||||
def rotate_body_to_ned(v_forward: float, v_right: float, v_down: float,
|
||||
yaw_rad: float) -> tuple[float, float, float]:
|
||||
"""绕下轴按航向角 ψ 旋转，返回 (北, 东, 地)；垂直分量不变。"""
|
||||
c = math.cos(yaw_rad)
|
||||
s = math.sin(yaw_rad)
|
||||
north = v_forward * c - v_right * s
|
||||
east = v_forward * s + v_right * c
|
||||
return north, east, v_down
|
||||
@@ -0,0 +1,25 @@
|
||||
"""像素偏移 → 视线角（针孔模型）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/src/line_of_sight.cpp
|
||||
（规格：docs/物理追踪思路.md §4.1 水平 β / §5.1 垂直 α）。
|
||||
|
||||
全程使用精确针孔模型 angle = atan(offset · 2·tan(θ/2) / size)：原线性近似
|
||||
双路切换在边界两侧不连续（θ=60°、边界比例 0.2 处约 8% 台阶），会向控制
|
||||
环路注入突变；atan 代价可忽略。构造期预计算针孔系数，Convert 仅一次乘加。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
|
||||
class PixelToAngle:
|
||||
def __init__(self, size_px: float, fov_rad: float) -> None:
|
||||
# 构造期校验：非法参数直接抛异常（初始化抛异常规范）
|
||||
if not (size_px > 0.0):
|
||||
raise ValueError("PixelToAngle: size_px 必须为正")
|
||||
if not (0.0 < fov_rad < math.pi):
|
||||
raise ValueError("PixelToAngle: fov_rad 必须在 (0, π) 内")
|
||||
self._pinhole_scale = 2.0 * math.tan(fov_rad / 2.0) / size_px
|
||||
|
||||
def convert(self, offset_px: float) -> float:
|
||||
"""偏移（像素，水平右正 / 垂直下正）→ 视线角（弧度，同向为正）。"""
|
||||
return math.atan(offset_px * self._pinhole_scale)
|
||||
@@ -0,0 +1,98 @@
|
||||
"""MAVSDK offboard 网关 —— 控制意图 → MAVSDK 设定点（语义映射）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/src/mavlink_command_gateway.cpp
|
||||
（C++ 版手工编码 MAVLink ID 84 字节流；Python 版改用 MAVSDK offboard API，
|
||||
按 A2 语义映射三分支）：
|
||||
|
||||
速率式（yaw_rate_dps ≠ 0）→ set_velocity_body(机体前向, 0, vz, yaw_rate)
|
||||
位置式（yaw_deg ≠ 0 且角速率为零）→ set_velocity_ned(NED 速度 + 绝对 yaw)
|
||||
两者均零 → set_velocity_body 配零角速率（保持当前航向，A2 回归语义）
|
||||
刹车 → set_velocity_body(0, 0, 0, 0)（MAVSDK 无刹车掩码的行为等效适配）
|
||||
|
||||
降级保持（DEGRADED_HOLD）不发送任何指令，由调用方（胶水层）判断模式。
|
||||
MAVSDK Python API 为 asyncio 异步接口，本网关方法均为协程；传输异常
|
||||
捕获后返回 False（与 C++ 传输失败传播一致），不向外抛异常。
|
||||
"""
|
||||
|
||||
import logging
|
||||
from typing import Any, Optional
|
||||
|
||||
from .types import ControlOutput
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class MavsdkCommandGateway:
|
||||
"""drone：mavsdk System（仅取 .offboard 属性，duck-typed 便于注入 mock）。"""
|
||||
|
||||
def __init__(self, drone: Any) -> None:
|
||||
self._offboard = drone.offboard
|
||||
self._started = False
|
||||
|
||||
async def start(self) -> bool:
|
||||
"""进入 offboard：PX4 要求先流式发送首条设定点再请求启动。"""
|
||||
try:
|
||||
await self._offboard.set_velocity_body(
|
||||
self._velocity_body(0.0, 0.0, 0.0, 0.0))
|
||||
await self._offboard.start()
|
||||
except Exception as exc: # OffboardError / 连接异常统一视为启动失败
|
||||
logger.warning("[MavsdkGateway] offboard 启动失败: %s", exc)
|
||||
return False
|
||||
self._started = True
|
||||
logger.info("[MavsdkGateway] offboard 已启动")
|
||||
return True
|
||||
|
||||
async def send_velocity_heading(self, output: ControlOutput,
|
||||
time_boot_ms: Optional[int] = None) -> bool:
|
||||
try:
|
||||
if output.yaw_rate_dps != 0.0:
|
||||
# 速率式：机体系速度 + 偏航角速度（NED 分量不再使用）
|
||||
await self._offboard.set_velocity_body(
|
||||
self._velocity_body(output.vx_body_mps, 0.0, output.vz_mps,
|
||||
output.yaw_rate_dps))
|
||||
elif output.yaw_deg != 0.0:
|
||||
# 位置式：NED 速度 + 绝对航向（A2：绝对航向合成在控制律完成）
|
||||
await self._offboard.set_velocity_ned(
|
||||
self._velocity_ned(output.vx_mps, output.vy_mps,
|
||||
output.vz_mps, output.yaw_deg))
|
||||
else:
|
||||
# 两者均零：机体系速度 + 零角速率 = 保持当前航向（A2 回归语义）
|
||||
await self._offboard.set_velocity_body(
|
||||
self._velocity_body(output.vx_body_mps, 0.0, output.vz_mps,
|
||||
0.0))
|
||||
except Exception as exc:
|
||||
logger.warning("[MavsdkGateway] 速度航向指令发送失败: %s", exc)
|
||||
return False
|
||||
return True
|
||||
|
||||
async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool:
|
||||
try:
|
||||
await self._offboard.set_velocity_body(
|
||||
self._velocity_body(0.0, 0.0, 0.0, 0.0))
|
||||
except Exception as exc:
|
||||
logger.warning("[MavsdkGateway] 刹车指令发送失败: %s", exc)
|
||||
return False
|
||||
return True
|
||||
|
||||
async def stop(self) -> None:
|
||||
try:
|
||||
await self._offboard.stop()
|
||||
except Exception as exc:
|
||||
logger.warning("[MavsdkGateway] offboard 停止失败: %s", exc)
|
||||
self._started = False
|
||||
logger.info("[MavsdkGateway] offboard 已停止")
|
||||
|
||||
@staticmethod
|
||||
def _velocity_body(forward: float, right: float, down: float,
|
||||
yawspeed: float) -> Any:
|
||||
from mavsdk import offboard as _mavsdk_offboard
|
||||
return _mavsdk_offboard.VelocityBodyYawspeed(
|
||||
forward_m_s=forward, right_m_s=right, down_m_s=down,
|
||||
yawspeed_deg_s=yawspeed)
|
||||
|
||||
@staticmethod
|
||||
def _velocity_ned(north: float, east: float, down: float,
|
||||
yaw: float) -> Any:
|
||||
from mavsdk import offboard as _mavsdk_offboard
|
||||
return _mavsdk_offboard.VelocityNedYaw(
|
||||
north_m_s=north, east_m_s=east, down_m_s=down, yaw_deg=yaw)
|
||||
@@ -0,0 +1,95 @@
|
||||
"""位置式 PID 控制器（纯算法构件）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/src/pid_controller.cpp
|
||||
（规格：docs/物理追踪思路.md §6.2 距离控制律、§6.4 Coast 清积分）。
|
||||
|
||||
anti-windup（条件积分）：输出饱和且本拍积分增量（error·dt）仍在推深饱和
|
||||
时冻结本步积分累加；判据刻意基于"增量方向"而非"积分项与输出同向"——
|
||||
后者在误差反号后仍会冻结回卷，导致饱和残留。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
from .rate_limiter import clamp
|
||||
|
||||
|
||||
@dataclass
|
||||
class PidParams:
|
||||
"""PID 参数（全部来自配置，代码不写死任何数值）。"""
|
||||
|
||||
kp: float = 0.0
|
||||
ki: float = 0.0
|
||||
kd: float = 0.0
|
||||
integral_limit: float = 0.0 # 积分项限幅（<=0 表示不限幅）
|
||||
derivative_filter_coef: float = 0.0 # 微分一阶低通系数 ∈ [0,1)：0 = 不滤波
|
||||
output_limit: float = 0.0 # 输出限幅（<=0 表示不限幅）
|
||||
|
||||
|
||||
class PidController:
|
||||
def __init__(self, params: PidParams) -> None:
|
||||
self._params = params
|
||||
self._integral = 0.0
|
||||
self._prev_error = 0.0
|
||||
self._filtered_derivative = 0.0
|
||||
self._last_output = 0.0
|
||||
self._has_prev_error = False
|
||||
|
||||
def update(self, error: float, dt_s: float) -> float:
|
||||
"""单步更新：输入误差与采样间隔（秒），返回限幅后的输出。
|
||||
|
||||
dt_s <= 0 时保持上次输出（异常节拍保护）。
|
||||
"""
|
||||
if dt_s <= 0.0:
|
||||
return self._last_output
|
||||
|
||||
# 积分：候选值先钳位（积分限幅）
|
||||
integral_candidate = self._integral + error * dt_s
|
||||
if self._params.integral_limit > 0.0:
|
||||
integral_candidate = clamp(integral_candidate,
|
||||
-self._params.integral_limit,
|
||||
self._params.integral_limit)
|
||||
|
||||
# 微分：一阶低通滤波（系数 ∈ [0,1)，0 = 不滤波）
|
||||
derivative = 0.0
|
||||
if self._has_prev_error:
|
||||
derivative = (error - self._prev_error) / dt_s
|
||||
coef = self._params.derivative_filter_coef
|
||||
derivative = coef * self._filtered_derivative + (1.0 - coef) * derivative
|
||||
self._filtered_derivative = derivative
|
||||
self._prev_error = error
|
||||
self._has_prev_error = True
|
||||
|
||||
# 位置式合成
|
||||
integral_term = self._params.ki * integral_candidate
|
||||
raw = self._params.kp * error + integral_term + self._params.kd * derivative
|
||||
|
||||
# anti-windup（条件积分）：输出饱和且本拍积分增量仍在推深饱和时冻结
|
||||
saturated = (self._params.output_limit > 0.0
|
||||
and abs(raw) > self._params.output_limit)
|
||||
increment_pushes_deeper = (error * dt_s) * raw > 0.0
|
||||
if not (saturated and increment_pushes_deeper):
|
||||
self._integral = integral_candidate
|
||||
|
||||
# 输出限幅
|
||||
if self._params.output_limit > 0.0:
|
||||
self._last_output = clamp(raw, -self._params.output_limit,
|
||||
self._params.output_limit)
|
||||
else:
|
||||
self._last_output = raw
|
||||
return self._last_output
|
||||
|
||||
def reset_integral(self) -> None:
|
||||
"""清空积分项（§6.4：Coast 时防止积分饱和）；保留微分历史与上次输出。"""
|
||||
self._integral = 0.0
|
||||
|
||||
def reset(self) -> None:
|
||||
"""清空全部内部状态（积分、微分历史、上次输出）。"""
|
||||
self._integral = 0.0
|
||||
self._prev_error = 0.0
|
||||
self._filtered_derivative = 0.0
|
||||
self._last_output = 0.0
|
||||
self._has_prev_error = False
|
||||
|
||||
@property
|
||||
def last_output(self) -> float:
|
||||
return self._last_output
|
||||
@@ -0,0 +1,37 @@
|
||||
"""限幅与变化率限制（纯算法构件）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h
|
||||
（规格：docs/物理追踪思路.md §8、状态机设计 S6 首条指令限幅）。
|
||||
"""
|
||||
|
||||
def clamp(v: float, lo: float, hi: float) -> float:
|
||||
"""静态限幅：将 v 钳制在 [lo, hi]（调用方保证 lo <= hi）。"""
|
||||
return min(max(v, lo), hi)
|
||||
|
||||
|
||||
class SlewRateLimiter:
|
||||
"""变化率限幅器：限制输出每秒最大变化量（单位由调用方决定）。
|
||||
|
||||
初值置 0 时天然实现"首条指令突变抑制"（S6）：切换进入视觉跟踪后
|
||||
首条指令从零按加速度限幅渐变。
|
||||
"""
|
||||
|
||||
def __init__(self, rate_limit: float, initial: float = 0.0) -> None:
|
||||
self._rate_limit = rate_limit
|
||||
self._output = initial
|
||||
|
||||
def limit(self, target: float, dt_s: float) -> float:
|
||||
"""朝 target 推进一个节拍；dt_s <= 0 或 rate_limit <= 0 时保持当前输出。"""
|
||||
if dt_s <= 0.0 or self._rate_limit <= 0.0:
|
||||
return self._output
|
||||
max_step = self._rate_limit * dt_s
|
||||
self._output = clamp(target, self._output - max_step, self._output + max_step)
|
||||
return self._output
|
||||
|
||||
def reset(self, value: float = 0.0) -> None:
|
||||
"""复位输出值（刹车/重置时归零，保证恢复跟踪后指令从零渐变）。"""
|
||||
self._output = value
|
||||
|
||||
@property
|
||||
def last_output(self) -> float:
|
||||
return self._output
|
||||
@@ -0,0 +1,19 @@
|
||||
"""输入超龄判定（单调毫秒时钟）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h
|
||||
（规格：docs/物理追踪思路.md §7.1：超龄数据只能触发降级或告警）。
|
||||
"""
|
||||
|
||||
|
||||
def is_stale(sample_time_ms: int, now_ms: int, valid_for_ms: int) -> bool:
|
||||
"""输入超龄判定。
|
||||
|
||||
sample_time_ms <= 0 视为从未取样，直接判超龄；now_ms < sample_time_ms
|
||||
（时间戳不一致：时钟源错配/打戳错误）同样判超龄——单一单调时钟下不应
|
||||
发生，宁可降级也不信任不可核实的取样。
|
||||
"""
|
||||
if sample_time_ms <= 0:
|
||||
return True
|
||||
if now_ms < sample_time_ms:
|
||||
return True
|
||||
return (now_ms - sample_time_ms) > valid_for_ms
|
||||
@@ -0,0 +1,220 @@
|
||||
"""视觉跟踪主控律（TrackingControlLaw）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/src/tracking_control_law.cpp
|
||||
（规格：docs/物理追踪思路.md §2.3/§4/§5/§6/§7/§8/§9）。
|
||||
|
||||
单周期八步流程：
|
||||
① 超龄判定（姿态优先于视觉，超龄 → 降级保持）
|
||||
② 判丢 → 零速刹车（进入时清积分与限幅器历史）
|
||||
③ Coast（is_predicted）→ 清距离积分防饱和
|
||||
④ 像素偏移 → 视线角 β/α（全程针孔模型）
|
||||
⑤ 水平航向（速率式/位置式，A2：位置式合成绝对 NED 航向）
|
||||
⑥ 垂直统一速度支路（A1：不切姿态微调模式）
|
||||
⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit）
|
||||
⑧ 机体系 → 局部 NED 旋转（A4）
|
||||
|
||||
日志纪律与 C++ 版一致：仅状态切换点打印，热路径零日志。
|
||||
Python 扩展：ControlOutput.vx_body_mps 记录旋转前的机体系前向分量，
|
||||
供 MAVSDK 机体速度设定点使用。
|
||||
"""
|
||||
|
||||
import logging
|
||||
import math
|
||||
|
||||
from .config import HeadingMode, NoDistanceAction, TrackingConfig
|
||||
from .frame_transform import rotate_body_to_ned
|
||||
from .line_of_sight import PixelToAngle
|
||||
from .pid_controller import PidController, PidParams
|
||||
from .rate_limiter import SlewRateLimiter, clamp
|
||||
from .staleness_checker import is_stale
|
||||
from .types import ControlMode, ControlOutput, ControlSnapshot, DegradedReason
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
_K_PI = 3.14159265358979323846
|
||||
_K_RAD_TO_DEG = 180.0 / _K_PI
|
||||
_K_DEG_TO_RAD = _K_PI / 180.0
|
||||
_K_MAX_DT_S = 0.5 # 单步 dt 上限（秒）：异常节拍保护
|
||||
|
||||
_MODE_NAMES = {
|
||||
ControlMode.VELOCITY_HEADING: "速度航向",
|
||||
ControlMode.BRAKE_HOVER: "零速刹车",
|
||||
ControlMode.DEGRADED_HOLD: "降级保持",
|
||||
}
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
def _build_distance_pid_params(cfg: TrackingConfig) -> PidParams:
|
||||
"""由距离配置构造 PID 参数（§6.2）；输出限幅取接近/后退限幅的较大者。"""
|
||||
return PidParams(
|
||||
kp=cfg.distance.kp,
|
||||
ki=cfg.distance.ki,
|
||||
kd=cfg.distance.kd,
|
||||
integral_limit=cfg.distance.integral_limit,
|
||||
derivative_filter_coef=cfg.distance.derivative_filter_coef,
|
||||
output_limit=max(cfg.distance.approach_velocity_limit_mps,
|
||||
cfg.distance.retreat_velocity_limit_mps),
|
||||
)
|
||||
|
||||
|
||||
class TrackingControlLaw:
|
||||
def __init__(self, cfg: TrackingConfig) -> None:
|
||||
self._cfg = cfg
|
||||
self._pixel_to_beta = PixelToAngle(cfg.camera.image_width,
|
||||
cfg.camera.fov_h_deg * _K_DEG_TO_RAD)
|
||||
self._pixel_to_alpha = PixelToAngle(cfg.camera.image_height,
|
||||
cfg.camera.fov_v_deg * _K_DEG_TO_RAD)
|
||||
self._distance_pid = PidController(_build_distance_pid_params(cfg))
|
||||
self._yaw_deg_limiter = SlewRateLimiter(cfg.heading.yaw_slew_limit_dps2)
|
||||
self._yaw_rate_limiter = SlewRateLimiter(cfg.heading.yaw_slew_limit_dps2)
|
||||
self._vx_limiter = SlewRateLimiter(cfg.accel_limit.ax_mps2)
|
||||
self._vz_limiter = SlewRateLimiter(cfg.accel_limit.az_mps2)
|
||||
self._last_now_ms = 0
|
||||
self._last_mode = ControlMode.DEGRADED_HOLD
|
||||
|
||||
def update(self, snapshot: ControlSnapshot, now_ms: int) -> ControlOutput:
|
||||
# ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ----
|
||||
# 时钟回拨或 now_ms 未前进（同值）时不信任差值，回退配置频率。
|
||||
dt_s = 1.0 / self._cfg.control.frequency_hz
|
||||
if self._last_now_ms > 0 and now_ms > self._last_now_ms:
|
||||
dt_s = min((now_ms - self._last_now_ms) / 1000.0, _K_MAX_DT_S)
|
||||
self._last_now_ms = now_ms
|
||||
|
||||
out = ControlOutput()
|
||||
out.mode = ControlMode.VELOCITY_HEADING # 正常路径默认模式，分支可覆盖
|
||||
|
||||
# ---- ① 超龄判定（§7.1 / §9.2） ----
|
||||
# 姿态过期或缺失：禁止空间转换和控制，立即退出正常控制（§9.2）
|
||||
if (not snapshot.attitude_present
|
||||
or is_stale(snapshot.attitude_time_ms, now_ms,
|
||||
self._cfg.control.attitude_stale_ms)):
|
||||
out.mode = ControlMode.DEGRADED_HOLD
|
||||
out.degraded_reason = DegradedReason.ATTITUDE_STALE
|
||||
return self._finish_and_log(out, "姿态过期/缺失，禁止控制")
|
||||
# 视觉追踪结果超龄：不得继续生成正常指令（§7.1）
|
||||
if is_stale(snapshot.track_time_ms, now_ms,
|
||||
self._cfg.control.visual_stale_ms):
|
||||
out.mode = ControlMode.DEGRADED_HOLD
|
||||
out.degraded_reason = DegradedReason.VISION_STALE
|
||||
return self._finish_and_log(out, "视觉追踪结果超龄")
|
||||
|
||||
# ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 BRAKE_HOVER） ----
|
||||
if not snapshot.track.tracked:
|
||||
if self._last_mode is not ControlMode.BRAKE_HOVER:
|
||||
# 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
|
||||
self._distance_pid.reset()
|
||||
self._yaw_deg_limiter.reset()
|
||||
self._yaw_rate_limiter.reset()
|
||||
self._vx_limiter.reset()
|
||||
self._vz_limiter.reset()
|
||||
out.mode = ControlMode.BRAKE_HOVER # 速度分量保持默认全零
|
||||
out.valid = True
|
||||
return self._finish_and_log(out, "判丢，零速刹车")
|
||||
|
||||
# ---- ③ Coast：is_predicted → 清积分防饱和（§6.4） ----
|
||||
if snapshot.track.is_predicted:
|
||||
self._distance_pid.reset_integral()
|
||||
# 航向通道为纯比例增益映射（§4.2），无积分项，无需清理
|
||||
out.coast_active = True
|
||||
|
||||
# ---- ④ 像素偏移 → 视线角（§4.1 水平 β / §5.1 垂直 α） ----
|
||||
# x_offset = cx − W/2（右正），y_offset = cy − H/2（下正）
|
||||
x_offset = snapshot.track.center[0] - self._cfg.camera.image_width * 0.5
|
||||
y_offset = snapshot.track.center[1] - self._cfg.camera.image_height * 0.5
|
||||
beta_deg = self._pixel_to_beta.convert(x_offset) * _K_RAD_TO_DEG
|
||||
alpha_deg = self._pixel_to_alpha.convert(y_offset) * _K_RAD_TO_DEG
|
||||
|
||||
# ---- ⑤ 水平航向通道（§4.2：增益 → slew → 角速度限幅） ----
|
||||
if self._cfg.heading.mode is HeadingMode.RATE:
|
||||
# 速率式：β → 偏航角速度指令，先变化率限幅再角速度限幅（FR-033）
|
||||
target_rate = self._cfg.heading.gain * beta_deg
|
||||
slewed = self._yaw_rate_limiter.limit(target_rate, dt_s)
|
||||
out.yaw_rate_dps = clamp(slewed, -self._cfg.heading.yaw_rate_limit_dps,
|
||||
self._cfg.heading.yaw_rate_limit_dps)
|
||||
else:
|
||||
# 位置式：β → 航向修正量，变化率限幅（FR-033）后与当前航向合成
|
||||
# 绝对 NED 航向——ID 84 的 yaw 字段是绝对值，直接发修正量会让
|
||||
# 机体转向"修正角"方向（A2 修复）。姿态已在 ① 通过超龄校验。
|
||||
target_yaw = self._cfg.heading.gain * beta_deg
|
||||
correction_deg = self._yaw_deg_limiter.limit(target_yaw, dt_s)
|
||||
out.yaw_deg = _wrap_deg_180(snapshot.yaw_rad * _K_RAD_TO_DEG
|
||||
+ correction_deg)
|
||||
|
||||
# ---- ⑥ 垂直速度通道（§5：统一速度支路） ----
|
||||
# 符号约定：α 下正（目标偏低）→ NED 下正 → vz 为正（下降趋近目标高度）。
|
||||
# 输出模式保持 VELOCITY_HEADING，距离通道前向速度不会被丢弃（A1 修复）。
|
||||
vz_target = self._cfg.vertical.vz_gain_mps_per_deg * alpha_deg
|
||||
slewed = self._vz_limiter.limit(vz_target, dt_s) # 加速度限幅（§8）
|
||||
out.vz_mps = clamp(slewed, -self._cfg.vertical.vz_limit_mps,
|
||||
self._cfg.vertical.vz_limit_mps)
|
||||
|
||||
# ---- ⑦ 距离通道（§6.2 / §6.3） ----
|
||||
# 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041）
|
||||
distance_available = (snapshot.associated_to_target
|
||||
and snapshot.radar_distance_m > 0.0
|
||||
and not is_stale(snapshot.radar_time_ms, now_ms,
|
||||
self._cfg.control.radar_stale_ms))
|
||||
if distance_available:
|
||||
# Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2）
|
||||
delta_d = snapshot.radar_distance_m - self._cfg.distance.d_exp_m
|
||||
vx_raw = self._distance_pid.update(delta_d, dt_s)
|
||||
slewed = self._vx_limiter.limit(vx_raw, dt_s) # 加速度限幅（§8）
|
||||
out.vx_mps = clamp(slewed,
|
||||
-self._cfg.distance.retreat_velocity_limit_mps,
|
||||
self._cfg.distance.approach_velocity_limit_mps)
|
||||
else:
|
||||
# 无可用目标距离：清积分防饱和，按配置降级动作（§6.3）
|
||||
self._distance_pid.reset_integral()
|
||||
action = self._cfg.distance.no_distance_action
|
||||
if action is NoDistanceAction.HOLD:
|
||||
# 保持：前向指令清零（安全占位语义）
|
||||
out.vx_mps = 0.0
|
||||
self._vx_limiter.reset(0.0)
|
||||
elif action is NoDistanceAction.SLOW_APPROACH:
|
||||
# 限速接近：缓速前向，同样经加速度限幅
|
||||
out.vx_mps = self._vx_limiter.limit(
|
||||
self._cfg.distance.no_distance_approach_limit_mps, dt_s)
|
||||
else: # NoDistanceAction.EXIT
|
||||
# 退出近距操作：整体降级保持
|
||||
out.mode = ControlMode.DEGRADED_HOLD
|
||||
out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT
|
||||
return self._finish_and_log(out, "距离未关联，退出近距操作")
|
||||
|
||||
# ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ----
|
||||
# 各通道限幅均在机体系完成（旋转为正交变换，合速度不变，限幅约束
|
||||
# 不被破坏）；输出端随快照航向旋转为北/东/地。旋转前记录机体系前向
|
||||
# 分量，供 MAVSDK 机体速度设定点直接使用（免反旋）。
|
||||
out.vx_body_mps = out.vx_mps
|
||||
out.vx_mps, out.vy_mps, out.vz_mps = rotate_body_to_ned(
|
||||
out.vx_mps, out.vy_mps, out.vz_mps, snapshot.yaw_rad)
|
||||
|
||||
out.valid = True
|
||||
return self._finish_and_log(out,
|
||||
"Coast 预测跟踪" if out.coast_active
|
||||
else "正常跟踪")
|
||||
|
||||
def reset(self) -> None:
|
||||
"""停止视觉闭环时清全部状态（§6.4）。"""
|
||||
self._distance_pid.reset()
|
||||
self._yaw_deg_limiter.reset()
|
||||
self._yaw_rate_limiter.reset()
|
||||
self._vx_limiter.reset()
|
||||
self._vz_limiter.reset()
|
||||
self._last_now_ms = 0
|
||||
self._last_mode = ControlMode.DEGRADED_HOLD
|
||||
|
||||
def _finish_and_log(self, out: ControlOutput, reason: str) -> ControlOutput:
|
||||
"""仅模式变化时打印状态切换日志（刹车进出、降级切换等）。"""
|
||||
if out.mode is not self._last_mode:
|
||||
logger.info("[ControlLaw] 状态切换: %s -> %s (%s)",
|
||||
_MODE_NAMES[self._last_mode], _MODE_NAMES[out.mode],
|
||||
reason)
|
||||
self._last_mode = out.mode
|
||||
return out
|
||||
@@ -0,0 +1,95 @@
|
||||
"""视觉跟踪控制律的中立输入/输出结构。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/vtc_types.h。
|
||||
|
||||
解耦说明：C++ 版的 ControlSnapshot 直接内嵌 target_tracker::TrackResult；本包为保持
|
||||
与 target_tracker 包互不依赖，自带字段对齐的 TrackState 值类型。调用方（胶水层）
|
||||
负责把 target_tracker.TrackResult 转换为 TrackState（字段均为 plain tuple/标量）。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, auto
|
||||
from typing import Optional
|
||||
|
||||
|
||||
class ControlMode(Enum):
|
||||
"""控制输出模式（§2.2 控制意图语义）。"""
|
||||
|
||||
VELOCITY_HEADING = auto() # 速度与航向意图
|
||||
BRAKE_HOVER = auto() # 零速刹车/悬停：判丢等场景
|
||||
DEGRADED_HOLD = auto() # 降级保持：超龄/退出近距操作，禁止正常控制
|
||||
|
||||
|
||||
class DegradedReason(Enum):
|
||||
"""降级原因（仅 DEGRADED_HOLD 模式有意义）。"""
|
||||
|
||||
NONE = auto()
|
||||
ATTITUDE_STALE = auto() # PX4 姿态过期：禁止空间转换和控制（§9.2）
|
||||
VISION_STALE = auto() # 视觉追踪结果超龄（§7.1）
|
||||
NO_DISTANCE_EXIT = auto() # 无距离关联且配置为退出近距操作（§6.3）
|
||||
|
||||
|
||||
@dataclass
|
||||
class TrackState:
|
||||
"""追踪结果视图（字段语义与 target_tracker.TrackResult 对齐，但独立定义）。"""
|
||||
|
||||
tracked: bool = False
|
||||
primary_id: Optional[int] = None
|
||||
center: tuple[float, float] = (0.0, 0.0)
|
||||
box: Optional[tuple[float, float, float, float]] = None
|
||||
confidence: float = 0.0
|
||||
is_predicted: bool = False
|
||||
lost_frames: int = 0
|
||||
n_active: int = 0
|
||||
raw: Optional[tuple[float, float, float, float]] = None
|
||||
|
||||
|
||||
@dataclass
|
||||
class ControlSnapshot:
|
||||
"""控制律输入快照：调用方在同一控制周期内采集并打戳。
|
||||
|
||||
TrackState 本身不含时间戳（§7.2），调用方必须在取样瞬间用单调时钟
|
||||
打戳填入 track_time_ms；超龄判定一律使用"取样时间 + valid_for_ms"。
|
||||
"""
|
||||
|
||||
track: TrackState = field(default_factory=TrackState)
|
||||
track_time_ms: int = 0 # 取样单调时钟（毫秒），<=0 视为从未取样
|
||||
|
||||
radar_distance_m: float = 0.0 # 机头前向单点距离（米）
|
||||
associated_to_target: bool = False # 相机—雷达空间关联成立标志（未关联不得当目标距离）
|
||||
radar_time_ms: int = 0
|
||||
|
||||
attitude_present: bool = False # 姿态源当前是否可用（过期 → 禁止控制）
|
||||
attitude_time_ms: int = 0
|
||||
yaw_rad: float = 0.0 # 航向（弧度，NED）；参与 NED 旋转与位置式航向合成
|
||||
roll_rad: float = 0.0 # 以下为预留字段（机体补偿未实现，§4.1）
|
||||
pitch_rad: float = 0.0
|
||||
yaw_rate_radps: float = 0.0
|
||||
velocity_x_mps: float = 0.0
|
||||
velocity_y_mps: float = 0.0
|
||||
velocity_z_mps: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class ControlOutput:
|
||||
"""控制律输出：限幅后的控制意图。
|
||||
|
||||
坐标语义：vx/vy/vz 为局部 NED（北/东/地）分量——各通道在机体系内计算
|
||||
并限幅后，随 snapshot.yaw_rad 旋转得到（旋转为正交变换，合速度不变）。
|
||||
vx_body_mps 为旋转前的机体系前向分量，供 MAVSDK 速度机体设定点使用
|
||||
（机体系侧向恒为 0，故不另设右向字段；vz 两系相同）。
|
||||
"""
|
||||
|
||||
mode: ControlMode = ControlMode.DEGRADED_HOLD
|
||||
valid: bool = False
|
||||
|
||||
vx_mps: float = 0.0 # 北向（§6.2）
|
||||
vy_mps: float = 0.0 # 东向（机体系恒 0，航向旋转后可非零）
|
||||
vz_mps: float = 0.0 # 垂直（NED 下正，§5.2）
|
||||
vx_body_mps: float = 0.0 # 机体系前向分量（旋转前，MAVSDK 机体设定点用）
|
||||
|
||||
yaw_deg: float = 0.0 # 位置式目标绝对航向（度，NED，右转为正，回绕 [-180,180)）
|
||||
yaw_rate_dps: float = 0.0 # 速率式偏航角速度指令（度/秒，右转为正）
|
||||
|
||||
degraded_reason: DegradedReason = DegradedReason.NONE
|
||||
coast_active: bool = False
|
||||
Reference in New Issue
Block a user