Skip to content

Feat/outpost-prediction#36

Open
heyeuu wants to merge 25 commits intomainfrom
feat/anti-jitter
Open

Feat/outpost-prediction#36
heyeuu wants to merge 25 commits intomainfrom
feat/anti-jitter

Conversation

@heyeuu
Copy link
Copy Markdown
Member

@heyeuu heyeuu commented Apr 14, 2026

前哨站预测能力集成 — 自动瞄准系统架构升级 PR(Feat/outpost-prediction)

一、总体目标

引入前哨站(Outpost)机器人预测与追踪能力,重构预测/快照/追踪/火控等模块以支持多类型机器人、提升解耦性与可配置性,并增加射击判定逻辑以降低误击风险。

二、核心架构变化

  • 预测后端化:新增 IRobotStateBackend 接口与工厂 make_robot_state_backend,根据设备类型选择 Regular/Outpost 实现;RobotState 改为基于后端,更新接口由单装甲 update(Armor3D) → 批量 update(std::span) -> bool。
  • Snapshot 后端化:新增 ISnapshotBackend 与工厂,Snapshot 改为通过 backend 提供 kinematics()/kinematics_at() 与 predicted_armors(t),移除直接暴露 EKF 低层 API(ekf_x、predict_at),新增 Snapshot::empty(timestamp)。
  • 新增并集成 Outpost 专用 EKF/快照/RobotState,实现 6D/4D EKF 参数集与布局管理。

三、前哨站(Outpost)实现要点

  • 新数据结构:OutpostArmorSlot / OutpostArmorLayout(相位/高度偏移与分配)。
  • OutpostEKFParameters:提供状态初始化、观测模型 h/H、状态转移 f、Q/R、雅可比 H、相位/高度相关工具函数等,支持按槽位测量模型。
  • OutpostRobotState:
    • 关联策略:基于方位/高度/马氏距离门控、多帧切换检测以推断旋转方向(spin_sign)、确认后更新布局。
    • 支持布局扩展与修正,收敛判定基于初始化、槽位分配、旋转方向锁定与最小更新次数。
  • OutpostSnapshotBackend:基于 EKF 与布局生成任意时刻运动学与预测装甲列表。

四、常规机器人(Regular)预测改动

  • 新增 RegularRobotState 与 RegularSnapshot 后端实现(PIMPL)。
  • EKF 参数调整:新增 armor_yaw、h_armor_z 等辅助函数;Q 过程噪声简化为统一实现;相关接口改为接收 DeviceId。

五、火控(FireControl)重构与新增 ShootEvaluator

  • 新增 ShootEvaluator(PIMPL)以做射击许可判断:基于命令缓存比较 yaw 与跟踪误差,按距离使用 first/second tolerance,并支持 auto_fire 开关。
  • FireControl 改动摘要:
    • 移除子弹速度缓冲/setter,直接使用 config.initial_bullet_speed(并校验 >10)。
    • solve 签名变更:solve(const Snapshot&, bool control, double current_yaw) -> std::optional;Result 新增 bool shoot_permitted。
    • 求解流程改为遍历 snapshot 提供的候选装甲(使用 snapshot.kinematics_at / snapshot 提供的预测信息),对可解轨迹应用 yaw/pitch offset,并用 ShootEvaluator 决定是否允许射击。
    • 物理模型调整:移除可配置 k/bias_scale,改用固定 kAirResistanceCoefficient(0.003)。
    • 配置项变更:移除 shoot_offset_{x,y,z} 与 k/bias_scale,新增 yaw_offset/pitch_offset(以度为配置,初始化时转换为弧度)、first_tolerance/second_tolerance/judge_distance、auto_fire 等;默认 initial_bullet_speed 从 20 提升到 26.6。

六、瞄准点选择器(AimPointChooser)

  • 去耦 EKF:choose_armor 的接口由接收 11D EKF 向量改为接收高层运动学参数(Eigen::Vector3d center_position, double angular_velocity),内部选择逻辑维持(低速/高速模式等)。

七、追踪决策(Decider)与 Tracker 集成

  • Decider 重构为基于 per-DeviceId 的 TargetMemory(含 missing/stable 计数、临时丢失标志)和帧级状态机。
  • Decider 输出变更:移除旧 State 枚举,新增字段 bool allow_takeover 与 bool tracking_confirmed,并携带 predictor::Snapshot。
  • Decider 新增 initialize(const YAML::Node&) -> std::expected<void, std::string>,用 YAML 驱动确认/临时丢失窗口配置;Tracker 初始化时会初始化 decider 并在失败时上报错误。

八、自动瞄准组件与上下文(AutoAim / Control)

  • AutoAimState / ControlState 行为调整:
    • yaw/pitch 默认值从 0 改为 NaN(表示无效),set_identity() 重命名为 reset()。
    • 新增 AutoAimState::set_hold_state / set_tracking_state / has_control_direction。
    • ControlState 移除 odom_to_muzzle_translation 字段。
  • 输出话题名变更:
    • /gimbal/auto_aim/controllable → /gimbal/auto_aim/auto_aim_enabled
    • /gimbal/auto_aim/shoot_permit → /gimbal/auto_aim/shoot_enable
  • component.cpp 重构:
    • 移除对 /referee/shooter/initial_speed 的依赖与就绪检查,update() 拆分为 handle_tf_not_ready(), publish_control_state(), forward_auto_aim_outputs() 等。
    • 引入 auto-aim 状态超时(100ms),当 Feishu 状态陈旧或缺失时发布“无效”状态(AutoAimState::reset/清空方向)。
    • 始终调用 publish_control_state() 并在失败时限流告警。

