first commit

2026-09-02 15:13:13 +08:00
commit cb00322a50
107 changed files with 10918 additions and 0 deletions
@@ -0,0 +1,49 @@
+# C++ → Python 移植计划（reference-c++ → utils）
+
+## 范围与环境
+- 移植 `reference-c++/target_tracker` 与 `reference-c++/visual_track_control` 两个子项目为 Python，放入 `utils/`；**不改动 reference-c++ 任何文件**。
+- Python 解释器：`D:\program file\conda\envs\pytorch\python.exe`（3.12.13，mavsdk 3.15.3 / numpy 2.4.3 已装）。需向该环境 `pip install pytest`。
+- 行为保真优先：单位、符号约定、限幅顺序、超龄/判丢/Coast 语义、异常消息文本全部与 C++ 版及规格文档一致。
+
+## 目录布局
+```
+utils/
+├── README.md                      # 唯一文档（结构、运行、MAVSDK 映射决策、实机接线片段）
+├── conftest.py                    # pytest sys.path 注入，使两包可顶层导入
+├── target_tracker/                # 包1：追踪库（仅依赖 stdlib + numpy）
+│   ├── __init__.py                # 导出 TargetTracker/TrackResult/Detection/FrameShape 等
+│   ├── kalman_box_filter.py       # 8 维恒速卡尔曼（numpy 矩阵；Q 用状态值、R 用 abs×2 的规格差异保留）
+│   ├── tracklet.py                # 轨道 + ToCenterSize/ToXYXY/ComputeIoU
+│   ├── target_tracker.py          # 六步主流程 + 贪婪关联 + 主目标锁定；LastResult 用 threading.Lock
+│   └── tests/                     # pytest 移植 27 条（kalman 5 / tracklet 12 / tracker 10，含 §9.4 六条验收）
+├── visual_track_control/          # 包2：控制律（不 import target_tracker）
+│   ├── __init__.py
+│   ├── vtc_types.py               # TrackObservation/ControlSnapshot/ControlOutput/枚举（frozen dataclass）
+│   ├── vtc_config.py              # stdlib json 加载，校验异常消息与 C++ 一致（含字段路径）
+│   ├── line_of_sight.py / pid_controller.py / rate_limiter.py / staleness_checker.py / frame_transform.py
+│   ├── tracking_control_law.py    # 八步单周期主流程，logging 仅模式切换打点
+│   ├── offboard_sender.py         # OffboardSender 协议 + YawIntegrator + LoopbackSender（演示/测试）
+│   ├── mavsdk_sender.py           # MAVSDK 实现（mavsdk 懒加载，其余模块零感知）
+│   ├── config/track_control.json  # 从 C++ 原样拷贝
+│   └── tests/                     # pytest 移植 8 套
+└── examples/
+    └── track_control_demo.py      # 唯一同时 import 两包的胶水点
+```
+
+## 关键设计
+1. **解耦边界**：C++ 版 ControlSnapshot 内嵌 tracker::TrackResult；Python 改为控制律自有的最小结构 `TrackObservation(tracked, is_predicted, center)`。visual_track_control 全程不出现 target_tracker 导入，demo 负责字段拷贝。两包内部一律相对导入、可独立搬移；mavsdk 只在 mavsdk_sender.py 内懒加载。
+2. **MAVSDK 映射（已确认方案）**：统一 `set_velocity_ned(VelocityNedYaw)`。
+   - 速率式：`YawIntegrator` 把 yaw_rate 积分为绝对航向（wrap180，dt 取发送侧单调时钟差）→ VelocityNedYaw(vn, ve, vd, 积分航向)；
+   - 位置式：直接用 out.yaw_deg；刹车：零速 + 保持积分航向；降级：不发送。
+   - 控制律输出仍与 C++ 一致（机体系计算限幅→输出端旋转为 NED），NED 分量直接对应 MAVSDK 字段；OffboardError 捕获后返回 False（对齐 C++ Send 布尔语义）。offboard.start()/stop() 生命周期不属于 sender，README 给接线片段。
+3. **数值**：全程 Python float（双精度），卡尔曼用 numpy（4×4 求逆 + 1e-9 正则化）；保留检测入口 NaN/Inf 过滤、置信度 NaN→−∞ 排序等防御逻辑；时间戳 `time.monotonic_ns()//10**6`。
+4. **MAVSDK sender 测试**：用 fake offboard 对象（鸭子类型）验证分发与积分行为，不 import mavsdk；gateway 相关 C++ 字节级用例改为验证 sender 收到的 VelocityNedYaw 内容。
+
+## 验证与交付
+1. `pip install pytest`（pytorch 环境）→ 两包测试全绿；
+2. 运行 demo（asyncio，20Hz，三阶段：正常 6s → Coast 4s → 判丢 3s，演示判丢阈值 60 帧），输出语义与 C++ 版一致；
+3. mavsdk_sender 冒烟测试（fake offboard，不连实机）；
+4. 撰写 utils/README.md。
+
+## 实施顺序
+pip 安装 pytest → target_tracker 包+测试 → visual_track_control 包+测试 → sender 层+测试 → demo → README → 汇总报告。