4.4 KiB
4.4 KiB
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 两包的胶水点
关键设计
- 解耦边界：C++ 版 ControlSnapshot 内嵌 tracker::TrackResult；Python 改为控制律自有的最小结构
TrackObservation(tracked, is_predicted, center)。visual_track_control 全程不出现 target_tracker 导入，demo 负责字段拷贝。两包内部一律相对导入、可独立搬移；mavsdk 只在 mavsdk_sender.py 内懒加载。 - 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 给接线片段。
- 速率式：
- 数值：全程 Python float（双精度），卡尔曼用 numpy（4×4 求逆 + 1e-9 正则化）；保留检测入口 NaN/Inf 过滤、置信度 NaN→−∞ 排序等防御逻辑；时间戳
time.monotonic_ns()//10**6。 - MAVSDK sender 测试：用 fake offboard 对象（鸭子类型）验证分发与积分行为，不 import mavsdk；gateway 相关 C++ 字节级用例改为验证 sender 收到的 VelocityNedYaw 内容。
验证与交付
pip install pytest（pytorch 环境）→ 两包测试全绿；- 运行 demo（asyncio，20Hz，三阶段：正常 6s → Coast 4s → 判丢 3s，演示判丢阈值 60 帧），输出语义与 C++ 版一致；
- mavsdk_sender 冒烟测试（fake offboard，不连实机）；
- 撰写 utils/README.md。
实施顺序
pip 安装 pytest → target_tracker 包+测试 → visual_track_control 包+测试 → sender 层+测试 → demo → README → 汇总报告。