九、运行时(runtime)整合

  • 精简并修正 action-throttler 使用:保留少量标签(identifier_failed、feishu_commit_failed、control_state_not_updated)。
  • 新增 commit_state(next_state) 集中提交 Feishu 状态并根据提交结果触发限流警告或重置限流器。
  • 主处理流调整为:识别 →(若有 2D 装甲)PNP → 3D 装甲 → tracker.decide(...) → 若 allow_takeover && snapshot 有效则调用 fire_control.solve(snapshot, tracking_confirmed, control_state.yaw) → 根据结果调用 AutoAimState::set_hold_state/ set_tracking_state → commit_state(next_state)。

十、调试与可视化改进

  • ActionThrottler:由单一全局 metronome 改为 per-action metronome 与配额(修复不同标签互相影响的问题)。
  • ArmorVisualizer:移除缓存/重建复杂逻辑,改为每帧构建并发布 visualization_msgs::msg::MarkerArray(每装甲 CUBE + ARROW),并发送 DELETE marker 清理失效 id。

十一、工具库与常量

  • 新增 mahalanobis_distance(TVec, TMat) 工具(utility/math/mahalanobis.hpp)。
  • EKF 暴露了 P() 访问协方差矩阵(utility/math/kalman_filter/ekf.hpp)。
  • 常量调整(utility/robot/constant.hpp):kOutpostRadius 微调 0.2765 → 0.275,并新增 kPredictedOutpostArmorPitch、kPredictedOtherArmorPitch、kOutpostArmorHeightStep、kOutpostAngularSpeed。

十二、配置(config/config.yaml)主要变更

  • 摄像头:framerate → 120,fixed_framerate → false,本地回放文件 location → outpost.mp4。
  • 识别器:model_location → tongji-yolov5.xml,infer_device → GPU,score_threshold 0.8→0.7,nms_threshold 0.45→0.3。
  • 追踪器:enemy_color blue→red,新增 max_temporary_loss_frames、max_unconfirmed_loss_frames、tracking_confirm_frames 等多帧确认/临时丢失参数。
  • 火控:initial_bullet_speed 20→26.6,移除 shoot_offset_* 与 k/bias_scale,新增 yaw_offset/pitch_offset、first_tolerance/second_tolerance/judge_distance、auto_fire=true,coming_angle 60→70。

十三、测试

  • 新增/修改测试:
    • test/action_throttler.cpp:新增 DifferentTagsDoNotShareInterval(验证 per-tag 独立节拍器)。
    • test/model_infer.cpp:引入 make_detector 工厂,新增负例测试(空图像、非法 ROI、未知模型)。
    • test/feishu_test.cpp:移除对 bullet_speed 的设置(对应接口变更)。
    • package.xml 与 CMakeLists.txt:添加 ament_cmake_gtest 测试依赖与在 BUILD_TESTING=true 下包含 test 子目录。

十四、公共 API 与兼容性影响(重要变更)

  • 移除或变更的公有接口(需注意兼容性):
    • RobotState::match(...) 与 RobotState::update(Armor3D const&) 被移除,替换为 update(std::span) -> bool。
    • Snapshot 低层 EKF 访问(ekf_x、predict_at)被移除,改为 kinematics()/kinematics_at()。
    • ControlState::odom_to_muzzle_translation 被移除。
    • tracker::State 枚举及相关头文件被移除;Decider 输出用布尔标志替代。
    • AutoAimState::set_identity() → reset()(语义改为清空/无效方向由 NaN 表示)。
    • FireControl::set_bullet_speed() 被移除; FireControl::solve 签名变更(增加 control 与 current_yaw 参数,Result 增加 shoot_permitted)。
    • AimPointChooser::choose_armor 签名改为接收 center_position 与 angular_velocity。
  • 新引入/更频繁使用的头文件/语言特性:, , 等。

十五、审查重点与风险评估

  • 高风险区域(需重点 review):
    • Outpost EKF 的测量模型 h/H、雅可比与 R 的数值稳定性与正确性(大量新数学实现)。
    • OutpostRobotState 的关联与 spin_sign 推断(多帧切换逻辑、布局修正、收敛判定)。
    • FireControl 的轨迹求解(固定 air resistance)与 ShootEvaluator 的容差策略对射击安全性的影响(误触发/漏触发风险)。
    • Snapshot/RobotState 后端化对上游模块(Decider、component、visualization 等)的兼容性与行为等价性。
    • AutoAimState 与 ControlState 的 yaw/pitch 从 0→NaN 的语义变更,需确认所有上游使用点已做有效性检查(has_control_direction()/isfinite)。
  • 中低风险项:
    • ActionThrottler、ArmorVisualizer 的实现调整、配置变更与测试更新(相对风险较低但仍需验证)。

十六、变更量与审查建议

  • 变更范围大:新增多处 predictor/outpost 与 predictor/regular 实现文件(大量新增代码)、fire_control、trajectory_solution、aim_point_chooser、decider、component、runtime、utility/context.hpp 等关键模块均有改动。
  • 建议审查流程:
    1. 数学/滤波/观测模型(OutpostEKFParameters、H/R/Q、TrajectorySolution)单独专家复审并覆盖单元测试。
    2. OutpostRobotState 的关联与布局更新逻辑走案例驱动测试(不同旋转方向、槽位缺失、噪声场景)。
    3. FireControl + ShootEvaluator 的集成测试(不同距离/角误差下的射击许可行为)。
    4. Snapshot/RobotState 后端化接口兼容性检查(Decider、component、visualization)。
    5. 全流程集成测试(识别→PNP→追踪→火控→提交状态)在常见配置与 outpost 回放视频上验证。

heyeuu added 21 commits March 22, 2026 16:21
…ings

