wip: remove ugly implementation generated by ai, refactoring the interface design and add a yaw optimizer

AGENTS.md

@@ -1,3 +1,19 @@
+## SOP
+
+1. 项目构建方法：
+
+如果该项目位于 RMCS 的工作区下，可以用如下方法构建
+```zsh
+zsh -lc "build-rmcs --packages-up-to rmcs_auto_aim_v2"
+```
+
+## 代码风格
+
+- 头文件遵循 “本地头文件” “标准库” “第三方库” 的顺序排列，并在这些头文件加上空行用以区分，头文件较少时可以合并为一大块
+- RMCS_PIMPL_DEFINITION 这个宏应该放在 class/struct 的最顶上
+- 在局部作用域，优先 using namespace xxx 以简化要大量调用的同一命名空间下的函数或类
+- Solution 表示一个纯洁的算法，比如 pnp，统一定义在 utility/math/ 下，命名类似 XxxxSolution，XxxxOptimizer
+
 ## Agent 协作开发规范（Auto Aim）
 
 本文件用于约束 Agent 在本仓库中的行为，目标是：
@@ -6,49 +22,23 @@
 - 降低无边界代码生成带来的维护风险
 - 让 Agent 在可控范围内稳定提供辅助价值
 
-## 1. 指令优先级与冲突处理
-
-当多条指令冲突时，按以下优先级执行（高到低）：
-
-1. System 指令
-2. Developer 指令
-3. 本文档（AGENTS.md）
-4. 用户普通实现请求
-
-如发生冲突，必须遵循更高优先级指令，并在回复中简要说明原因。
-
-## 2. 核心原则：算法开发由人类主导
+## 核心原则：算法开发由人类主导
 
 Agent 不应主导大规模算法功能实现。面对“完整实现大需求”时，先进行风险判断。
 
-### 2.1 高风险判定（命中任意两条即视为高风险）
+### 高风险判定（命中任意两条即视为高风险）
 
 - 缺少充分且可执行的实现计划（Plan）
 - 缺少开发者提供的明确框架/边界/修改目标
 - 缺少验收标准、测试标准或回归范围
 
-### 2.2 高风险场景下的行为
+### 高风险场景下的行为
 
 - 不直接生成大规模 Feature 代码
 - 输出结构化建议：风险点、拆解方案、最小可验证下一步
 - 将主导权交还开发者，等待进一步明确指令
 
-## 3. 允许与禁止的工作边界
-
-### 3.1 允许（默认可执行）
-
-- 小范围修复（bug fix）
-- 已有方案下的局部实现
-- 测试补充、文档整理、日志与可观测性改进
-- 不改变核心算法路径的重构
-
-### 3.2 禁止（默认不执行）
-
-- 在缺少明确方案时主导完整算法功能开发
-- 未经约束地跨模块大改
-- 以“兜底默认值”掩盖配置或调用错误
-
-## 4. 慎重提取辅助函数
+## 慎重提取辅助函数
 
 提取公共辅助函数前必须评估：
 
@@ -58,7 +48,7 @@ Agent 不应主导大规模算法功能实现。面对“完整实现大需求
 
 若仅为单点使用或短期逻辑，优先保持局部实现。
 
-## 5. 关于非法条件检查与失败策略
+## 关于非法条件检查与失败策略
 
 默认策略：尽早失败（fail fast），避免 silent fallback。
 
@@ -68,17 +58,3 @@ Agent 不应主导大规模算法功能实现。面对“完整实现大需求
 - 仅在越界、索引失效、外部系统不稳定等场景增加必要检查
 
 目标是在错误发生时暴露真实原因，降低后期调试成本。
-
-## 6. 标准响应模板（高风险请求）
-
-当判定为高风险请求时，按以下结构回复：
-
-1. 风险判断：说明命中的高风险条件
-2. 不直接实现原因：说明可维护性/可验证性风险
-3. 推荐最小下一步：给出 1 个可执行且可验证的下一步
-
-示例：
-
-- 风险判断：当前需求缺少明确实现边界和验收标准
-- 不直接实现原因：直接大范围生成会提高回归风险，难以定位问题
-- 推荐最小下一步：先确定模块边界与接口草案，我基于该草案实现第一阶段并附带测试

config/config.yaml

@@ -1,12 +1,13 @@
 use_visualization: true
 use_painted_image: true
-is_local_runtime: true
+is_local_runtime: false
 
 capturer:
   show_loss_framerate: false
   show_loss_framerate_interval: 500
   reconnect_wait_interval: 100
