diff --git a/.idea/code_drone-test.iml b/.idea/code_drone-test.iml index d0876a7..a7b1882 100644 --- a/.idea/code_drone-test.iml +++ b/.idea/code_drone-test.iml @@ -2,7 +2,7 @@ - + \ No newline at end of file diff --git a/.zcode/plans/plan-sess_b7314a3c-d6f0-45f0-9768-58161c9cf637.md b/.zcode/plans/plan-sess_b7314a3c-d6f0-45f0-9768-58161c9cf637.md deleted file mode 100644 index 2c6af37..0000000 --- a/.zcode/plans/plan-sess_b7314a3c-d6f0-45f0-9768-58161c9cf637.md +++ /dev/null @@ -1,49 +0,0 @@ -# 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 → 汇总报告。 \ No newline at end of file diff --git a/README.en.md b/README.en.md deleted file mode 100644 index 686bee4..0000000 --- a/README.en.md +++ /dev/null @@ -1,163 +0,0 @@ -# code-test-drone - -This project contains a C++ reference implementation of a visual target tracking and control system and its Python port, designed for drone tracking and control of moving targets. - -## Project Structure - -``` -code-test-drone/ -├── reference-c++/ # C++ reference implementation -│ ├── target_tracker/ # Target tracking library (independent sub-project) -│ │ ├── include/ # Header files directory -│ │ ├── src/ # Source files directory -│ │ └── tests/ # Unit tests -│ └── visual_track_control/# Visual tracking control library -│ ├── include/ # Header files directory -│ ├── src/ # Source files directory -│ └── tests/ # Unit tests -└── utils/ # Python port - ├── target_tracker/ # Target tracking (Python version) - ├── visual_track_control/# Tracking control (Python version) - └── tests/ # Unit tests -``` - -## Main Modules - -### target_tracker - Target Tracking Library - -Provides target tracking functionality based on Kalman filtering: - -- **KalmanBoxFilter**: 8-dimensional Kalman filter (position, velocity) -- **Tracklet**: Target trajectory management and state updates -- **TargetTracker**: Main tracker, containing association and election logic - -Core features: -- Greedy association algorithm for detection matching -- Three-level fallback mechanism for main target election -- Supports Coast (pure prediction) and Lost (missed) state management - -### visual_track_control - Visual Tracking Control Library - -Generates drone control commands based on tracking results: - -- **PIDController**: Distance PID control -- **TrackingControlLaw**: Tracking control law -- **MavlinkCommandGateway**: MAVLink command gateway -- **PixelToAngle**: Pixel to angle conversion -- **StalenessChecker**: Data staleness check -- **SlewRateLimiter**: Rate limiter - -Supports multiple control modes: -- **Rate Mode**: Angular rate control -- **Position Mode**: Absolute heading control -- **Distance Control**: Approach/retreat strategy - -## Environment Requirements - -### C++ Version -- CMake 3.10+ -- C++17 Compiler -- MAVLink Library (MAVSDK) - -### Python Version -- Python 3.12+ -- Dependencies: `numpy`, `pymavlink`, `mavsdk` - -## Installation - -### Python Environment -```bash -pip install -r requirements.txt -``` - -### C++ Build -```bash -cd reference-c++/target_tracker -mkdir build && cd build -cmake .. -make - -cd ../../visual_track_control -mkdir build && cd build -cmake .. -make -``` - -## Testing - -### Python Tests -```bash -cd utils -pytest -``` - -### C++ Tests -```bash -cd reference-c++/target_tracker/build -ctest - -cd ../../visual_track_control/build -ctest -``` - -## Configuration - -Both C++ and Python versions use JSON format configuration files: - -```json -{ - "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 - }, - "distance": { - "d_exp_m": 10.0, - "kp": 0.5, - "ki": 0.05, - "kd": 0.1, - "integral_limit": 2.0 - } -} -``` - -## Usage Examples - -### Python Tracking Demo -```bash -cd utils -python demo_track_control.py -``` - -### C++ Tracking Demo -```bash -cd reference-c++/visual_track_control/build -./track_control_demo -``` - -## Porting Notes - -Please refer to the README in each subdirectory for interface differences between the Python version and the C++ version: -- [target_tracker Python Version](./utils/target_tracker/README.md) -- [visual_track_control Python Version](./utils/visual_track_control/README.md) - -## License - -This project follows open source license agreements. - -## Contributing Guidelines - -Feel free to submit Issues and Pull Requests to improve this project. \ No newline at end of file diff --git a/README.md b/README.md index 0319ba7..ca3b414 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,4 @@ - -抱歉，我无法访问代码仓库。让我直接根据提供的代码结构为你生成 README。 - # code-test-drone 本项目包含一个视觉目标追踪与控制系统的 C++ 参考实现及其 Python 移植版本，用于无人机对移动目标的追踪与控制。 diff --git a/trajectory_plot.png b/trajectory_plot.png new file mode 100644 index 0000000..d6af646 Binary files /dev/null and b/trajectory_plot.png differ diff --git a/工作进度.md b/工作进度.md new file mode 100644 index 0000000..ad3ad22 --- /dev/null +++ b/工作进度.md @@ -0,0 +1,6 @@ +## 1.已完成工作： +- 两C++模块移植到python +- mock测试通过 +- 仿真测试一切正常 +## 2.未完成工作 +状态机代码尚未完成 \ No newline at end of file