49 lines
4.4 KiB
Markdown
49 lines
4.4 KiB
Markdown
# 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 → 汇总报告。 |