-  # hikcamera or local_video
+
+  # source: "local_video"
   source: "hikcamera"
   hikcamera:
     # int
@@ -18,13 +19,13 @@ capturer:
     # float
     gain: 16.9807
 
-    invert_image: false
+    invert_image: true
     software_sync: false
     trigger_mode: false
     fixed_framerate: false
   local_video:
     # 替换为你具体的路径
-    location: "/workspaces/alliance/test_videos/outpost.mp4"
+    location: "/workspaces/data/autoaim/1777297317784.avi"
     # double 帧率
     frame_rate: 60
     # bool 是否循环播放
@@ -36,11 +37,10 @@ identifier:
   binarization_threshold: 0.5
 
   # openvino infer
-  avaliable_models:
-    - "tongji-yolov5.xml"
-    - "shenzhen-0526.onnx"
-    - "shenzhen-0708.onnx"
-  model_location: "tongji-yolov5.xml"
+  model_location: "shenzhen-0526.onnx"
+  # model_location: "shenzhen-0708.onnx"
+  # model_location: "tongji-yolov5.xml"
+
   infer_device: "AUTO"
   use_roi_segment: false
   roi_rows: 640
@@ -52,8 +52,8 @@ identifier:
   nms_threshold: 0.3
 
 tracker:
-  # blue or red
   enemy_color: red
+  # enemy_color: blue
   max_temporary_loss_frames: 4
   max_unconfirmed_loss_frames: 2
   tracking_confirm_frames: 2
@@ -74,39 +74,25 @@ pose_estimator:
   distort_coeff:
     [-0.064232403853946, -0.087667493884102, 0, 0, 0.792381808294582]
 
-  transforms:
-    - parent: "imu_link"
-      child: "pitch_link"
-      t: [0., 0., 0.]
-      q: [1., 0., 0., 0.]
-    - parent: "pitch_link"
-      child: "muzzle_link"
-      t: [0., 0., 0.]
-      q: [1., 0., 0., 0.]
-    - parent: "pitch_link"
-      child: "camera_link"
-      t: [0., 0., 0.]
-      q: [1., 0., 0., 0.]
-
 fire_control:
-  initial_bullet_speed: 26.6 # m/s
+  initial_bullet_speed: 21.0 # m/s
   shoot_delay: 0.1 # s
   yaw_offset: 0.0 # degree
   pitch_offset: 0.0 # degree
 
-  coming_angle: 70.0 # degree
-  leaving_angle: 20.0 # degree
-  outpost_coming_angle: 70.0 # degree
-  outpost_leaving_angle: 30.0 # degree
-  angular_velocity_threshold: 120 # degree/s
+  coming_angle: 55.0      # degree
+  leaving_angle: 30.0     # degree
+  outpost_coming_angle: 50.0      # degree
+  outpost_leaving_angle: 40.0     # degree
 
   first_tolerance: 3 # 近距离射击容差，degree
   second_tolerance: 2 # 远距离射击容差，degree
   judge_distance: 2 #距离判断阈值
   auto_fire: true # 是否由自瞄控制射击
 
 visualization:
-  framerate: 60
-  monitor_host: "127.0.0.1"
+  framerate: 50
+  # monitor_host: "localhost"
+  monitor_host: "192.168.3.125"
   monitor_port: "5000"
   stream_type: "RTP_JEPG"

src/adapter/sentry.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <eigen3/Eigen/Geometry>
+#include <rmcs_description/sentry_description.hpp>
+#include <rmcs_executor/component.hpp>
+
+namespace rmcs {
+
+class Adapter {
+public:
+    explicit Adapter(rmcs_executor::Component& component) { component.register_input("/tf", tf_); }
+
+    [[nodiscard]] auto ready() const -> bool { return tf_.ready(); }
+
+    [[nodiscard]] auto camera_transform() const -> Eigen::Isometry3d {
+        return fast_tf::lookup_transform<rmcs_description::OdomGimbalImu,
+            rmcs_description::CameraLink>(*tf_);
+    }
+
+    [[nodiscard]] auto barrel_direction() const -> Eigen::Vector3d {
+        return *fast_tf::cast<rmcs_description::OdomGimbalImu>(
+            rmcs_description::PitchLink::DirectionVector { Eigen::Vector3d::UnitX() }, *tf_);
+    }
+
+private:
+    rmcs_executor::Component::InputInterface<rmcs_description::SentryTf> tf_;
+};
+
+}