- Refactor armor_detection logic for better performance.
- Increase camera frame rate to 120 FPS in configuration.
- Add logic to handle single-frame false positives (misidentification).
- Remove redundant aerodynamic coefficients to simplify the model.
- Add `yaw_offset` and `pitch_offset` for improved alignment control.
@heyeuu heyeuu self-assigned this Apr 14, 2026
@heyeuu heyeuu added the enhancement New feature or request label Apr 14, 2026
@coderabbitai
Copy link
Copy Markdown
Contributor

coderabbitai bot commented Apr 14, 2026

Warning

Rate limit exceeded

@heyeuu has exceeded the limit for the number of commits that can be reviewed per hour. Please wait 33 minutes and 51 seconds before requesting another review.

Your organization is not enrolled in usage-based pricing. Contact your admin to enable usage-based pricing to continue reviews beyond the rate limit, or try again in 33 minutes and 51 seconds.

⌛ How to resolve this issue?

After the wait time has elapsed, a review can be triggered using the @coderabbitai review command as a PR comment. Alternatively, push new commits to this PR.

We recommend that you space out your commits to avoid hitting the rate limit.

🚦 How do rate limits work?

CodeRabbit enforces hourly rate limits for each developer per organization.

Our paid plans have higher rate limits than the trial, open-source and free plans. In all cases, we re-allow further reviews after a brief timeout.

Please see our FAQ for further information.

ℹ️ Review info
⚙️ Run configuration

Configuration used: Organization UI

Review profile: CHILL

Plan: Pro

Run ID: d580c8b9-dfd3-4350-a2f6-0aa3d274d2d6

📥 Commits

Reviewing files that changed from the base of the PR and between 079a7e4 and 53bbf5d.

📒 Files selected for processing (1)
  • src/module/predictor/regular/robot_state.cpp

Walkthrough

本次提交将预测器/快照与 RobotState 抽象化为后端接口,新增常规/前哨后端实现;重构追踪器/Decider、FireControl 与射击评估器,调整组件间数据流及运行时提交逻辑,并同步更新配置、测试和若干工具接口(NaN 初始值、EKF P() 访问器、mahalanobis 等)。

Changes

