deformable infantry v2#70
Conversation
…st angle parameters and rename chassis/joint board classes
…Ds and zero points for left and right boards, and update class references in the hardware implementation.
…on and modify dji_motor to include encoder_angle output
…on and modify dji_motor to include encoder_angle output
…ers, improve logging, and enhance joint calibration handling
… IDs, and logging for improved functionality
…add alpha-beta angle filter for improved velocity estimation
…meters, update zero points, and improve command update logic for better control and performance
…ance command update logic for better performance
…oved accuracy and modify velocity limits in chassis controller for enhanced control
- Renamed SimpleGimbalController to DeformableInfantryGimbalController and updated related references. - Added gimbal calibration subscription and callback in DeformableInfantry class. - Introduced TopBoard class for managing gimbal pitch and friction motors. - Implemented DeformableChassis class to handle chassis control with joystick and keyboard inputs. - Enhanced angle error calculations and control logic for gimbal and chassis. - Registered new input and output interfaces for gimbal and chassis components. - Added support for auto-aim functionality in DeformableInfantryGimbalController.
Enhance Docker support and update deformable-infantry-v2 configurations
…prove torque calculation
Co-authored-by: Copilot <copilot@github.com>
… improved performance
…able-infantry-v2 Includes: refactor: consolidate deformable-specific logic after merges
Catch up with latest origin/main changes.
代码审查摘要Walkthrough该拉取请求为 RMCS 系统引入了完整的"可变形步兵"机器人平台支持，包括新型硬件集成、高级控制算法（ADRC、滑动模式控制）、配置框架和用户界面。同时改进了构建系统以支持默认目录的符号链接验证和恢复工作流。 Changes构建系统改进
可变形步兵平台集成
Sequence Diagram(s)sequenceDiagram
participant Remote as 遥控 (VT13)
participant Vision as 视觉自瞄
participant HW as 硬件集成<br/>(Omni/V2)
participant Gimbal as 云台控制
participant Chassis as 底盘控制
participant Joint as 关节伺服<br/>(ADRC)
participant UI as 裁判UI
participant Ref as 裁判系统
Remote->>HW: 摇杆/鼠标/键盘<br/>输入
Vision->>Gimbal: 自瞄目标方向
HW->>Gimbal: IMU姿态/编码器反馈
Gimbal->>Chassis: 云台偏转角度
Remote->>Chassis: 底盘速度指令
Chassis->>Chassis: PID轨迹规划
Chassis->>Joint: 关节角度目标
Joint->>Joint: ADRC跟踪<br/>(TD→ESO→NLESF)
Joint->>HW: 关节力矩命令
HW->>Ref: 状态采集<br/>(HP/能量/位置)
Ref->>UI: 更新显示元素
UI->>UI: 渲染腿部弧线<br/>和状态环
Estimated code review effort🎯 5 (Critical) | ⏱️ ~120 minutes Possibly related PRs
Suggested reviewers
Poem
✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
|
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 15
Note
Due to the large number of review comments, Critical, Major severity comments were prioritized as inline comments.
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp (1)
260-266:⚠️ Potential issue | 🟠 Major | ⚡ Quick win栈上固定大小数组
is_in_quadratic_constraint[20]在新重载下更易溢出并触发std::terminate新增的
solve(HalfPlaneConstraint)重载，最大可能多边形顶点数为4 + N（N =half_planes.size()，每条半平面最多新增 1 个顶点），用户向量长度并无硬上限。一旦N > 16，会越过当前的 20 个布尔位上限并触发std::terminate()，相当于在线运行时直接崩溃整个进程。原// TODO: Remove this注释也表明作者意识到这是临时方案；新重载使该路径更易被命中。建议改为
std::vector<bool>（或boost::container::small_vector<bool, 20>之类的小缓冲优化），并将std::terminate()替换为返回 NaN 或抛出可恢复异常。♻️ 建议修复
- bool is_in_quadratic_constraint[20]; - if (sizeof(is_in_quadratic_constraint) < linear_constraint.size()) [[unlikely]] - std::terminate(); // TODO: Remove this - for (size_t i = 0; i < linear_constraint.size(); i++) - is_in_quadratic_constraint[i] = + std::vector<uint8_t> is_in_quadratic_constraint(linear_constraint.size()); + for (size_t i = 0; i < linear_constraint.size(); i++) + is_in_quadratic_constraint[i] = is_point_inside_quadratic_constraint(linear_constraint[i], quadratic_constraint);🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp` around lines 260 - 266, The fixed-size stack array is_in_quadratic_constraint[20] can overflow when solve(HalfPlaneConstraint) produces >20 vertices; replace it with a dynamic container (e.g., std::vector<bool> or boost::container::small_vector<bool,20>) and size it from linear_constraint.size() before use, remove the sizeof check and std::terminate() and instead handle the error by throwing a descriptive exception or returning a recoverable error/NaN from the surrounding function; update usages in the loop that calls is_point_inside_quadratic_constraint(linear_constraint[i], quadratic_constraint) to index into the new container.
🟡 Minor comments (24)
rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp-106-111 (1)
106-111:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win确认
kMG5010Ei36的torque_constant = 0.3是否正确。按照本文件的约定（参见
kMG6012Ei8用1.09/8.0把出轴 Kt 折算到电机转子侧），torque_constant表示的是减速前的转子侧扭矩常数，最终输出力矩按torque_constant * reduction_ratio累乘。kMG5010Ei10取0.1，而新增的kMG5010Ei36取0.3：若两者共用同一颗 MG5010 电机本体，转子侧 Kt 理论上应当一致（≈0.1），此处 3 倍的差异会使status_current_to_torque_coefficient_与torque_to_command_current_coefficient_同步偏离 3 倍，导致：
- 上报的
torque_output_数值与实际扭矩偏差约 3 倍；- 力矩闭环 / 力矩前馈控制器的等效增益错位，可能引发抖动或不稳定；
max_torque_ = 25.0仅作展示，不会自动暴露此偏差。请对照 LK 厂家 MG5010 36:1 版本的官方手册确认 Kt 数值（以及它给出的是转子侧还是出轴侧），必要时按
kMG6012Ei8的形式写成<出轴Kt>/36.0以保持惯例一致。LK Tech / SteadyWin MG5010 36:1 reduction motor torque constant Kt datasheet specification🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp` around lines 106 - 111, The torque_constant for Type::kMG5010Ei36 looks incorrect vs the MG5010Ei10 entry and should be verified against LK MG5010 36:1 datasheet; update the kMG5010Ei36 block in lk_motor.hpp so torque_constant represents the rotor-side Kt (not post-reduction) consistent with the convention used for kMG6012Ei8 (i.e., compute as output-shaft_Kt / reduction_ratio or set to the motor's rotor Kt ≈ 0.1 if the datasheet confirms that), keep reduction_ratio = 36.0, and then ensure status_current_to_torque_coefficient_ and torque_to_command_current_coefficient_ will reflect that corrected torque_constant. If the datasheet provides output-shaft Kt, store it as (output_shaft_Kt / 36.0) in torque_constant to maintain consistency..script/build-rmcs-41-42 (1)
41-42:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
rm成功而mkdir -p失败时工作区会处于无目录状态
rm "${link_name}"成功后，若mkdir -p "${link_name}"失败（如磁盘满），符号链接已删除但新目录未创建，后续colcon build会因找不到build/而报错。可考虑先mkdir、再rm，或使用mv原子性替换（但跨文件系统时不可用），降低出错窗口。🛡️ 建议的防御性调整顺序
- rm "${link_name}" || exit 1 - mkdir -p "${link_name}" || exit 1 + mkdir -p "${link_name}.tmp" || exit 1 + rm "${link_name}" || { rm -rf "${link_name}.tmp"; exit 1; } + mv "${link_name}.tmp" "${link_name}" || exit 1注意：
mv在同文件系统内是原子操作，跨文件系统则不然。对于rmcs_ws内部目录，通常为同一文件系统，此方案可行。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In @.script/build-rmcs around lines 41 - 42, The current sequence removes "${link_name}" then creates it, which can leave no directory if mkdir -p fails; instead create the replacement directory first and atomically swap it in: create a temporary directory (e.g., "${link_name}.tmp") with mkdir -p, then use mv -T "${link_name}.tmp" "${link_name}" to replace the old path (mv -T is atomic on the same filesystem), and fallback to the original rm + mkdir approach if mv fails or if cross-filesystem; reference the "${link_name}" and the two commands rm and mkdir in .script/build-rmcs when implementing this defensive replacement..script/build-rmcs-cross-197-205 (1)
197-205:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
--link-default的前置条件校验应在构建开始前执行
check_default_linkable当前在colcon build完成之后才检查阻塞条件（如build/已存在且非符号链接）。若用户带着--link-default运行，但存在阻塞的真实目录，必须等待整个（可能长达数分钟的）交叉编译完成后才会收到错误。建议将三个
check_default_linkable调用移到构建前的验证区（例如，sysroot/toolchain 检查之后、colcon build之前），只保留link_default_base调用在构建完成后。♻️ 建议的调整
if [[ ! -d "${sysroot}" ]]; then ... fi + +if ((link_default)); then + check_default_linkable "${build_base}" build + check_default_linkable "${install_base}" install + check_default_linkable "${log_base}" log +fi ...colcon build... if ((link_default)); then - check_default_linkable "${build_base}" build - check_default_linkable "${install_base}" install - check_default_linkable "${log_base}" log - link_default_base "${build_base}" build link_default_base "${install_base}" install link_default_base "${log_base}" log fi注意：构建前仅校验
link_name是否被非符号链接占用即可；[[ ! -d "${target}" ]]的校验（目标目录是否存在）仍应保留在构建后，因为构建前目标目录可能尚不存在。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In @.script/build-rmcs-cross around lines 197 - 205, Move the three pre-link checks so they run before starting the colcon build: call check_default_linkable for build_base, install_base and log_base during the pre-build validation phase (e.g., right after sysroot/toolchain checks) rather than after build completion; keep the three link_default_base calls where they are to create the symlinks post-build. When moving check_default_linkable, ensure the pre-build check only verifies that the intended link name is not occupied by a non-symlink (i.e., detect if an existing path is a real directory/file that would block creating a symlink) and do not perform the full target-existence check ([[ ! -d "${target}" ]]) until after the build finishes.rmcs_ws/src/rmcs_core/src/controller/smc/smc_observer.cpp-18-19 (1)
18-19:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
velocity_limit_/acceleration_limit_缺乏合理性校验。若 YAML 中误配为负数，第 44 行和第 52 行的
std::clamp(value, -limit, limit)会因为lo > hi触发未定义行为（C++ 标准要求lo <= hi）。建议构造函数中追加if (limit < 0) limit = std::abs(limit);或直接RCLCPP_FATAL抛错，与SlidingModeController中处理terminal_*参数的方式保持一致。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/smc/smc_observer.cpp` around lines 18 - 19, The constructor reads velocity_limit_ and acceleration_limit_ without validating they are non-negative, which can make subsequent std::clamp calls (using -limit..limit) undefined when limit < 0; update the constructor code that sets velocity_limit_ and acceleration_limit_ (the get_parameter(...) assignments) to validate each limit: either normalize negative values with std::abs(...) or mirror SlidingModeController’s behavior and call RCLCPP_FATAL / throw when a limit is negative; ensure the validation happens immediately after assigning velocity_limit_ and acceleration_limit_ so later uses (e.g., the clamp calls) are safe.rmcs_ws/src/rmcs_executor/src/executor.hpp-189-189 (1)
189-189:⚠️ Potential issue | 🟡 Minor | ⚡ Quick winnamespace 关闭花括号后多余的分号。
};在 namespace 闭合后不是必要语法。虽然在 C++ 中尾部;会被解析为命名空间作用域内的空声明而不报错，但属于非惯用写法，并会触发-Wpedantic/-Wextra-semi警告。建议恢复为}：♻️ 建议修改
-}; // namespace rmcs_executor +} // namespace rmcs_executor🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_executor/src/executor.hpp` at line 189, Remove the extraneous semicolon after the namespace closing for rmcs_executor: replace the current "}; // namespace rmcs_executor" with a plain "}" comment form so the namespace rmcs_executor is closed without the trailing semicolon to avoid -Wpedantic / -Wextra-semi warnings.rmcs_ws/src/rmcs_core/src/controller/smc/sliding_mode_controller.cpp-99-111 (1)
99-111:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win移除注释掉的死代码并在入口处统一处理 NaN 输入。
- 第 110 行
// *control_torque_ = 0.0;是调试残留，建议删除。apply_terminal_velocity_limit_（line 144）已对 NaN 输入做了短路返回，但calc_raw_control_torque_没有；当*current_velocity_/*target_velocity_/*angle_error_为 NaN 时，std::clamp在 GCC 上会原样回传 NaN，最终输出仍为 NaN（与reset()含义重叠）。在update()入口检测一次，更易于下游消费者区分"主动 reset"与"上游脏数据"两种状态。♻️ 建议修改
void update() override { if ((*switch_left_ == rmcs_msgs::Switch::DOWN && *switch_right_ == rmcs_msgs::Switch::DOWN) || (*switch_left_ == rmcs_msgs::Switch::UNKNOWN && *switch_right_ == rmcs_msgs::Switch::UNKNOWN)) { reset(); return; } + if (std::isnan(*angle_error_) || std::isnan(*current_velocity_) || std::isnan(*target_velocity_)) { + reset(); + return; + } double control_torque = std::clamp(calc_raw_control_torque_(), -torque_limit_, torque_limit_); control_torque = apply_terminal_velocity_limit_(control_torque); *control_torque_ = std::clamp(control_torque, -torque_limit_, torque_limit_); - // *control_torque_ = 0.0; }🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/smc/sliding_mode_controller.cpp` around lines 99 - 111, Remove the commented-out debug line "// *control_torque_ = 0.0;" and add a NaN-safety guard at the start of update(): after the existing switch/reset check call calc_raw_control_torque_() into a local double (before clamping), if std::isnan(raw) then set *control_torque_ to std::numeric_limits<double>::quiet_NaN() (so downstream can distinguish upstream-bad-data from reset()) and return; otherwise proceed with std::clamp(raw, -torque_limit_, torque_limit_), apply_terminal_velocity_limit_(...), and store the final clamped value into *control_torque_; keep reset() behavior unchanged.rmcs_ws/src/rmcs_core/src/controller/adrc/adrc_V2_说明.md-3-7 (1)
3-7:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win文档中引用的源文件名与实际新增文件不一致。
文档第 7 行和第 193 行（"## 5.
adrc_controller_v2.cpp里的整合关系"）引用的是adrc_controller_v2.cpp，但本 PR 实际新增的 ADRC 控制器源文件是adrc_controller.cpp（位于rmcs_ws/src/rmcs_core/src/controller/adrc/）。需要把文档内的这两处adrc_controller_v2.cpp统一改成adrc_controller.cpp，避免后续阅读者按图索骥找不到对应文件。📝 建议修改
对应代码文件： - `TD.hpp` - `ESO.hpp` - `NLESF.hpp` -- `adrc_controller_v2.cpp` +- `adrc_controller.cpp`-## 5. `adrc_controller_v2.cpp` 里的整合关系 +## 5. `adrc_controller.cpp` 里的整合关系🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/adrc/adrc_V2_说明.md` around lines 3 - 7, The document rmcs_core/src/controller/adrc/adrc_V2_说明.md incorrectly references the new ADRC source as adrc_controller_v2.cpp; update both occurrences (the one near line 7 and the "## 5. `adrc_controller_v2.cpp` 里的整合关系" heading around line 193) to the actual filename adrc_controller.cpp so the doc matches the added source file; ensure any other stray mentions of adrc_controller_v2.cpp in this markdown are also replaced with adrc_controller.cpp.rmcs_ws/src/rmcs_core/src/controller/smc/smc_observer.cpp-24-25 (1)
24-25:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win移除 ROS 参数名中末尾的下划线以保持命名一致性。
参数名
yaw_expect_velocity_和yaw_expect_acceleration_末尾的下划线与其他参数（如control_error、velocity）的命名风格不一致。这些下划线疑似从 C++ 成员变量的命名约定误抄过来。建议去掉末尾下划线，并同步更新对应的 YAML 配置文件。♻️ 建议修改
cpp 文件（lines 24-25）：
- register_output(get_parameter("yaw_expect_velocity_").as_string(), yaw_expect_velocity_); - register_output(get_parameter("yaw_expect_acceleration_").as_string(), yaw_expect_acceleration_); + register_output(get_parameter("yaw_expect_velocity").as_string(), yaw_expect_velocity_); + register_output(get_parameter("yaw_expect_acceleration").as_string(), yaw_expect_acceleration_);YAML 配置文件（deformable-infantry.yaml 中的 4 个类似块）：
- yaw_expect_velocity_: /chassis/*/expect_velocity - yaw_expect_acceleration_: /chassis/*/expect_acceleration + yaw_expect_velocity: /chassis/*/expect_velocity + yaw_expect_acceleration: /chassis/*/expect_acceleration🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/smc/smc_observer.cpp` around lines 24 - 25, 当前代码在 register_output(...) 中使用的参数名 get_parameter("yaw_expect_velocity_") 和 get_parameter("yaw_expect_acceleration_") 带有末尾下划线，与项目中其他参数命名不一致；请在 smc_observer.cpp 中将这两个 get_parameter 调用的字符串参数分别改为 "yaw_expect_velocity" 和 "yaw_expect_acceleration"（保留变量名 yaw_expect_velocity_、yaw_expect_acceleration_ 不变），并更新所有相关 YAML 配置（如 deformable-infantry.yaml 中相应的 4 处块）以移除参数名末尾的下划线，确保 register_output 与配置文件一致；参考标识符：register_output、get_parameter、yaw_expect_velocity_、yaw_expect_acceleration_。rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp-55-55 (1)
55-55:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
rotary_knob_被注册但从未使用，建议删除。第 55 行注册了
/remote/rotary_knob，第 213 行声明了InputInterface<double> rotary_knob_，但整个文件中没有任何位置读取该输入值。update()方法及其调用的子方法均未涉及rotary_knob_。这是未启用的死代码，会增加 executor 依赖图噪声。建议删除以下两处：
♻️ 建议清理
- register_input("/remote/rotary_knob", rotary_knob_);- InputInterface<double> rotary_knob_;🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp` at line 55, 删除未使用的输入接口 rotary_knob_：在构造函数中移除 register_input("/remote/rotary_knob", rotary_knob_) 的调用，且在类中删除成员声明 InputInterface<double> rotary_knob_（包括任何构造器初始化或默认初始化），确保没有其他代码引用 rotary_knob_（若有引用一并清理）；保留 register_input 的其他调用不变。rmcs_ws/src/rmcs_core/src/controller/smc/sliding_mode_controller.cpp-114-125 (1)
114-125:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
/ 2000 / pi_中的魔法常量2000含义不明且缺乏文档。const double velocity_error = (*current_velocity_ - *target_velocity_) / 2000 / pi_;根据代码追踪，
*current_velocity_和*target_velocity_来自电机速度反馈（单位：rad/s，见lk_motor.hpp第188行注释），但此处被无故除以 2000 再除以 π。这个 2000 值：
- 未作为参数或命名常量定义
- 在整个代码库中仅出现在此处，无其他参考
- 不是 RPM→rad/s 换算（应为 2π/60）
- 不是编码器分辨率（16-bit = 65536）
- 无任何注释或文档说明其含义
建议：
- 将 2000 提取为带清晰注释的
static constexpr常量（说明其物理含义和单位）或 ROS 参数，明确表示该因子的用途；- 在 SMC 控制器头部补充一两行注释，说明
current_velocity_与target_velocity_的物理单位（确认为 rad/s）。这样可避免后续参数调试和算法迁移时的踩坑风险。
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/smc/sliding_mode_controller.cpp` around lines 114 - 125, The magic constant 2000 in calc_raw_control_torque_() is undocumented and should be replaced with a named constant and unit documentation: extract the literal into a static constexpr (e.g., velocity_scaling_factor or velocity_unit_conversion) with a clear comment describing its physical meaning and units, or expose it as a configurable parameter, and update the SMC controller header to state that current_velocity_ and target_velocity_ are in rad/s; then replace the inline "/ 2000 / pi_" with a division by that named constant (and pi_) so the purpose is explicit in calc_raw_control_torque_().rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp-70-74 (1)
70-74:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win对
half_plane入参缺少有限性校验每条
HalfPlaneConstraint的(a, b, c)直接进入sutherland_hodgman。若调用方上游计算出 NaN/Inf（例如除以 0 的归一化失败），edge_compare返回 NaN，分支判断<0/>0/==0全部为 false，会无声地把整条半平面"跳过"，最终返回的"可行点"实际并未受到该约束限制。建议在循环开头对(a, b, c)加std::isfinite守卫，并在违例时直接返回 NaN 向量与日志告警。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp` around lines 70 - 74, In the loop over half_planes before calling sutherland_hodgman, validate that half_plane.a, half_plane.b and half_plane.c are finite (use std::isfinite); if any is not finite, log a warning/error and immediately return Eigen::Vector2d::Constant(nan_) to signal infeasibility instead of silently skipping the constraint. Apply this guard in the same scope where polygon and sutherland_hodgman are used (referencing HalfPlaneConstraint/half_plane, sutherland_hodgman, edge_compare, polygon, and nan_) so malformed (NaN/Inf) constraints can’t bypass the solver.rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp-13-14 (1)
13-14:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
dt未做正值校验，存在被零除风险构造函数直接接受
dt而不做校验。若上层错误传入dt = 0或负值，update()中的velocity_ = velocity_ + (beta_ * residual) / dt_会产生inf/-nan，并通过wrap_angle_持续污染angle_、velocity_，且没有任何报错路径。建议在构造函数中断言或截断为安全下界。🛡️ 建议修复
AlphaBetaAngleFilter(double dt, double alpha, double beta) - : dt_(dt), alpha_(alpha), beta_(beta) {} + : dt_(dt > 0.0 && std::isfinite(dt) ? dt : 1e-3) + , alpha_(alpha) + , beta_(beta) {}🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp` around lines 13 - 14, 构造函数 AlphaBetaAngleFilter 接受的 dt 未校验会导致 update() 中的 velocity_ = velocity_ + (beta_ * residual) / dt_ 在 dt_ 为 0 或负时产生 inf/NaN 并通过 wrap_angle_ 污染 angle_/velocity_；请在 AlphaBetaAngleFilter(double dt, double alpha, double beta) 中对传入 dt 做防护：要么断言/抛出异常（如 illegal_argument）阻止非法值，要么将 dt_ 截断到一个安全下界（例如 std::max(dt, eps)），然后在构造器内记录/注释该行为；确保关联符号为构造函数 AlphaBetaAngleFilter、成员 dt_、以及在 update() 中使用的 velocity_ 计算与 wrap_angle_ 行为都能在非法 dt 情况下得到保护。rmcs_ws/src/rmcs_core/src/controller/adrc/TD.hpp-85-91 (1)
85-91:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
sanitize_config未防御 NaN 输入若
cfg.r或cfg.h为 NaN，std::max(NaN, kEps)在 IEEE 754 下返回值依赖实现（多数实现返回第一个参数），随后fhan中的r*h*h与sqrt会传播 NaN，导致update整条链路失效且不易排查。建议显式以std::isfinite检测并替换为安全默认值。🛡️ 建议修复
void sanitize_config() { constexpr double kEps = 1e-9; - cfg_.r = std::max(cfg_.r, kEps); - cfg_.h = std::max(cfg_.h, kEps); - cfg_.max_vel = std::max(cfg_.max_vel, 0.0); - cfg_.max_acc = std::max(cfg_.max_acc, 0.0); + if (!std::isfinite(cfg_.r)) cfg_.r = kEps; + if (!std::isfinite(cfg_.h)) cfg_.h = kEps; + cfg_.r = std::max(cfg_.r, kEps); + cfg_.h = std::max(cfg_.h, kEps); + if (std::isnan(cfg_.max_vel)) cfg_.max_vel = std::numeric_limits<double>::infinity(); + if (std::isnan(cfg_.max_acc)) cfg_.max_acc = std::numeric_limits<double>::infinity(); + cfg_.max_vel = std::max(cfg_.max_vel, 0.0); + cfg_.max_acc = std::max(cfg_.max_acc, 0.0); }🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/adrc/TD.hpp` around lines 85 - 91, sanitize_config currently uses std::max which doesn't handle NaN; update sanitize_config to explicitly check std::isfinite(cfg_.r) and std::isfinite(cfg_.h) and replace non-finite values with a safe default (e.g. kEps) before taking max, and similarly ensure cfg_.max_vel and cfg_.max_acc are finite and non-negative (replace non-finite with 0.0 or a safe default); this prevents NaN from propagating into fhan (and update) by guaranteeing r and h are valid finite values.rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp-102-111 (1)
102-111:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win本地
to_referee_anglelambda 与DeformableChassisLegArcs::to_referee_angle_实现不一致，缺少角度归一化
DeformableChassisLegArcs::to_referee_angle_通过% 360+ 负值条件校正，显式保证返回值在[0, 360)范围内（返回uint16_t）；而此处的 lambda 直接static_cast<int>(std::round(...))后返回int，未做任何归一化。当
*chassis_angle_不在[0, 2π)范围内时（累计角、负角或异常值），lambda 会返回负整数或超过360的值。传给set_angle(uint16_t, ...)时，负int会隐式转换为极大的uint16_t值（如-100→65436），导致 UI 显示错乱。建议直接复用
DeformableChassisLegArcs::to_referee_angle_或实现相同的归一化逻辑。同时应加std::isfinite守卫，避免*chassis_angle_为 NaN 时的未定义行为。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp` around lines 102 - 111, 在 update_chassis_direction_indicator 中当前本地 lambda to_referee_angle 未做角度归一化且返回 int，会在传入非标准角度或 NaN 时导致负值被转换为大 uint16_t 而出错；请改为直接复用 DeformableChassisLegArcs::to_referee_angle_ 或在该 lambda 中实现等价逻辑：先用 std::isfinite(*chassis_angle_) 检查，归一化角度到 [0, 2π) 后按 DeformableChassisLegArcs::to_referee_angle_ 的 %360 规则生成 uint16_t 范围内的 [0,360) 角度，再传入 chassis_direction_indicator_.set_angle（保持类型为 uint16_t 或显式转换），以消除负值/NaN 导致的 UI 错误。rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp-165-167 (1)
165-167:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
y_max == 0时y_sign退化为 -1。
y_max > 0 ? 1.0 : -1.0会把y_max == 0（无角速度需求）和NaN都映射为 -1。最后result.y() *= y_sign会把约束求解返回的非负 y 值翻成负号，等同于在零目标场景注入了一个反方向的角加速度上限。建议显式用std::copysign或y_max < 0 ? -1.0 : 1.0让零保持正号。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp` around lines 165 - 167, The current ternary y_max > 0 ? 1.0 : -1.0 maps y_max == 0 (and NaN) to -1, causing result.y() to be flipped when target is zero; update the sign logic around chassis_control_torque.torque (the x_max, y_max destructuring and the y_sign variable) to preserve zero as positive by using either std::copysign or the explicit test y_max < 0 ? -1.0 : 1.0 so that result.y() is multiplied by the correct sign and zero stays non-negative.rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp-108-128 (1)
108-128:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
update_trajectory对delta_time缺少非正值保护。第 122 行
delta_velocity / delta_time在delta_time == 0时会得到±inf，再std::clamp到[-acceleration_limit, acceleration_limit]虽不至于 NaN，但当acceleration_limit也是 inf（默认 fallback 见 deformable_joint_controller 中td_max_acc = inf）时仍会传播 inf 进target_velocity_state_。给一个if (delta_time <= 0.0) return;是 O(1) 且可避免上层错误调度引入的姿态崩盘。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp` around lines 108 - 128, The update_trajectory method can divide by delta_time and propagate ±inf into target_* states when delta_time <= 0 (and acceleration_limit may be inf); inside update_trajectory (method name update_trajectory) add a guard at the start that returns immediately if delta_time <= 0.0 to avoid computing delta_velocity / delta_time and prevent inf from reaching target_acceleration_state_, target_velocity_state_, target_physical_state_, and target_motor_state_; keep the rest of the loop unchanged.rmcs_ws/src/rmcs_core/src/hardware/device/VT13.hpp-242-248 (1)
242-248:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win公共方法名拼写错误：
botton_update→button_update。
botton_update是单词 button 的拼写错误，且属于已暴露的公开成员。update_status()内部第 179 行也直接调用了它。建议趁此 PR 引入新接口时一并改正，避免错别字进入未来调用代码。✏️ 建议
- botton_update(); + button_update();- void botton_update() { + void button_update() {Also applies to: 179-180
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/hardware/device/VT13.hpp` around lines 242 - 248, The public method name botton_update is misspelled; rename the method to button_update and update all callers (notably the call inside update_status that currently invokes botton_update) to use the new name; ensure you update the method declaration and any forward declarations/usages of botton_update in the VT13 class (and related headers) so signatures remain consistent and rebuilds cleanly.rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp-97-99 (1)
97-99:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
measurement_velocity_已注册为必需输入但全程未使用。构造函数第 98 行
register_input(get_parameter("measurement_velocity").as_string(), measurement_velocity_);强制要求 YAML 配置measurement_velocity参数，但read_inputs_/run_joint_servo_全程没有引用measurement_velocity_，下游 ESO 也是用last_u_与measurement_angle_估计速度。这意味着：
- 引入了一条与 ESO 的"角度+控制量"估速结构无关的硬性依赖；
- 如果该 YAML 字段写错或将来要去掉，会触发参数缺失异常但实际行为完全不变。
要么把它真正接入伺服回路（例如让 ESO 用观测速度做 z2 注入或用作 D 项），要么移除
register_input调用并去掉对应参数。Also applies to: 336-340
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp` around lines 97 - 99, measurement_velocity_ is registered as a required input via register_input(...) but never used; either remove the registration and associated YAML parameter, or actually consume the input in the servo path. To fix: if you want to drop the unused dependency, delete the register_input(get_parameter("measurement_velocity").as_string(), measurement_velocity_) calls (lines around the constructor and the similar block at 336-340) and remove the corresponding YAML parameter; otherwise, wire measurement_velocity_ into read_inputs_ and run_joint_servo_ (and the ESO update path) by replacing the estimated velocity use (where last_u_ and measurement_angle_ are used to estimate speed) with the provided measurement_velocity_ or pass it into the ESO as the z2/velocity injection so the value is actually consumed.rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp-308-317 (1)
308-317:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win短暂的 NaN 反馈会导致后续
alpha_ddot_rad误差。当某次迭代中
alpha_rad或alpha_dot_rad出现非有限值时直接return joint;，但没有重置last_joint_velocity_valid_或last_joint_velocity_。下一次反馈恢复有效时，算法仍会以dt_为分母对相隔多个周期的速度差求差分，得到放大若干倍的alpha_ddot_rad，并随之污染下游populate_joint_geometry_的radius_ddot与控制律输出。🛡️ 建议在 NaN 早退路径上同步失效缓存
if (!joint.alpha_rad.array().isFinite().all() - || !joint.alpha_dot_rad.array().isFinite().all()) - return joint; + || !joint.alpha_dot_rad.array().isFinite().all()) { + last_joint_velocity_valid_ = false; + return joint; + }🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp` around lines 308 - 317, When early-returning due to non-finite joint.alpha_rad or joint.alpha_dot_rad, invalidate the cached velocity so a future valid sample doesn't produce a huge derivative: inside the same NaN-check branch (the block that currently does "return joint;") set last_joint_velocity_valid_ = false and reset last_joint_velocity_ to a safe state (e.g. zero or match current joint.alpha_dot_rad) before returning so subsequent alpha_ddot_rad calculation (which uses last_joint_velocity_ and dt_) won't compute a spurious large value.rmcs_ws/src/rmcs_core/src/referee/status/field.hpp-45-52 (1)
45-52:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win将
PowerHeatData的前三个字段改用实际语义名称，而非 reserved 占位符。根据 RoboMaster 裁判系统协议 0x0202，
PowerHeatData确实是 14 字节，字段布局正确。但前三个字段命名为reserved_1/2/3不够清晰，建议改为：
uint16_t chassis_voltage（对应reserved_1）uint16_t chassis_current（对应reserved_2）float chassis_power（对应reserved_3）这样可以提高代码可读性，并与官方协议对齐。同时，建议为
PowerHeatData及其他结构体（如GameStatus、GameRobotHp等）添加static_assert(sizeof(...) == N)断言，确保在编译期捕获任何意外的对齐或字段变更。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/referee/status/field.hpp` around lines 45 - 52, Rename the first three members of struct PowerHeatData from reserved_1, reserved_2, reserved_3 to chassis_voltage, chassis_current, chassis_power respectively (types: uint16_t, uint16_t, float) so the field names match the 0x0202 protocol; update any usages of PowerHeatData members accordingly. After the struct definitions, add compile-time assertions like static_assert(sizeof(PowerHeatData) == 14) and similarly add static_asserts for related structs mentioned in the review (e.g., GameStatus, GameRobotHp) to ensure their sizes remain as expected. Ensure symbol names referenced are PowerHeatData, chassis_voltage, chassis_current, chassis_power, GameStatus, and GameRobotHp.rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp-644-672 (1)
644-672:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win优化 PID 输入以避免 NaN 状态污染。
当
steering_control_angle或steering_control_velocity包含 NaN 时，虽然MatrixPidCalculator::update()对 I 项（积分）和 D 项（微分）应用了exclude_nan()过滤，但 P 项（比例）的计算kp * err会直接产生 NaN，该 NaN 在当前迭代中未被过滤，导致：
steering_angle_pid_的 P 项输出 NaN- 该 NaN 级联传入
steering_velocity_pid_的输入steering_velocity_pid_的 P 项再次输出 NaN- 虽最终在第 672 行通过
unaryExpr过滤为 0，但 PID 内部的last_err_已被赋值为 NaN虽然系统在下一迭代会自动恢复（
last_err_被重新赋值），但在 NaN 被设置的迭代中，整个 PID 级联的输出都被污染为 NaN，这反映了欠佳的设计。建议在调用 PID 前对每个分量检测std::isnan，仅对有效轮号累计积分和更新 PID 状态（避免 NaN 进入状态机）。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp` around lines 644 - 672, Pre-filter steering_control_angle and steering_control_velocity to prevent NaNs entering the PID state: before calling steering_angle_pid_.update and steering_velocity_pid_.update, build masked vectors (e.g., valid_control_angle and valid_control_velocity) that replace std::isnan entries with values that produce zero error for that channel (for example use steering_status.angle and steering_status.velocity respectively) or otherwise neutralize the P term; then pass those masked vectors into steering_angle_pid_.update / steering_velocity_pid_.update so the P term never receives NaN and the PID internals (last_err_, integrator) are not polluted by NaN. Ensure you reference steering_control_angle, steering_control_velocity, steering_status.angle, steering_status.velocity, steering_angle_pid_.update and steering_velocity_pid_.update when making the change.rmcs_ws/src/rmcs_bringup/config/deformable-infantry.yaml-96-122 (1)
96-122:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
gimbal_controller中存在未被控制器读取的参数
DeformableInfantryGimbalController当前实现里没有读取yaw_vel_ff_gain、yaw_acc_ff_gain、pitch_acc_ff_gain，这些前馈增益在本配置里是 dead config，调参时会误导操作者以为生效。另外
use_encoder_pitch: false也被控制器忽略（见deformable_infantry_gimbal_controller.cpp中use_encoder_pitch硬编码评论，建议在那里统一修复）。建议要么删除这些无用键、要么把它们接入控制器。
建议清理：删除未使用键
yaw_velocity_kp: 6.4 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 - yaw_vel_ff_gain: 0.47 - yaw_acc_ff_gain: 0.00 - # Placeholder pitch gains copied from omni-infantry until pitch sweep is available. pitch_angle_kp: 8.0 pitch_angle_ki: 0.0 pitch_angle_kd: 0.0 pitch_velocity_kp: 0.0 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - - pitch_acc_ff_gain: 0.0🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_bringup/config/deformable-infantry.yaml` around lines 96 - 122, The YAML exposes yaw_vel_ff_gain, yaw_acc_ff_gain, pitch_acc_ff_gain and use_encoder_pitch but DeformableInfantryGimbalController (see deformable_infantry_gimbal_controller.cpp) doesn't read or use them; either remove these dead keys from the config or add parameter handling and wiring: add class members (e.g., yaw_vel_ff_gain, yaw_acc_ff_gain, pitch_acc_ff_gain, use_encoder_pitch), load them from ros__parameters in the controller's parameter parsing/initialize method, and apply the gains in the control computation where yaw feedforward and pitch handling occur (and replace the hardcoded use_encoder_pitch comment with the loaded flag).rmcs_ws/src/rmcs_bringup/config/deformable-infantry-v2.yaml-97-122 (1)
97-122:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
gimbal_controller中的部分参数未被控制器读取
DeformableInfantryGimbalController的实现里只读取了：
upper_limit/lower_limit/pitch_torque_control/manual_joystick_sensitivity/manual_mouse_sensitivity/*_angle_*/*_velocity_*这几类参数，未读取 当前 v2 YAML 中的inertia: 1.0、friction: 1.65。这两个参数在该控制器中是 dead config，对实际运行无影响，容易让调参/维护人员误以为生效。另外，本 YAML 中
use_encoder_pitch: true实际也未被控制器读取（详见对应控制器文件的评论），属于同一类问题。建议要么删除这些无用键以避免误导，要么在控制器里把它们接入实际使用。
建议清理：删除未使用键
gimbal_controller: ros__parameters: - inertia: 1.0 # kg·m² - friction: 1.65 # Nm/(rad/s) - upper_limit: -0.61 # -35 deg lower_limit: 0.05 # 6 deg - use_encoder_pitch: true🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_bringup/config/deformable-infantry-v2.yaml` around lines 97 - 122, The YAML exposes inertia, friction, and use_encoder_pitch but DeformableInfantryGimbalController does not read or use these keys, causing dead config; either remove these keys from the deformable-infantry-v2.yaml to avoid confusion, or update the controller (DeformableInfantryGimbalController) to read and apply them: add parameter parsing for "inertia" and "friction" in the controller's parameter load path and use them in the dynamics/torque computation, and add a boolean read for "use_encoder_pitch" where pitch sensor selection or processing occurs so the config becomes effective.rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml-32-53 (1)
32-53:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win
value_broadcaster组件被注释，但参数块仍保留，配置形成死代码。第 32 行将
rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster的注册整行注释掉，但第 35–53 行依然完整定义了value_broadcaster的forward_list。这会导致：执行器不会加载该组件，整段forward_list中列举的力矩 / 悬挂模式 / 悬挂力矩 / 控制力矩话题都不会被广播，但配置看起来"已生效"，对调试和回放（明显是为了 PR 中提到的 supercap/motor debug logging 而设计的）会造成误导。请在合并前明确意图：要么取消第 32 行的注释以启用调试广播，要么删掉第 35–53 行避免无效配置。
建议修改（启用广播）
- # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - - rmcs::AutoAimComponent -> auto_aim_component + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs::AutoAimComponent -> auto_aim_component或者，如果当前不希望启用广播，则删除整段未使用的
value_broadcaster配置块。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml` around lines 32 - 53, value_broadcaster 的组件注册（rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster）被注释掉，但其参数块（ros__parameters.forward_list）仍然存在，造成“假生效”的配置；请要么恢复/取消注释映射行以启用 ValueBroadcaster（rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster），确保 forward_list 中的主题被实际广播，要么删除整个 value_broadcaster 参数块（包含 forward_list）以消除无效配置并避免误导调试/回放。
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: 1a4d3875-6b2d-4319-8bd6-ba787b5608f0
📒 Files selected for processing (40)
.script/build-rmcs.script/build-rmcs-cross.script/complete/_build-rmcs-crossrmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yamlrmcs_ws/src/rmcs_bringup/config/deformable-infantry-v2.yamlrmcs_ws/src/rmcs_bringup/config/deformable-infantry.yamlrmcs_ws/src/rmcs_core/plugins.xmlrmcs_ws/src/rmcs_core/src/controller/adrc/ESO.hpprmcs_ws/src/rmcs_core/src/controller/adrc/NLESF.hpprmcs_ws/src/rmcs_core/src/controller/adrc/TD.hpprmcs_ws/src/rmcs_core/src/controller/adrc/adrc_V2_说明.mdrmcs_ws/src/rmcs_core/src/controller/adrc/adrc_controller.cpprmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpprmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpprmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpprmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpprmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpprmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpprmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpprmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpprmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpprmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpprmcs_ws/src/rmcs_core/src/controller/smc/sliding_mode_controller.cpprmcs_ws/src/rmcs_core/src/controller/smc/smc_observer.cpprmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpprmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpprmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-v2.cpprmcs_ws/src/rmcs_core/src/hardware/device/VT13.hpprmcs_ws/src/rmcs_core/src/hardware/device/dji_motor_with_encoder.hpprmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpprmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpprmcs_ws/src/rmcs_core/src/hardware/utility/ring_buffer.hpprmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpprmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpprmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpprmcs_ws/src/rmcs_core/src/referee/status.cpprmcs_ws/src/rmcs_core/src/referee/status/field.hpprmcs_ws/src/rmcs_executor/src/executor.hpprmcs_ws/src/rmcs_msgs/include/rmcs_msgs/VT13_switch.hpprmcs_ws/src/rmcs_msgs/include/rmcs_msgs/hard_sync_snapshot.hpp
| if (!refresh_limit_()) { | ||
| disable_output_(); | ||
| return; | ||
| } | ||
|
|
||
| const double measurement_or_error = *measurement_; | ||
| if (!std::isfinite(measurement_or_error)) { | ||
| disable_output_(); | ||
| return; | ||
| } | ||
|
|
||
| double reference = 0.0; | ||
| double measurement = 0.0; | ||
|
|
||
| if (use_error_input_mode_) { | ||
| reference = 0.0; | ||
| measurement = -measurement_or_error; | ||
| } else { | ||
| reference = *setpoint_; | ||
| if (!std::isfinite(reference)) { | ||
| disable_output_(); | ||
| return; | ||
| } | ||
| measurement = measurement_or_error; | ||
| } | ||
|
|
||
| const auto td_out = td_.update(reference); | ||
| const auto eso_out = eso_.update(measurement, last_u_); | ||
|
|
||
| const double e1 = td_out.x1 - eso_out.z1; | ||
| const double e2 = td_out.x2 - eso_out.z2; | ||
|
|
||
| const auto nlesf_out = nlesf_.compute(e1, e2, eso_out.z3, b0_); | ||
| const double scaled_u = kt_ * nlesf_out.u; | ||
|
|
||
| const double final_u = std::clamp(scaled_u, output_min_, output_max_); | ||
| *control_ = final_u; | ||
| last_u_ = final_u; | ||
| } |
There was a problem hiding this comment.
disable_output_ 未复位 TD/ESO 内部状态，可能在恢复时产生控制突跳
当 refresh_limit_() 失败、measurement 或 setpoint 出现 NaN 时，仅将输出置为 NaN 并把 last_u_ 清零，但 TD 的 (x1, x2) 与 ESO 的 (z1, z2, z3) 仍保留了上一次的值。等到测量恢复后：
- ESO 的
z1与新测量y之间可能有很大偏差，导致e = z1 - y暴增并通过beta1/2/3把z2/z3推到极端值； - TD 内部状态对新参考可能完全过时，
x1/x2与设定值差距很大； nlesf_在e1 = td.x1 - eso.z1失常的情况下可能输出接近被 clamp 到极限的u。
结果是控制器在丢点恢复后一两个周期内可能输出一次接近上下界的"踢"，对真实执行器（电机扭矩等）是危险的。建议在 disable_output_() 中同步对 td_.reset(...)、eso_.reset(...) 进行复位，或提供"暂停"状态机让恢复路径走 warm-start。
如需我可以提交一版修复，包含 warm-start 逻辑（首次恢复时用最新的 *measurement_ 重置 ESO 的 z1、用最新的 *setpoint_ 重置 TD 的 x1）。
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@rmcs_ws/src/rmcs_core/src/controller/adrc/adrc_controller.cpp` around lines
67 - 105, disable_output_() only sets outputs to NaN/zero but does not reset
TD/ESO internal states (td_.x1/x2, eso_.z1/z2/z3), causing large transients when
measurements/setpoints recover; modify disable_output_() to also reset or pause
the state estimators and time derivate: call td_.reset(...) and eso_.reset(...)
(or enter a paused/warm-start state) and on first valid recovery perform a
warm-start using the latest *setpoint_ to initialize td_ (x1) and latest
*measurement_ to initialize eso_ (z1) so e1/e2 remain bounded and nlesf_ does
not produce a clamped kick. Ensure last_u_ handling remains consistent with the
reset behavior.
| const double effective_limit = std::max(0.0, raw_limit); | ||
| if (std::isfinite(last_limit_) && std::abs(effective_limit - last_limit_) <= 1e-9) { | ||
| return true; | ||
| } | ||
|
|
||
| auto cfg = nlesf_.config(); | ||
| cfg.u_min = -effective_limit; | ||
| cfg.u_max = effective_limit; | ||
| nlesf_.set_config(cfg); | ||
| last_limit_ = effective_limit; | ||
| return true; |
There was a problem hiding this comment.
limit 参数语义可能与用户预期不一致（与 kt_ 的单位耦合）
refresh_limit_() 把 effective_limit 直接灌到 NLESF::Config::u_min/u_max，但随后的 update() 又对 nlesf_out.u 乘以 kt_ 才得到 *control_。这意味着 limit 输入实际限制的是 NLESF 内部的 u，最终下发的 *control_ 上界其实是 kt_ * effective_limit。当 kt_ != 1 时，外部以"输出物理量"理解 limit 的调用方会得到与预期不一致的结果。
建议二选一：
- 在
refresh_limit_()中改为cfg.u_min = -effective_limit / kt_; cfg.u_max = effective_limit / kt_;，保持limit始终是*control_的物理上界； - 或在文档中明确说明
limit的单位是 NLESF 内部的u，并把kt_在limit注释中标注。
同时，建议结合 output_min_/output_max_ 一起取交集，避免 limit 大于 output_max_ 时实际被 output_max_ 兜底导致 limit 失效的歧义。
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@rmcs_ws/src/rmcs_core/src/controller/adrc/adrc_controller.cpp` around lines
122 - 132, refresh_limit_() currently writes effective_limit straight into
nlesf_.config().u_min/u_max, but update() later multiplies nlesf_out.u by kt_ to
produce *control_, so when kt_ != 1 the external meaning of limit (as a bound on
*control_) is incorrect; fix by either (A) dividing by kt_ when setting
cfg.u_min/u_max (cfg.u_min = -effective_limit/kt_; cfg.u_max =
effective_limit/kt_) so limit remains the physical bound on *control_, or (B)
document that limit is a bound on the internal NLESF u (not *control_) and
annotate kt_ in the comment; additionally intersect the computed u_min/u_max
with output_min_/output_max_ so the effective limit respects external output
bounds (use nlesf_.config()/set_config and last_limit_ accordingly).
| void update_mode_selection_(bool suspension_mode) { | ||
| if (suspension_mode != last_suspension_mode_) { | ||
| apply_mode_config_(active_mode_config_(suspension_mode)); | ||
| last_suspension_mode_ = suspension_mode; | ||
| } | ||
| } | ||
|
|
||
| const ModeConfig& active_mode_config_(bool suspension_mode) const { | ||
| return suspension_mode ? suspension_mode_config_ : normal_mode_config_; | ||
| } | ||
|
|
||
| void apply_mode_config_(const ModeConfig& mode_config) { | ||
| td_.set_config(mode_config.td); | ||
| eso_.set_config(mode_config.eso); | ||
| nlesf_.set_config(mode_config.nlesf); | ||
| } |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
fd -t f -e hpp 'TD.hpp|ESO.hpp|NLESF.hpp' --exec rg -nP '(reset|set_config)\s*\(' {}Repository: Alliance-Algorithm/RMCS
Length of output: 485
🏁 Script executed:
# First, locate the controller file and search for reset_states_ method
fd -t f -name 'deformable_joint_controller.cpp' --exec cat -n {} \; | head -100Repository: Alliance-Algorithm/RMCS
Length of output: 298
🏁 Script executed:
# Search for reset_states_ in the controller
fd -t f -name '*controller*.cpp' -o -name '*controller*.hpp' | xargs rg -nP 'reset_states_' 2>/dev/null | head -20Repository: Alliance-Algorithm/RMCS
Length of output: 624
🏁 Script executed:
# Get full file paths for the header files
fd -t f \( -name 'TD.hpp' -o -name 'ESO.hpp' -o -name 'NLESF.hpp' \)Repository: Alliance-Algorithm/RMCS
Length of output: 298
🏁 Script executed:
# Look at full reset method signatures for TD, ESO, NLESF
fd -t f \( -name 'TD.hpp' -o -name 'ESO.hpp' -o -name 'NLESF.hpp' \) --exec cat -n {} +Repository: Alliance-Algorithm/RMCS
Length of output: 298
🏁 Script executed:
# Get the implementation of reset_states_ and surrounding code
fd -type f -name 'deformable_joint_controller.cpp' -exec sed -n '230,310p' {} \;Repository: Alliance-Algorithm/RMCS
Length of output: 238
🏁 Script executed:
# Find the header files with a simpler approach
fd -type f -name '*.hpp' | grep -E '(TD|ESO|NLESF)'Repository: Alliance-Algorithm/RMCS
Length of output: 238
🏁 Script executed:
# Get reset method signatures for TD, ESO, NLESF
fd -type f -name '*.hpp' | xargs grep -A 5 'void reset'Repository: Alliance-Algorithm/RMCS
Length of output: 240
🏁 Script executed:
# Get the implementation around mode switching and reset
fd -t f -name 'deformable_joint_controller.cpp' -exec sed -n '230,310p' {} \;Repository: Alliance-Algorithm/RMCS
Length of output: 298
🏁 Script executed:
# Find header files with correct syntax
fd -t f -name '*.hpp' | grep -iE '(td|eso|nlesf)' | head -20Repository: Alliance-Algorithm/RMCS
Length of output: 298
🏁 Script executed:
# Search for reset method definitions more broadly
rg -t cpp -t hpp 'void reset\(' | grep -E '(TD|ESO|NLESF)' | head -20Repository: Alliance-Algorithm/RMCS
Length of output: 95
🏁 Script executed:
# Find the controller file and read it
find . -name 'deformable_joint_controller.cpp' -type fRepository: Alliance-Algorithm/RMCS
Length of output: 146
🏁 Script executed:
# Search for header files
find . -name '*.hpp' -type f | grep -iE '(td|eso|nlesf)' | head -10Repository: Alliance-Algorithm/RMCS
Length of output: 224
🏁 Script executed:
# Search for reset methods in any file
rg 'void reset\(' | grep -iE '(td|eso|nlesf)' | head -20Repository: Alliance-Algorithm/RMCS
Length of output: 265
🏁 Script executed:
# Read the controller file
cat ./rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp | head -350 | tail -150Repository: Alliance-Algorithm/RMCS
Length of output: 6261
🏁 Script executed:
# Read the header files
cat ./rmcs_ws/src/rmcs_core/src/controller/adrc/TD.hppRepository: Alliance-Algorithm/RMCS
Length of output: 2454
🏁 Script executed:
cat ./rmcs_ws/src/rmcs_core/src/controller/adrc/ESO.hppRepository: Alliance-Algorithm/RMCS
Length of output: 2337
🏁 Script executed:
cat ./rmcs_ws/src/rmcs_core/src/controller/adrc/NLESF.hppRepository: Alliance-Algorithm/RMCS
Length of output: 2153
模式切换时应重置 TD/ESO 内部状态以避免瞬态尖峰
apply_mode_config_() 仅通过 set_config() 更新参数，未重置 TD 的 (x1, x2) 或 ESO 的 (z1, z2, z3) 状态。当两套配置的 eso.w0、nlesf.k1/k2 差异较大时，原有状态与新增益的乘积会产生不连续输出。虽然 reset_states_() 方法已存在，但仅在初始化时调用，模式切换时未触发。
建议在 apply_mode_config_() 中调用 reset_states_(inputs.measurement_angle, inputs.setpoint_angle) 来重置滤波器状态。注意 NLESF 为无状态计算，无需重置。
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp`
around lines 232 - 247, apply_mode_config_ currently only calls set_config on
td_, eso_, and nlesf_ but does not reset internal TD/ESO states causing
transient spikes when switching configs; modify apply_mode_config_ (used by
update_mode_selection_) to call reset_states_ with the current measurement and
setpoint (inputs.measurement_angle, inputs.setpoint_angle) after applying
td_.set_config(mode_config.td) and eso_.set_config(mode_config.eso); do not
reset anything for nlesf_ since NLESF is stateless. Ensure you reference
apply_mode_config_, td_, eso_, reset_states_, and inputs.* when implementing.
| static Eigen::Vector2d solve( | ||
| const Eigen::Vector2d& objective, const BoundaryConstraint& boundary_constraint, | ||
| const std::vector<HalfPlaneConstraint>& half_planes, | ||
| const QuadraticConstraint& quadratic_constraint) { | ||
| std::vector<Eigen::Vector2d> polygon = { | ||
| {boundary_constraint.x_max, boundary_constraint.y_max}, | ||
| { 0.0, boundary_constraint.y_max}, | ||
| { 0.0, -boundary_constraint.y_max}, | ||
| {boundary_constraint.x_max, -boundary_constraint.y_max}, | ||
| }; | ||
|
|
||
| for (const auto& half_plane : half_planes) { | ||
| sutherland_hodgman(polygon, {half_plane.a, half_plane.b, -half_plane.c}); | ||
| if (polygon.size() < 3) | ||
| return Eigen::Vector2d::Constant(nan_); | ||
| } | ||
|
|
||
| auto quadratic_constraint_matrixes = | ||
| calculate_quadratic_constraint_matrixes(quadratic_constraint); | ||
| auto quadratic_constraint_best_point = | ||
| calculate_quadratic_constraint_best_point(objective, quadratic_constraint_matrixes); | ||
|
|
||
| if (is_point_inside_polygon(quadratic_constraint_best_point, polygon)) | ||
| return quadratic_constraint_best_point; | ||
| else | ||
| return calculate_best_value_at_intersections( | ||
| objective, polygon, quadratic_constraint_matrixes); | ||
| } |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# 找到所有使用新 solve(HalfPlaneConstraint) 重载的调用点，确认其是否传入了 x >= 0 的半平面
fd -t f -e cpp -e hpp | xargs rg -nP -C5 '\bQcpSolver::solve\s*\('
fd -t f -e cpp -e hpp | xargs rg -nP -C3 'HalfPlaneConstraint'Repository: Alliance-Algorithm/RMCS
Length of output: 2476
🏁 Script executed:
cat -n rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp | sed -n '190,210p'Repository: Alliance-Algorithm/RMCS
Length of output: 1501
🏁 Script executed:
cat -n rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp | sed -n '100,140p'Repository: Alliance-Algorithm/RMCS
Length of output: 2165
🏁 Script executed:
rg -n 'solve.*RhombusConstraint' -A 20 rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hppRepository: Alliance-Algorithm/RMCS
Length of output: 49
🏁 Script executed:
cat -n rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp | sed -n '50,90p'Repository: Alliance-Algorithm/RMCS
Length of output: 2231
🏁 Script executed:
rg -B 10 'half_planes' rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp | head -40Repository: Alliance-Algorithm/RMCS
Length of output: 1520
🏁 Script executed:
cat -n rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp | sed -n '20,70p'Repository: Alliance-Algorithm/RMCS
Length of output: 2389
🏁 Script executed:
rg -B 5 'struct HalfPlaneConstraint' rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hppRepository: Alliance-Algorithm/RMCS
Length of output: 167
🏁 Script executed:
cat -n rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp | sed -n '88,105p'Repository: Alliance-Algorithm/RMCS
Length of output: 1111
🏁 Script executed:
rg -A 5 'calculate_polygon_constraints' rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp | head -20Repository: Alliance-Algorithm/RMCS
Length of output: 672
新 solve 重载的初始多边形为非对称半矩形，与旧重载语义不一致，缺乏文档说明
第 63-68 行构造的初始多边形仅覆盖 x ∈ [0, x_max] × [-y_max, y_max]，而旧的 solve(... RhombusConstraint ...) 显式包含负 x 坐标的对称多边形（见第 101 行的 -rhombus_right）。新重载无条件排除了整个左半区域。
更关键的是，deformable_omni_wheel_controller.cpp 第 196-205 行传入的 8 个 HalfPlaneConstraint 不包含任何 x >= 0 的约束（只有对 y 和交叉项的约束）。这意味着调用方期望可行域应该由这些约束动态裁定，而非被初始化时的硬编码矩形截断。
建议：
- 若
x >= 0是本意（例如电机仅工作在正向区间），请在结构体文档、函数注释或参数说明中明确记录该假设； - 若非本意，请将初始多边形改为对称的
[-x_max, x_max] × [-y_max, y_max]，与旧重载保持一致。
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp` around lines 59
- 86, The new solve(...) overload builds an initial polygon limited to x ∈ [0,
x_max] which unintentionally excludes the left half-plane and differs from the
earlier solve(... RhombusConstraint ...) behavior; either document that
BoundaryConstraint.x_max implies x≥0 or change the initial polygon to be
symmetric. To fix, in function solve(...) that takes BoundaryConstraint and
half_planes, update the initial polygon to use [-boundary_constraint.x_max,
boundary_constraint.x_max] × [-boundary_constraint.y_max,
boundary_constraint.y_max] (matching the rhombus-based overload) OR add a clear
comment/parameter doc on BoundaryConstraint and the solve(...) overload that x
is assumed non-negative so callers are aware; ensure references to polygon,
BoundaryConstraint, and the caller deformable_omni_wheel_controller expectations
are respected.
| TwoAxisGimbalSolver gimbal_solver_{ | ||
| *this, get_parameter("upper_limit").as_double(), get_parameter("lower_limit").as_double(), | ||
| true}; |
There was a problem hiding this comment.
use_encoder_pitch 被硬编码为 true，YAML 配置被静默忽略
gimbal_solver_ 直接传入字面量 true，但两份 bringup 配置里都暴露了 use_encoder_pitch 字段：
deformable-infantry.yaml：use_encoder_pitch: falsedeformable-infantry-v2.yaml：use_encoder_pitch: true
也就是说 v1 配置中希望走"非编码器 pitch"路径（pitch_link_to_yaw_link，依赖 IMU 估计的 yaw 轴在 PitchLink 下的投影），但当前实现对 v1 也强制走 pitch_link_to_yaw_link_from_encoder，会用编码器 pitch 做云台运动学解算，这与 v1 的硬件/标定假设不匹配，可能导致 v1 上线时 yaw 跟随出现可观察的偏差。
建议从参数读取，使 YAML 的语义生效；同时也修复了配置项被忽略带来的可维护性问题。
🐛 修复建议
TwoAxisGimbalSolver gimbal_solver_{
*this, get_parameter("upper_limit").as_double(), get_parameter("lower_limit").as_double(),
- true};
+ get_parameter_or<bool>("use_encoder_pitch", false)};📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| TwoAxisGimbalSolver gimbal_solver_{ | |
| *this, get_parameter("upper_limit").as_double(), get_parameter("lower_limit").as_double(), | |
| true}; | |
| TwoAxisGimbalSolver gimbal_solver_{ | |
| *this, get_parameter("upper_limit").as_double(), get_parameter("lower_limit").as_double(), | |
| get_parameter_or<bool>("use_encoder_pitch", false)}; |
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In
`@rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp`
around lines 182 - 184, gimbal_solver_ 的第三个构造参数被硬编码为 true，导致配置项
use_encoder_pitch 被忽略；改为从参数服务器读取该布尔值并传入 TwoAxisGimbalSolver 构造器（例如通过
get_parameter("use_encoder_pitch").as_bool() 或等效接口），确保 gimbal_solver_ 在构造时使用配置的
use_encoder_pitch 值而不是字面 true，从而让 deformable-infantry.yaml /
deformable-infantry-v2.yaml 的语义生效并修复不同 bringup 配置下的运动学路径差异。
| #else | ||
| # define PACKED_STRUCT(...) struct __VA_ARGS__ | ||
| #endif |
There was a problem hiding this comment.
回退分支（fallback）对硬件协议结构体而言存在静默数据损坏风险
当编译器既不是 MSVC 也不是 GCC/Clang 时，PACKED_STRUCT 会退化为普通 struct，不会应用任何对齐/填充压缩。对于用于解析硬件串口协议的结构体（如 Dr16DataPart1/2/3），布局错误将导致数据静默错误，且无任何编译期提示。
建议至少加上 #warning，或在对布局正确性有严格要求的场景下改为 #error：
🛡️ 建议修复
`#else`
-# define PACKED_STRUCT(...) struct __VA_ARGS__
+# warning "PACKED_STRUCT: unknown compiler, struct packing disabled — hardware protocol layout may be incorrect"
+# define PACKED_STRUCT(...) struct __VA_ARGS__
`#endif`📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| #else | |
| # define PACKED_STRUCT(...) struct __VA_ARGS__ | |
| #endif | |
| `#else` | |
| # warning "PACKED_STRUCT: unknown compiler, struct packing disabled — hardware protocol layout may be incorrect" | |
| # define PACKED_STRUCT(...) struct __VA_ARGS__ | |
| `#endif` |
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp` around lines 23 - 25, The
fallback for PACKED_STRUCT currently defines plain struct which can silently
break packed layouts (used by Dr16DataPart1/2/3); change the fallback branch so
it does not silently compile — either emit a compile error (`#error`) or at
minimum a visible warning (`#warning`) when neither MSVC nor GCC/Clang are
detected, and document that PACKED_STRUCT must be implemented for the target;
update the PACKED_STRUCT fallback macro and ensure developers are alerted before
the code using Dr16DataPart1/2/3 can be built on unsupported compilers.
| template <typename T> | ||
| class RingBuffer { | ||
| public: | ||
| constexpr explicit RingBuffer(size_t size) { | ||
| if (size <= 2) | ||
| size = 2; | ||
| else | ||
| size = round_up_to_next_power_of_2(size); | ||
| mask = size - 1; | ||
| storage_ = new Storage[size]; | ||
| }; | ||
|
|
||
| ~RingBuffer() { | ||
| clear(); | ||
| delete[] storage_; | ||
| } |
There was a problem hiding this comment.
违反 Rule of Five：含原始所有权指针但未禁用/实现拷贝与移动。
storage_ 是 new[] 出来的裸指针，编译器合成的拷贝构造与拷贝赋值会做浅拷贝，一旦发生拷贝就会出现双重 delete[]；而 in_/out_ 是 std::atomic<size_t>（不可拷贝/移动），实际上会让默认拷贝构造在使用点报错，但默认移动也未定义、容易在容器/参数传递中踩坑。建议显式删除拷贝、显式实现或删除移动，明确所有权语义。
♻️ 建议修复
constexpr explicit RingBuffer(size_t size) {
...
};
+
+ RingBuffer(const RingBuffer&) = delete;
+ RingBuffer& operator=(const RingBuffer&) = delete;
+ RingBuffer(RingBuffer&&) = delete;
+ RingBuffer& operator=(RingBuffer&&) = delete;Also applies to: 182-188
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@rmcs_ws/src/rmcs_core/src/hardware/utility/ring_buffer.hpp` around lines 11 -
26, The class RingBuffer currently owns a raw pointer storage_ and has
std::atomic<size_t> members in_/out_, but does not declare copy/move semantics;
explicitly delete the copy constructor and copy assignment (RingBuffer(const
RingBuffer&) = delete; RingBuffer& operator=(const RingBuffer&) = delete;) and
implement a move constructor and move assignment that transfer ownership of
storage_, mask and size and move the in_/out_ counters by reading source atomics
(e.g. src.in_.load()/src.out_.load()) into the destination and then resetting
the source in_/out_ to 0 and source.storage_ to nullptr to prevent double
delete; ensure the move assignment handles self-assignment, frees existing
storage_ before taking ownership, and leaves the moved-from object in a safe
destructible state so ~RingBuffer and clear() remain correct.
| T* back() { | ||
| auto in = in_.load(std::memory_order::acquire); | ||
|
|
||
| if (in == out_.load(std::memory_order::relaxed)) | ||
| return nullptr; | ||
| else | ||
| return std::launder(reinterpret_cast<T*>(storage_[in & mask].data)); | ||
| } |
There was a problem hiding this comment.
back() 返回的是“下一个待写槽位”，而非最新写入的元素。
in_ 语义是“下一个写入位置”，因此最近一次写入的元素位于 (in - 1) & mask，而不是 in & mask。当前实现返回的槽位要么尚未构造、要么是早已被覆盖前的旧元素的内存，调用方解引用该指针属于未定义行为。空判断逻辑（in == out 时返回 nullptr）虽然正确，但非空时返回的指针并不是“buffer 内最后一个元素”。
🐛 建议修复
T* back() {
auto in = in_.load(std::memory_order::acquire);
if (in == out_.load(std::memory_order::relaxed))
return nullptr;
else
- return std::launder(reinterpret_cast<T*>(storage_[in & mask].data));
+ return std::launder(reinterpret_cast<T*>(storage_[(in - 1) & mask].data));
}📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| T* back() { | |
| auto in = in_.load(std::memory_order::acquire); | |
| if (in == out_.load(std::memory_order::relaxed)) | |
| return nullptr; | |
| else | |
| return std::launder(reinterpret_cast<T*>(storage_[in & mask].data)); | |
| } | |
| T* back() { | |
| auto in = in_.load(std::memory_order::acquire); | |
| if (in == out_.load(std::memory_order::relaxed)) | |
| return nullptr; | |
| else | |
| return std::launder(reinterpret_cast<T*>(storage_[(in - 1) & mask].data)); | |
| } |
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@rmcs_ws/src/rmcs_core/src/hardware/utility/ring_buffer.hpp` around lines 70 -
77, The back() method currently returns storage_[in & mask] but in_ represents
the next write index, so back() must return the most recently written slot at
index (in - 1) & mask; change the logic in back() to load in_ (as currently
done), keep the empty check (in == out_.load(...)) and when non-empty compute
size_t idx = (in - 1) & mask and return
std::launder(reinterpret_cast<T*>(storage_[idx].data)); ensure you use the same
atomic loads and memory orders as before and reference in_, out_, mask and
storage_ when making the change.
| template <typename... Args> | ||
| bool emplace_back(Args&&... args) { | ||
| return emplace_back_multi( | ||
| [&](std::byte* storage) { new (storage) T{std::forward<Args...>(args...)}; }, 1); | ||
| } |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
rg -nP -C2 '\bemplace_back\s*\(' --type=cppRepository: Alliance-Algorithm/RMCS
Length of output: 8116
🏁 Script executed:
# Check the exact content of the problematic line and surrounding context
cat -n rmcs_ws/src/rmcs_core/src/hardware/utility/ring_buffer.hpp | sed -n '105,115p'Repository: Alliance-Algorithm/RMCS
Length of output: 582
🏁 Script executed:
# Compare with the correct implementation in rmcs_utility
cat -n rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp | sed -n '148,155p'Repository: Alliance-Algorithm/RMCS
Length of output: 414
修复 emplace_back 中模板参数包展开错误，否则多参数和零参数调用会编译失败
std::forward<Args...>(args...) 中，Args... 在模板实参列表内展开，导致 std::forward 收到多个或零个模板参数，违反其签名要求。仓库中的 rmcs_utility 已采用正确形式。
修复方案
template <typename... Args>
bool emplace_back(Args&&... args) {
return emplace_back_multi(
- [&](std::byte* storage) { new (storage) T{std::forward<Args...>(args...)}; }, 1);
+ [&](std::byte* storage) { new (storage) T{std::forward<Args>(args)...}; }, 1);
}🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@rmcs_ws/src/rmcs_core/src/hardware/utility/ring_buffer.hpp` around lines 106
- 110, The emplace_back template incorrectly uses std::forward<Args...>(args...)
causing parameter-pack expansion errors; in the emplace_back implementation
(which calls emplace_back_multi with a lambda constructing T), change the
placement-new call to use perfect forwarding per-parameter: new (storage)
T{std::forward<Args>(args)...}; keep the rest (emplace_back_multi, T, Args,
args) unchanged so zero- and multi-argument constructions compile correctly.
| constexpr static size_t round_up_to_next_power_of_2(size_t n) { | ||
| n--; | ||
| n |= n >> 1; | ||
| n |= n >> 2; | ||
| n |= n >> 4; | ||
| n |= n >> 8; | ||
| n |= n >> 16; | ||
| n |= n >> 32; | ||
| n++; | ||
| return n; | ||
| } |
There was a problem hiding this comment.
🧩 Analysis chain
🌐 Web query:
C++ standard expr.shift undefined behavior shift width
💡 Result:
In C++, using the built-in shift operators (<< and >>) with an invalid shift amount is undefined behavior. Specifically, for an expression like E1 << E2 or E1 >> E2, the behavior is undefined if E2 is negative, or if E2 is greater than or equal to the width (in bits) of the promoted left operand [1][2][3]. In addition, for right-shift the standard also distinguishes cases based on the value/sign of the left operand: - If E1 is unsigned (or E1 is signed but non-negative), the result is well-defined per the quotient rule [2]. - If E1 is signed and negative, the result of right shift is implementation-defined (commonly arithmetic shift) [2]. So “shift width” UB typically refers to the “shift count is >= bit width of promoted left operand” rule, which is undefined behavior [1][3]. Sources also summarize the same rule on a dedicated operator page: “The behavior is undefined if rhs is negative or is greater or equal the number of bits in the promoted lhs” [3].
Citations:
- 1: https://timsong-cpp.github.io/cppwp/expr.shift
- 2: https://www.ece.uvic.ca/~frodo/cppdraft/n4659/html/expr.shift
- 3: https://dev.cppreference.com/c/language/operator_arithmetic
🏁 Script executed:
fd -t f "ring_buffer.hpp" -x cat {}Repository: Alliance-Algorithm/RMCS
Length of output: 14642
🏁 Script executed:
fd -t f "ring_buffer.hpp" -x head -50 {}Repository: Alliance-Algorithm/RMCS
Length of output: 2683
n >> 32 在 32 位平台上是未定义行为。
size_t 在 32 位目标上为 32 位时，移位量等于或超过类型宽度属于 UB（C++ [expr.shift]/1）。当前 RMCS 主要面向 64 位目标，影响较小，但该 utility 是 header-only、可能被复用，建议修复避免跨架构构建时踩坑。
推荐修复方案（无需额外 includes）：
♻️ 建议修复
constexpr static size_t round_up_to_next_power_of_2(size_t n) {
n--;
n |= n >> 1;
n |= n >> 2;
n |= n >> 4;
n |= n >> 8;
n |= n >> 16;
- n |= n >> 32;
+ if constexpr (sizeof(size_t) > 4)
+ n |= n >> 32;
n++;
return n;
}🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@rmcs_ws/src/rmcs_core/src/hardware/utility/ring_buffer.hpp` around lines 170
- 180, The function round_up_to_next_power_of_2 uses a hardcoded n >> 32 which
is UB on 32-bit targets; replace the fixed-shift sequence with a safe, portable
loop that performs successive right-shifts up to the bitwidth of size_t (e.g.,
iterate shift = 1; shift < sizeof(size_t)*8; shift <<= 1 and apply n |= n >>
shift) so the code adapts to 32/64-bit platforms; update the body of
round_up_to_next_power_of_2 accordingly while keeping it constexpr and
header-only.
6 participants
涵盖v1 v2 3车通用
Deformable 专属 (14 文件) — 底盘/云台/硬件/UI 全套 deformable 控制器与配置
依赖组件 (10 文件）— ADRC、SMC 控制器，ring_buffer、hard_sync 等辅助
共享修改 —
plugins.xml注册、裁判系统协议修复、cross-build `--link-default 软链接等