Cohort / File(s) Summary
预测器后端抽象
src/module/predictor/snapshot.cpp, src/module/predictor/snapshot.hpp, src/module/predictor/robot_state.cpp, src/module/predictor/robot_state.hpp, src/module/predictor/backend/robot_state_backend.cpp, src/module/predictor/backend/robot_state_backend.hpp, src/module/predictor/backend/snapshot_backend.hpp
引入 ISnapshotBackend/IRobotStateBackend 抽象并改造 Snapshot/RobotState 为后端驱动,新增工厂与分类函数,API 从直接暴露 EKF 状态改为 backend 委托。
前哨后端实现
src/module/predictor/outpost/*
.../armor_layout.hpp, .../ekf_parameter.hpp, .../robot_state.hpp, .../robot_state.cpp, .../snapshot.hpp, .../snapshot.cpp
新增 Outpost 专用 EKF 参数、装甲布局与 OutpostRobotState/OutpostSnapshot 后端,实现相位/高度偏移、关联、旋转方向跟踪与布局扩展逻辑。
常规后端实现
src/module/predictor/regular/*
.../ekf_parameter.hpp, .../robot_state.hpp, .../robot_state.cpp, .../snapshot.hpp, .../snapshot.cpp
新增 Regular 后端并迁移原 EKF 关联/预测逻辑到该后端,包含初始化、预测、融合与收敛判定等实现。
FireControl 与轨迹
src/kernel/fire_control.cpp, src/kernel/fire_control.hpp, src/module/fire_control/trajectory_solution.cpp, src/module/fire_control/trajectory_solution.hpp
移除可变子弹速度与旧阻力/偏移参数,改用 config.initial_bullet_speed、yaw/pitch 偏移与固定 air_resistance;solve 签名变更并返回包含 shoot_permitted 的 Result。
ShootEvaluator
src/module/fire_control/shoot_evaluator.hpp, src/module/fire_control/shoot_evaluator.cpp
新增 ShootEvaluator(配置驱动)和命令缓存,用于基于容差/距离/历史命令决定是否允许开火。
AimPointChooser API
src/module/fire_control/aim_point_chooser.hpp, src/module/fire_control/aim_point_chooser.cpp
移除对 11D EKF 向量的依赖,API 改为接收 center_positionangular_velocity
Tracker / Decider 重构
src/module/tracker/decider.hpp, src/module/tracker/decider.cpp, src/kernel/tracker.hpp, src/kernel/tracker.cpp, src/module/tracker/state.hpp
引入可配置 TargetMemory、frame-based 状态机、allow_takeover/tracking_confirmed 输出与 YAML 初始化,移除旧的 State 枚举并改进清理/超时与仲裁逻辑。
动作节流器 & 测试
src/module/debug/action_throttler.hpp, test/action_throttler.cpp, test/*
将全局 metronome 拆为每标签计时/配额状态;新增测试验证不同标签不共享间隔;若干测试适配改动。
组件与运行时集成
src/component.cpp, src/runtime.cpp, src/utility/shared/context.hpp
重命名/行为变更:AutoAim/ControlState 使用 NaN 初值与 reset/set_hold/set_tracking/has_control_direction;Component 输出主题重命名;runtime 重构提交流程,使用 commit_state 聚合并调整 tracker/fire_control/visualization 调用时序。
装甲可视化
src/module/debug/visualization/armor_visualizer.cpp
移除缓存重建逻辑,每次构建并发布 visualization_msgs::msg::MarkerArray,管理新增与删除 id。
实用工具与数学
src/utility/math/kalman_filter/ekf.hpp, src/utility/math/mahalanobis.hpp, src/utility/model/armor_detection.hpp, src/utility/robot/constant.hpp
为 EKF 添加 P() 访问器;新增 mahalanobis_distance 工具;在 InferResultAdapter 中增加 std::is_standard_layout 断言;新增并调整若干与前哨相关常量。
配置与构建 / 测试 清单
CMakeLists.txt, package.xml, config/config.yaml, test/CMakeLists.txt, test/*
CMake 条件加入 test 子目录,package.xml 增加 ament_cmake_gtest 测试依赖,更新 config(相机帧率、模型位置、推断/跟踪/火控参数与阈值),若干测试新增或调整。

Sequence Diagram(s)

sequenceDiagram
    participant Runtime
    participant Tracker as Decider/Tracker
    participant Predictor as Snapshot/RobotState Backend
    participant FireControl
    participant Feishu

    Runtime->>Tracker: submit detected armors, timestamp
    Tracker->>Predictor: ensure/update backend (initialize/predict/update)
    Predictor-->>Tracker: Snapshot (kinematics, predicted_armors)
    Tracker-->>Runtime: decide (allow_takeover, target_id, tracking_confirmed, snapshot)
    Runtime->>FireControl: solve(snapshot, control=tracking_confirmed, current_yaw)
    FireControl-->>Runtime: Result (yaw,pitch,shoot_permitted)
    Runtime->>Feishu: commit_state(next_state)
    Feishu-->>Runtime: commit result
Loading

Estimated code review effort

🎯 4 (Complex) | ⏱️ ~65 分钟

Possibly related PRs

Suggested reviewers

  • creeper5820

🐰 我在代码田里挖洞忙,
后端重搭把预测安放,
瞄准由角度代替旧量纲,
判火存档细读每一方,
重构落地,胡萝卜请大家尝!

🚥 Pre-merge checks | ✅ 1 | ❌ 2

❌ Failed checks (1 warning, 1 inconclusive)

Check name Status Explanation Resolution
Docstring Coverage ⚠️ Warning Docstring coverage is 5.02% which is insufficient. The required threshold is 80.00%. Write docstrings for the functions missing them to satisfy the coverage threshold.
Title check ❓ Inconclusive PR标题"Feat/outpost-prediction"只指代"outpost prediction"功能,但本PR包含大量其他重要改动,如重构组件、火力控制、追踪系统、预测器后端等,标题无法全面反映实际变更范围。 建议更新标题以涵盖主要改动范围,例如"Refactor prediction backend and add outpost support"或更具体地描述核心变更内容。
✅ Passed checks (1 passed)
Check name Status Explanation
Description Check ✅ Passed Check skipped - CodeRabbit’s high-level summary is enabled.

✏️ Tip: You can configure your own custom pre-merge checks in the settings.

✨ Finishing Touches
🧪 Generate unit tests (beta)
  • Create PR with unit tests
  • Commit unit tests in branch feat/anti-jitter

Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out.

❤️ Share

Comment @coderabbitai help to get the list of available commands and usage tips.

Copy link
Copy Markdown
Contributor

@coderabbitai coderabbitai bot left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actionable comments posted: 8

🧹 Nitpick comments (5)
src/utility/math/mahalanobis.hpp (1)

11-16: 建议明确这是“平方马氏距离”而非“马氏距离”。

Line 14 计算的是二次型 。当前函数名容易误导阈值使用,建议重命名为 mahalanobis_distance_sq(或改为返回 sqrt(d²) 并同步调用方阈值)。

♻️ 建议修改示例(保留 d² 语义)
 template <class TVec, class TMat>
-inline auto mahalanobis_distance(TVec const& innovation, TMat const& covariance)
+inline auto mahalanobis_distance_sq(TVec const& innovation, TMat const& covariance)
     -> std::optional<double> {
     auto solved   = covariance.ldlt().solve(innovation);
-    auto distance = innovation.dot(solved);
-    if (!std::isfinite(distance)) return std::nullopt;
-    return distance;
+    auto distance_sq = innovation.dot(solved);
+    if (!std::isfinite(distance_sq) || distance_sq < 0.0) return std::nullopt;
+    return distance_sq;
 }
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@src/utility/math/mahalanobis.hpp` around lines 11 - 16, The function
mahalanobis_distance computes the quadratic form (d²) but is named as if it
returned the actual Mahalanobis distance; rename the function to
mahalanobis_distance_sq to reflect it returns the squared distance (update the
function declaration/definition and all call sites that use
mahalanobis_distance), or alternatively change the implementation to return
std::sqrt(distance) and update threshold logic; ensure references to the
function, any comments/docs, and threshold comparisons (e.g., uses of the return
value) are updated accordingly; the change affects the function that computes
solved = covariance.ldlt().solve(innovation) and distance =
innovation.dot(solved).
src/kernel/fire_control.hpp (1)

18-23: shoot_permitted 补默认值。

这是这次新增的公共返回字段;如果后面出现 Result result; 这种默认构造路径,它会保持未定义状态。直接在声明处初始化为 false 会更稳妥。

🔧 建议修改
     struct Result {
         double pitch;
         double yaw;
         double horizon_distance;
-        bool shoot_permitted;
+        bool shoot_permitted { false };
     };
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@src/kernel/fire_control.hpp` around lines 18 - 23, Result 结构体中的成员
shoot_permitted 当前未初始化,会在默认构造(例如 `Result result;`)时成为未定义值;在 `struct Result`
的声明处将 `shoot_permitted` 直接初始化为 `false`(例如 `bool shoot_permitted =
false;`)以确保默认构造时为确定性值,定位标识符:Result、shoot_permitted。
src/module/debug/action_throttler.hpp (1)

67-70: 避免直接改 FramerateCounter 的内部字段。

reset() 这里直接清 last_reach_interval_timestamp,等于把 ActionThrottler 绑定到了 FramerateCounter 的内部实现细节上。后面如果 FramerateCounter 再增加状态,这里的重置很容易变得不完整。更稳妥的做法是给 FramerateCounter 提供显式 reset(),或者在这里重建一个新的计数器并重新设置 interval。

🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@src/module/debug/action_throttler.hpp` around lines 67 - 70, The reset()
implementation mutates FramerateCounter internals
(metronome.last_reach_interval_timestamp) making ActionThrottler dependent on
FramerateCounter internals; instead add and call an explicit
FramerateCounter::reset() (or recreate the metronome instance) from
ActionThrottler::reset() so the counter's full state is properly reset; update
reset() to call limit.reset() and then either metronome.reset() (preferred) or
replace metronome with a newly-constructed FramerateCounter configured with the
existing interval, and remove direct writes to last_reach_interval_timestamp.
src/module/fire_control/shoot_evaluator.cpp (1)

32-33: 建议使用 deg2rad 工具函数替代魔法数字 57.3。

代码中使用 57.3 作为 180/π 的近似值进行角度转换,但该近似值精度有限(实际值约为 57.2957795...)。项目中已经有 utility/math/angle.hpp 提供的 deg2rad 函数,建议统一使用。

♻️ 建议的修改
+#include "utility/math/angle.hpp"  // 如果尚未包含
+
 // 在 Impl 中
-    double first_tolerance_ { 4.0 / 57.3 };
-    double second_tolerance_ { 2.0 / 57.3 };
+    double first_tolerance_ { util::deg2rad(4.0) };
+    double second_tolerance_ { util::deg2rad(2.0) };

同样适用于 initialize() 中的第 45-46 行:

-        first_tolerance_  = config.first_tolerance / 57.3;
-        second_tolerance_ = config.second_tolerance / 57.3;
+        first_tolerance_  = util::deg2rad(config.first_tolerance);
+        second_tolerance_ = util::deg2rad(config.second_tolerance);
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@src/module/fire_control/shoot_evaluator.cpp` around lines 32 - 33, Replace
the hardcoded angle conversion magic number 57.3 by using the project's deg2rad
helper: update the member initializers first_tolerance_ and second_tolerance_ to
use deg2rad(4.0) and deg2rad(2.0) respectively, and similarly replace the
conversions in initialize() (the two occurrences noted around lines 45-46) to
call deg2rad(...) from utility/math/angle.hpp; ensure the header is included if
not already and use the deg2rad symbol to improve precision and consistency.
src/module/predictor/robot_state.cpp (1)

45-49: ensure_backend 后的空检查可能冗余

ensure_backend(armors.front()) 调用后,如果 make_backend 总是返回有效指针,则第 48 行的 backend ? 检查是多余的。如果 make_backend 可能返回 nullptr,建议在 ensure_backend 中添加断言或处理此情况。

♻️ 建议简化
     auto update(std::span<Armor3D const> armors) -> bool {
         if (armors.empty()) return false;
         ensure_backend(armors.front());
-        return backend ? backend->update(armors) : false;
+        return backend->update(armors);
     }

或者在 ensure_backend 中添加后置条件检查:

     auto ensure_backend(Armor3D const& armor) -> void {
         if (backend) return;
         backend = make_backend(armor.genre, pending_time_stamp);
+        // assert(backend && "make_backend should never return nullptr");
     }
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@src/module/predictor/robot_state.cpp` around lines 45 - 49, The null-check
after ensure_backend in update is likely redundant; either guarantee backend is
non-null by modifying ensure_backend (or make_backend) to never return nullptr
and add a postcondition/assertion there (e.g., assert(backend) or throw on
failure), or keep defensive behavior by having ensure_backend return a bool and
early-return on failure; update the code paths in update (the update method,
ensure_backend function, and make_backend) accordingly so you only have one
clear responsibility for handling a missing backend and remove the redundant
"backend ?" conditional if you enforce a non-null postcondition.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.

Inline comments:
In `@src/module/debug/visualization/armor_visualizer.cpp`:
- Around line 112-145: The Marker timestamps are created with a static
rclcpp_clock (RCL_STEADY_TIME), causing time-source mismatch with RViz; replace
usage of rclcpp_clock and current_time with the node's clock (e.g., call
node->get_clock()->now() where current_time is set) or set the stamp to zero
when constructing markers inside make_marker so header.stamp uses the ROS time
domain; update all places using rclcpp_clock (the static rclcpp_clock variable
and the current_time assignment) and ensure make_marker receives the
node-derived time (or a zero time) before publishing via rclcpp_pub->publish so
RViz/TF compare times from the same clock.
- Around line 117-139: The markers currently use only armor.id which causes ID
collisions across devices; change all places that build marker IDs (calls to
make_marker and the current_ids/previous_ids keys) to combine armor.genre
(device ID) and armor.id into a single stable unique ID (e.g.,
(static_cast<uint32_t>(armor.genre) << 8) | armor.id or by appending device ID
into the marker namespace), and apply the same composition for both ADD and
DELETE markers and when inserting/looking up IDs in current_ids and previous_ids
so add/delete logic remains consistent.

In `@src/module/fire_control/aim_point_chooser.cpp`:
- Around line 29-37: The file uses std::atan2 and multiple std::abs calls inside
choose_armor (and surrounding functions) but does not explicitly include
<cmath>; add an explicit `#include` <cmath> at the top of
src/module/fire_control/aim_point_chooser.cpp so that functions like std::atan2
and std::abs are guaranteed to be available regardless of transitive includes
and toolchain/include-order changes.

In `@src/module/predictor/backend/robot_state_backend.hpp`:
- Line 44: 文件中打开的命名空间是 `namespace rmcs::predictor`(在顶部),但结尾注释写成了 `// namespace
rmcs::predictor::detail`; 将结尾的注释改为与实际声明一致为 `// namespace
rmcs::predictor`,或者如果意图是关闭 `rmcs::predictor::detail`,则修改开头的 `namespace` 声明以包含
`::detail`(检查并统一 `namespace rmcs::predictor` 与结尾注释中的命名空间名以保持一致)。

In `@src/module/predictor/outpost/robot_state.cpp`:
- Around line 424-428: The implementation defines both OutpostRobotState()
noexcept and OutpostRobotState(Clock::time_point stamp) noexcept but the header
only declares the parameterized ctor; update the class declaration in
robot_state.hpp to also declare the default constructor OutpostRobotState()
noexcept (matching the implementation) so the declarations and definitions
align, keeping the explicit/ noexcept qualifiers consistent with the existing
OutpostRobotState(Clock::time_point) declaration and retaining the pimpl/Impl
usage.

In `@src/module/predictor/regular/ekf_parameter.hpp`:
- Around line 17-18: This header uses std::numbers::pi in functions armor_yaw
and H but does not include the <numbers> header; add a direct `#include` <numbers>
at the top of the file so the header compiles standalone and does not rely on
transitive includes, ensuring references to std::numbers::pi in armor_yaw and H
resolve.

In `@src/module/predictor/regular/robot_state.cpp`:
- Around line 69-70: The convergence check uses min_updates = 3 but compares
with update_count using '>' so it only returns true after the 4th update; change
the comparison in the return expression that references r_ok, l_ok, update_count
and min_updates from 'update_count > min_updates' to 'update_count >=
min_updates' so that "at least 3 updates" is treated correctly.
- Around line 73-79: get_snapshot() and distance() currently read ekf.x even
when the object has been marked uninitialized after timeout; change both to
first check the object's uninitialized flag (the same flag set by reset) and if
not initialized return a safe empty value: for get_snapshot() return
Snapshot::empty(...) using the same device/color/armor_num/time_stamp context,
and for distance() return a sentinel (e.g. NaN via
std::numeric_limits<double>::quiet_NaN()) without accessing ekf.x; ensure no
direct reads of ekf.x occur when the uninitialized flag is true.

---

Nitpick comments:
In `@src/kernel/fire_control.hpp`:
- Around line 18-23: Result 结构体中的成员 shoot_permitted 当前未初始化,会在默认构造(例如 `Result
result;`)时成为未定义值;在 `struct Result` 的声明处将 `shoot_permitted` 直接初始化为 `false`(例如
`bool shoot_permitted = false;`)以确保默认构造时为确定性值,定位标识符:Result、shoot_permitted。

In `@src/module/debug/action_throttler.hpp`:
- Around line 67-70: The reset() implementation mutates FramerateCounter
internals (metronome.last_reach_interval_timestamp) making ActionThrottler
dependent on FramerateCounter internals; instead add and call an explicit
FramerateCounter::reset() (or recreate the metronome instance) from
ActionThrottler::reset() so the counter's full state is properly reset; update
reset() to call limit.reset() and then either metronome.reset() (preferred) or
replace metronome with a newly-constructed FramerateCounter configured with the
existing interval, and remove direct writes to last_reach_interval_timestamp.

In `@src/module/fire_control/shoot_evaluator.cpp`:
- Around line 32-33: Replace the hardcoded angle conversion magic number 57.3 by
using the project's deg2rad helper: update the member initializers
first_tolerance_ and second_tolerance_ to use deg2rad(4.0) and deg2rad(2.0)
respectively, and similarly replace the conversions in initialize() (the two
occurrences noted around lines 45-46) to call deg2rad(...) from
utility/math/angle.hpp; ensure the header is included if not already and use the
deg2rad symbol to improve precision and consistency.

In `@src/module/predictor/robot_state.cpp`:
- Around line 45-49: The null-check after ensure_backend in update is likely
redundant; either guarantee backend is non-null by modifying ensure_backend (or
make_backend) to never return nullptr and add a postcondition/assertion there
(e.g., assert(backend) or throw on failure), or keep defensive behavior by
having ensure_backend return a bool and early-return on failure; update the code
paths in update (the update method, ensure_backend function, and make_backend)
accordingly so you only have one clear responsibility for handling a missing
backend and remove the redundant "backend ?" conditional if you enforce a
non-null postcondition.

In `@src/utility/math/mahalanobis.hpp`:
- Around line 11-16: The function mahalanobis_distance computes the quadratic
form (d²) but is named as if it returned the actual Mahalanobis distance; rename
the function to mahalanobis_distance_sq to reflect it returns the squared
distance (update the function declaration/definition and all call sites that use
mahalanobis_distance), or alternatively change the implementation to return
std::sqrt(distance) and update threshold logic; ensure references to the
function, any comments/docs, and threshold comparisons (e.g., uses of the return
value) are updated accordingly; the change affects the function that computes
solved = covariance.ldlt().solve(innovation) and distance =
innovation.dot(solved).
🪄 Autofix (Beta)

Fix all unresolved CodeRabbit comments on this PR:

  • Push a commit to this branch (recommended)
  • Create a new PR with the fixes

ℹ️ Review info
⚙️ Run configuration

Configuration used: Organization UI

Review profile: CHILL

Plan: Pro

Run ID: 2cfd39eb-c499-4368-b17b-235842827dc0

📥 Commits

Reviewing files that changed from the base of the PR and between 2e54dc6 and 49822e9.

📒 Files selected for processing (47)
  • CMakeLists.txt
  • config/config.yaml
  • package.xml
  • src/component.cpp
  • src/kernel/fire_control.cpp
  • src/kernel/fire_control.hpp
  • src/kernel/tracker.cpp
  • src/kernel/tracker.hpp
  • src/module/debug/action_throttler.hpp
  • src/module/debug/visualization/armor_visualizer.cpp
  • src/module/fire_control/aim_point_chooser.cpp
  • src/module/fire_control/aim_point_chooser.hpp
  • src/module/fire_control/shoot_evaluator.cpp
  • src/module/fire_control/shoot_evaluator.hpp
  • src/module/fire_control/trajectory_solution.cpp
  • src/module/fire_control/trajectory_solution.hpp
  • src/module/predictor/backend/robot_state_backend.cpp
  • src/module/predictor/backend/robot_state_backend.hpp
  • src/module/predictor/backend/snapshot_backend.hpp
  • src/module/predictor/outpost/armor_layout.hpp
  • src/module/predictor/outpost/ekf_parameter.hpp
  • src/module/predictor/outpost/robot_state.cpp
  • src/module/predictor/outpost/robot_state.hpp
  • src/module/predictor/outpost/snapshot.cpp
  • src/module/predictor/outpost/snapshot.hpp
  • src/module/predictor/regular/ekf_parameter.hpp
  • src/module/predictor/regular/robot_state.cpp
  • src/module/predictor/regular/robot_state.hpp
  • src/module/predictor/regular/snapshot.cpp
  • src/module/predictor/regular/snapshot.hpp
  • src/module/predictor/robot_state.cpp
  • src/module/predictor/robot_state.hpp
  • src/module/predictor/snapshot.cpp
  • src/module/predictor/snapshot.hpp
  • src/module/tracker/decider.cpp
  • src/module/tracker/decider.hpp
  • src/module/tracker/state.hpp
  • src/runtime.cpp
  • src/utility/math/kalman_filter/ekf.hpp
  • src/utility/math/mahalanobis.hpp
  • src/utility/model/armor_detection.hpp
  • src/utility/robot/constant.hpp
  • src/utility/shared/context.hpp
  • test/CMakeLists.txt
  • test/action_throttler.cpp
  • test/feishu_test.cpp
  • test/model_infer.cpp
💤 Files with no reviewable changes (1)
  • src/module/tracker/state.hpp

Copy link
Copy Markdown
Contributor

@coderabbitai coderabbitai bot left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actionable comments posted: 2

Caution

Some comments are outside the diff and can’t be posted inline due to platform limitations.

⚠️ Outside diff range comments (1)
src/module/tracker/decider.cpp (1)

263-285: ⚠️ Potential issue | 🟡 Minor

移除未使用的优先级模式定义。

mode1mode2 在此文件中定义但从未使用。请移除这些未使用的常量定义。

🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@src/module/tracker/decider.cpp` around lines 263 - 285, Remove the unused
PriorityMode constants mode1 and mode2 from decider.cpp: these locals ({
DeviceId::HERO, ... } and { DeviceId::ENGINEER, ... } definitions) are defined
but never referenced, so delete the declarations of mode1 and mode2 to avoid
unused symbol warnings and dead code; search for the identifiers mode1 and mode2
and remove their full const PriorityMode definitions (including the DeviceId
entries) and ensure no other code depends on them.
🧹 Nitpick comments (1)
src/module/predictor/regular/ekf_parameter.hpp (1)

18-25: 建议把装甲板数量的判定统一成一个来源。

现在同一套几何分支一部分走 armor_num(device)armor_yaw() / h_armor_z()),另一部分走显式 armor_numh_armor_xyz() / H())。当前调用方在 src/module/predictor/regular/robot_state.cppsrc/module/predictor/regular/snapshot.cpp 里是对齐的,但这会把 h()H() 的一致性变成隐式约束,后续只改一边时很容易把观测函数和 Jacobian 改散。建议全链路只保留一个来源:要么全部从 device 推导,要么统一显式传 armor_num

Also applies to: 146-158, 202-210

🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@src/module/predictor/regular/ekf_parameter.hpp` around lines 18 - 25, The
code mixes two sources for the armor plate count (calling armor_num(device)
inside armor_yaw() / h_armor_z() but passing an explicit armor_num into
h_armor_xyz() / H()), which risks divergence; pick one source and make it
consistent across the chain: either (A) change h_armor_xyz(...) and H(...) to
accept DeviceId (or otherwise call armor_num(device) internally) so all geometry
functions derive armor count from device (update all call sites in
robot_state.cpp and snapshot.cpp accordingly), or (B) change
armor_yaw(DeviceId,const EKF::XVec,int) and h_armor_z(DeviceId,const
EKF::XVec,int) to accept an explicit armor_num parameter and use that instead of
calling armor_num(device) (and update callers to pass the same explicit
armor_num used for h_armor_xyz/H). Ensure function signatures and all callers
for armor_yaw, h_armor_z, h_armor_xyz, and H are updated together so there's a
single source of truth for armor count.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.

Inline comments:
In `@src/module/predictor/backend/robot_state_backend.hpp`:
- Around line 3-5: This header (robot_state_backend.hpp) uses std::uint8_t in
its public API but does not include <cstdint>, so add an explicit `#include`
<cstdint> at the top of the file to ensure std::uint8_t is defined and the
header is self-contained (don’t rely on transitive includes).

In `@src/module/predictor/regular/robot_state.cpp`:
- Around line 51-55: 当前实现会在 update_single(...) 内对 update_count 增量,导致批量调用
update(std::span<Armor3D const> armors) 时每融合一块装甲都增加一次 update_count,从而破坏
is_converged() 关于“至少 3 次时序更新”的语义;请把对 update_count 的自增逻辑从 update_single(...)
中移除,并在 update(std::span<Armor3D const> armors) 中在循环结束后(当 fused 为
true)只做一次自增,这样每次成功的批量更新只增加一次 update_count,保持 is_converged() 的预期行为。确保删除或注释掉
update_single 中的增量并在 update 中在确认 fused 后执行 update_count++(或等价赋值)。

---

Outside diff comments:
In `@src/module/tracker/decider.cpp`:
- Around line 263-285: Remove the unused PriorityMode constants mode1 and mode2
from decider.cpp: these locals ({ DeviceId::HERO, ... } and {
DeviceId::ENGINEER, ... } definitions) are defined but never referenced, so
delete the declarations of mode1 and mode2 to avoid unused symbol warnings and
dead code; search for the identifiers mode1 and mode2 and remove their full
const PriorityMode definitions (including the DeviceId entries) and ensure no
other code depends on them.

---

Nitpick comments:
In `@src/module/predictor/regular/ekf_parameter.hpp`:
- Around line 18-25: The code mixes two sources for the armor plate count
(calling armor_num(device) inside armor_yaw() / h_armor_z() but passing an
explicit armor_num into h_armor_xyz() / H()), which risks divergence; pick one
source and make it consistent across the chain: either (A) change
h_armor_xyz(...) and H(...) to accept DeviceId (or otherwise call
armor_num(device) internally) so all geometry functions derive armor count from
device (update all call sites in robot_state.cpp and snapshot.cpp accordingly),
or (B) change armor_yaw(DeviceId,const EKF::XVec,int) and
h_armor_z(DeviceId,const EKF::XVec,int) to accept an explicit armor_num
parameter and use that instead of calling armor_num(device) (and update callers
to pass the same explicit armor_num used for h_armor_xyz/H). Ensure function
signatures and all callers for armor_yaw, h_armor_z, h_armor_xyz, and H are
updated together so there's a single source of truth for armor count.
🪄 Autofix (Beta)

Fix all unresolved CodeRabbit comments on this PR:

  • Push a commit to this branch (recommended)
  • Create a new PR with the fixes

ℹ️ Review info
⚙️ Run configuration

Configuration used: Organization UI

Review profile: CHILL

Plan: Pro

Run ID: 95f390de-bc20-4ebb-99c9-b5baa93d7b64

📥 Commits

Reviewing files that changed from the base of the PR and between 49822e9 and b08a6c6.

📒 Files selected for processing (6)
  • src/module/debug/visualization/armor_visualizer.cpp
  • src/module/fire_control/aim_point_chooser.cpp
  • src/module/predictor/backend/robot_state_backend.hpp
  • src/module/predictor/regular/ekf_parameter.hpp
  • src/module/predictor/regular/robot_state.cpp
  • src/module/tracker/decider.cpp
🚧 Files skipped from review as they are similar to previous changes (2)
  • src/module/fire_control/aim_point_chooser.cpp
  • src/module/debug/visualization/armor_visualizer.cpp

Copy link
Copy Markdown
Contributor

@coderabbitai coderabbitai bot left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actionable comments posted: 2

🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.

Inline comments:
In `@src/module/predictor/backend/robot_state_backend.hpp`:
- Around line 42-43: The factory make_robot_state_backend currently only takes
RobotStateBackendKind and a time_point but must also accept and forward the
DeviceId so backends can hold the real device identifier; update the signature
of make_robot_state_backend to include a DeviceId parameter and propagate that
DeviceId into the concrete backend constructors, ensure IRobotStateBackend
implementations store the DeviceId and pass it down to RegularRobotState (so
RegularRobotState::Impl::initialize can call
EKFParameters::P_initial_dig(device), h(device, ...) and H(device, ...)) while
keeping armor-based logic limited to Armor3D/ArmorGenre for armor_num
derivation; search for make_robot_state_backend, RobotStateBackendKind,
IRobotStateBackend::Clock::time_point, DeviceId,
RegularRobotState::Impl::initialize, EKFParameters::P_initial_dig, h and H to
apply the change and update callers accordingly.

In `@src/module/predictor/regular/robot_state.cpp`:
- Around line 3-5: This file uses std::numeric_limits<double>::infinity() (see
usage in robot_state.cpp) but doesn't include <limits>; add a direct include for
<limits> at the top of robot_state.cpp so the code doesn't rely on transitive
includes (i.e., insert `#include` <limits> alongside the existing includes to
ensure compilation is robust).
🪄 Autofix (Beta)

Fix all unresolved CodeRabbit comments on this PR:

  • Push a commit to this branch (recommended)
  • Create a new PR with the fixes

ℹ️ Review info
⚙️ Run configuration

Configuration used: Organization UI

Review profile: CHILL

Plan: Pro

Run ID: b04cf086-36d5-4681-a2d0-ebdb95d44851

📥 Commits

Reviewing files that changed from the base of the PR and between b08a6c6 and 079a7e4.

📒 Files selected for processing (2)
  • src/module/predictor/backend/robot_state_backend.hpp
  • src/module/predictor/regular/robot_state.cpp

heyeuu and others added 2 commits April 14, 2026 22:59
Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com>
@heyeuu heyeuu requested a review from creeper5820 April 14, 2026 15:06
@heyeuu heyeuu moved this from Todo to In progress in RMCS Auto Aim V2 Apr 14, 2026
@heyeuu heyeuu added this to the 预测及解算模块 milestone Apr 14, 2026
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

enhancement New feature or request

Projects

Status: In progress

Development

Successfully merging this pull request may close these issues.

1 participant