Skip to content

dev: migrate sentry#56

Open
creeper5820 wants to merge 13 commits intomainfrom
dev/migrate-sentry
Open

dev: migrate sentry#56
creeper5820 wants to merge 13 commits intomainfrom
dev/migrate-sentry

Conversation

@creeper5820
Copy link
Contributor

@creeper5820 creeper5820 commented Mar 15, 2026

哨兵机器人迁移(dev/migrate-sentry)

核心功能

  • 新增 Sentry 硬件组件 rmcs_core::hardware::Sentry,实现于 rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp。该组件通过内嵌 TopBoard / BottomBoard 管理云台(俯仰/偏航)、底盘(轮毂/转向)、IMU、CAN/UART 电机控制器(LkMotor、DjiMotor)和裁判 UART,并提供状态 Trigger 服务。组件已在 rmcs_core/plugins.xml 中导出。

导航与控制器集成

  • 底盘控制器(chassis_controller.cpp)
    • 增加 navigation_command_velocity 输入、navigation_velocity_scale 参数(默认 1.0)。
    • 将导航速度按云台偏航旋转后融合进平移速度计算。
    • 若输入不可用则回退为 NaN,调整部分常量与实现细节(包含角速度上限修改、头文件调整)。
  • 简化云台控制器(simple_gimbal_controller.cpp)
    • 新增 /rmcs_navigation/command_velocity 输入,若有效则按 control_dt_(1e-3)将偏航增量叠加。
  • 双偏航控制器(dual_yaw_controller.cpp)
    • 移除内部 DualYawStatus 组件。
    • 新增并导出 /gimbal/yaw/angle 与 /gimbal/yaw/velocity 输出,运行时计算并归一化 yaw 角与速度。
    • 修改控制角度输出绑定与 PID/Torque 计算流程;当 chassis 输出不可用或角度 NaN 时重置 PID 状态并将控制量置为 NaN。
  • 方向轮控制器(steering_wheel_controller.cpp)
    • 用参数驱动的值替代硬编码 PID 初始值(chassis_translation_kp/ki/kd)。

数值与实现改进

  • two_axis_gimbal_solver.hpp:改用容差检查代替精确零比较,改为就地归一化以减少临时分配。
  • 多处添加或包含 、使用 std::remainder 等数值稳定性与表达式简化调整。

硬件接口增强

  • DjiMotor 新增公有访问器 int last_raw_angle() const。
  • LkMotor 新增公有访问器 int64_t last_raw_angle() const。

配置与依赖

  • 新增配置文件 rmcs_ws/src/rmcs_bringup/config/sentry.yaml(约 145 行),包含 rmcs_executor 更新率、组件映射及详尽的 ros__parameters(电机零点、PID 参数、限幅、摩擦轮/加热/供弹等)。
  • rmcs_ws/src/rmcs_core/package.xml:新增依赖 std_srvs。
  • Dockerfile:安装 ros-$ROS_DISTRO-navigation2 并在 LLVM 安装中加入 libomp-${LLVM_VERSION}-dev。

脚本与补全

  • 新增脚本:
    • .script/save-map-once:支持 remote/local 模式,通过 ros2 service call /save_pcd_map 触发地图保存。
    • .script/remote-status:通过 ssh-remote 在远端执行环境脚本并调用 /rmcs/service/status 获取状态。
    • .script/complete/_save-map-once:为 save-map-once 提供 zsh 补全(remote/local)。

子模块与样式

  • 从 .gitmodules 与工作树中移除 rmcs_ws/src/hikcamera 子模块引用(其 commit 引用已删除)。
  • .clang-format:通过注释停用 AlignArrayOfStructures: Right 规则(未删除该行,仅注释)。

变更量与审查建议

  • 最大的新增为 sentry.cpp(大量新增实现),dual_yaw_controller 与 chassis_controller 也有较大改动;总体审查工作量中等偏高。
  • 注意点:新导出的组件/outputs(Sentry 类与 dual_yaw 的 yaw 输出)与新增参数/服务依赖(std_srvs、/save_pcd_map、/rmcs/service/status)可能需要在部署/bringup 测试中验证互操作性与参数正确性。

@coderabbitai
Copy link

coderabbitai bot commented Mar 15, 2026

Walkthrough

新增 rmcs_core 的 Sentry 硬件组件实现并注册为插件;为底盘与云台控制器接入导航速度输入与若干参数化改动;移除 hikcamera 子模块;新增多项运维脚本与 zsh 完成;添加大量 Sentry 启动配置与少量设备访问器。

Changes

Cohort / File(s) Summary
构建与仓库元数据
/.clang-format, Dockerfile, /.gitmodules
禁用 .clang-format 中的 AlignArrayOfStructures 规则;Dockerfile 中加入 navigation2 与 LLVM OpenMP 开发库;从 .gitmodules 中移除 rmcs_ws/src/hikcamera 子模块条目。
运维脚本与补全
.script/remote-status, .script/save-map-once, .script/complete/_save-map-once
新增远程状态查询脚本与 /save_pcd_map 触发脚本(支持 local/remote),并添加对应 zsh 完成脚本。
Sentry 硬件组件及注册
rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp, rmcs_ws/src/rmcs_core/plugins.xml, rmcs_ws/src/rmcs_core/package.xml
新增 rmcs_core::hardware::Sentry 组件实现(包含 TopBoard/BottomBoard、CAN/UART、电机/IMU 驱动、TF 发布与状态服务);在 plugins.xml 注册该组件并在 package.xml 添加 std_srvs 依赖。
Sentry 配置
rmcs_ws/src/rmcs_bringup/config/sentry.yaml
新增 Sentry 启动参数文件,定义组件映射与大量硬件/控制器参数(电机零点、PID、限位、摩擦轮、热管理等)。
底盘控制器变更
rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp, rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_controller.cpp
底盘控制器增加 /rmcs_navigation/command_velocity 输入与 navigation_velocity_scale 参数,按云台偏航将导航速度转换到底盘坐标并融合;转向轮 PID 改为参数驱动初始化,若干常量与包含头文件调整。
云台控制器与求解器改动
rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp, rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp, rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp
DualYawController:移除内部 Status 组件,新增 yaw_angle/yaw_velocity 输出,改进角度处理(使用 std::remainder)并在不可用时重置 PID;SimpleGimbalController 接入导航速度输入并引入固定 control_dt;求解器改用容差判零与就地归一化。
硬件设备接口小改动
rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp, rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp
为 DjiMotor 与 LkMotor 添加公共访问器 last_raw_angle()

Sequence Diagram(s)

sequenceDiagram
  autonumber
  participant User as 用户/脚本
  participant SSH as ssh-remote
  participant ROS as ROS2 (节点/服务)
  participant Sentry as Sentry Component
  participant Top as TopBoard
  participant Bottom as BottomBoard
  participant HW as Motors/IMU/CAN/UART

  User->>ROS: 调用本地服务或发布导航命令
  alt 使用远端模式
    User->>SSH: 通过 ssh-remote 执行远端服务调用
    SSH->>ROS: 在远端环境调用 ROS2 服务
  end
  ROS->>Sentry: 下发控制命令 / 请求状态
  Sentry->>Top: 读取云台传感器,发送云台命令
  Sentry->>Bottom: 读取底盘传感器,发送底盘命令
  Top->>HW: CAN/UART 交互(电机、IMU、TF)
  Bottom->>HW: CAN/UART 交互(电机、转向、裁判接口)
  HW-->>Sentry: 传感器与电机状态反馈
  Sentry-->>ROS: 发布状态输出(含 yaw_angle/yaw_velocity)或服务响应
Loading

Estimated code review effort

🎯 5 (Critical) | ⏱️ ~120 minutes

Poem

🐰 新硬件登场铸齿轮,Top Bottom 同步唱和声,
导航速度随云台转,脚本遥控一键成,
电机脉动与参数行,代码花园又一新,
小兔轻跳庆上线,祝合并顺利又平稳。

🚥 Pre-merge checks | ✅ 2 | ❌ 1

❌ Failed checks (1 warning)

Check name Status Explanation Resolution
Docstring Coverage ⚠️ Warning Docstring coverage is 0.00% which is insufficient. The required threshold is 80.00%. Write docstrings for the functions missing them to satisfy the coverage threshold.
✅ Passed checks (2 passed)
Check name Status Explanation
Description Check ✅ Passed Check skipped - CodeRabbit’s high-level summary is enabled.
Title check ✅ Passed The pull request title 'dev: migrate sentry' directly and accurately describes the main objective - migrating the Sentry robot implementation. The extensive changes include adding a new Sentry hardware component, related configuration files, navigation integration, and dependencies, all of which align with this migration theme.

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

✨ Finishing Touches
📝 Generate docstrings
  • Create stacked PR
  • Commit on current branch
🧪 Generate unit tests (beta)
  • Create PR with unit tests
  • Commit unit tests in branch dev/migrate-sentry
📝 Coding Plan
  • Generate coding plan for human review comments

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.

Tip

You can customize the tone of the review comments and chat replies.

Configure the tone_instructions setting to customize the tone of the review comments and chat replies. For example, you can set the tone to Act like a strict teacher, Act like a pirate and more.

Copy link

@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: 5

🧹 Nitpick comments (3)
rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp (1)

138-139: 潜在的零向量归一化风险。

yaw_axis->z()yaw_axis->x() 同时为零时(即轴与 Y 方向对齐),pitch 向量长度为零,调用 normalize() 会导致未定义行为(除以零)。

虽然 yaw_axis_filtered_ 作为单位向量持续维护(第 128 行),在正常云台操作中此风险较低,但考虑添加防御性检查可提高鲁棒性。

🛡️ 可选的防御性检查
         pitch = {yaw_axis->z(), yaw_axis->x()};
+        if (pitch.squaredNorm() < 1e-12) {
+            // Handle degenerate case where yaw axis is aligned with Y
+            pitch = {1.0, 0.0};
+        }
         pitch.normalize();
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp`
around lines 138 - 139, The code constructs pitch = {yaw_axis->z(),
yaw_axis->x()} then calls pitch.normalize() which can divide by zero if that
vector is (near) zero; add a defensive check in the block that builds pitch
(referencing pitch, yaw_axis and yaw_axis_filtered_) to compute the vector norm
and only call normalize() when norm > epsilon (e.g. 1e-8); if the norm is <=
epsilon, set pitch to a safe fallback (for example reuse a previous valid pitch,
or compute a different orthogonal vector such as a unit vector along a canonical
axis) and normalize that fallback, and optionally emit a debug/warning log so
the rare case is visible.
.script/remote-status (1)

1-5: 脚本结构良好,但复杂的 sed 表达式可考虑添加注释

脚本正确使用了 set -euo pipefail 进行严格错误处理。第 5 行的嵌套引号和 sed 模式较复杂,建议添加简短注释说明其用途,以便后续维护。

♻️ 可选:添加注释提升可读性
 #!/bin/zsh
 
 set -euo pipefail
 
+# 远程调用 /rmcs/service/status 服务,使用 sed 提取 message='...' 中的内容
 ssh-remote "bash -lc 'source ~/env_setup.bash && msg=\$(ros2 service call /rmcs/service/status std_srvs/srv/Trigger \"{}\" | sed -n \"s/.*message='\\''\\(.*\\)'\\'')/\\1/p\") && printf \"%b\\n\" \"\$msg\"'"
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In @.script/remote-status around lines 1 - 5, 在第 5 行包含复杂嵌套引号和 sed 表达式的
ssh-remote 命令(ssh-remote "bash -lc 'source ~/env_setup.bash && msg=$(ros2
service call /rmcs/service/status std_srvs/srv/Trigger \"{}\" | sed -n
\"s/.*message='\\''\\(.*\\)'\\'')/\\1/p\") && printf \"%b\\n\" \"$msg\"'")
上方或行内添加一行简短注释,说明该 sed 表达式的目标(从 ros2 服务调用的输出中提取 message
字段并去除外层引号)以及为何需要双重/转义引号,便于后续维护与调试;保留现有命令不变,仅增注释以提高可读性。
rmcs_ws/src/rmcs_bringup/config/sentry.yaml (1)

33-34: 建议将板卡序列号改为设备级覆盖配置

Line 33-34 硬编码具体序列号会把该配置绑定到单台设备。建议基础配置保留占位值,并通过 launch 参数或机体专用 overlay 注入真实序列号。

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

In `@rmcs_ws/src/rmcs_bringup/config/sentry.yaml` around lines 33 - 34, The
current config hardcodes device-specific serials in the keys
board_serial_top_board and board_serial_bottom_board; change these to
placeholder values (e.g., empty strings or "UNKNOWN") in the base sentry.yaml
and remove the hardcoded "d4-1d2b" and "d4-041d" values, then update the
device-specific launch/overlay mechanism to inject the real serials via launch
arguments or the machine-specific overlay so each device gets its own
board_serial_top_board and board_serial_bottom_board at runtime.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.

Inline comments:
In `@rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp`:
- Around line 143-154: The code currently clamps only translational_velocity but
then adds nav_velocity (computed from navigation_command_velocity_ and
navigation_velocity_scale_) which can push the result over
translational_velocity_max; update the return path (after computing nav_velocity
and before returning translational_velocity + nav_velocity) to enforce the
translational magnitude limit by computing the combined vector
(translational_velocity + nav_velocity) and, if its norm exceeds
translational_velocity_max, scale it down to translational_velocity_max (e.g.,
combined *= translational_velocity_max / combined.norm()); use the existing
symbols translational_velocity, nav_velocity (and nav_velocity_odom),
navigation_command_velocity_, navigation_velocity_scale_, gimbal_yaw_angle_, and
translational_velocity_max to locate and implement the clamp.

In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp`:
- Around line 84-89: The current normalization of yaw_angle computed from
*top_yaw_angle_ + *bottom_yaw_angle_ only subtracts/adds 2π once and can still
be out of range for multi-turn inputs; update the normalization logic around the
local variable yaw_angle (computed from top_yaw_angle_ and bottom_yaw_angle_) so
it repeatedly wraps into [0, 2π) (or uses std::fmod with corrective
adds/subtracts) until yaw_angle is within bounds, then assign the normalized
value to *yaw_angle_; ensure you modify the block that reads top_yaw_angle_,
bottom_yaw_angle_ and writes yaw_angle_ so all multi-revolution inputs are
correctly folded into the target interval.

In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp`:
- Around line 74-80: The code multiplies navigation yaw_speed by a hardcoded 1
ms instead of the real controller timestep, causing incorrect yaw_shift scaling
when the executor update_rate differs; update the navigation integration to use
the actual control timestep variable (control_dt_) or accept dt from
rmcs_executor::Executor and replace the fixed 1e-3 multiplier where
navigation_command_velocity_, yaw_speed, and yaw_shift are updated (also update
the similar occurrence noted around the second usage) so yaw_shift += yaw_speed
* actual_dt.

In `@rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp`:
- Around line 82-90: 当前 ROS 回调与硬件更新在不同线程并发访问 TopBoard/BottomBoard
导致数据竞争(相关符号:Sentry::update、SentryCommand::update、command_update、TopBoard/BottomBoard,以及成员方法
last_raw_angle(), calibrate_zero_point(), update_status(),
generate_command());请将所有会读写 board 的 ROS callbacks/service handlers
改为“投递请求到更新线程”或在 Sentry 内用一个单一互斥锁保护对 top_board_ 和 bottom_board_ 的所有访问(包含
update(), command_update() 与回调处理路径),确保回调只 enqueue 请求结构并返回,由更新线程在
Sentry::update()/SentryCommand::update() 中串行消费并调用相应的 board 方法,从而消除数据竞态与校准穿插问题。
- Around line 1-10: The file is missing headers causing compile failures: add
`#include` <ostream> to provide the std::ostream overloads used by std::println in
the code that writes into std::ostringstream (the block around the
feedback_message usage) and add `#include` <array> to cover the std::array usage
at/near line 114; alternatively, replace the std::println(...) into the
ostringstream with a std::format(...) + ostringstream.write if you prefer not to
add headers—locate the code that constructs feedback_message and the std::array
instance to apply the fix.

---

Nitpick comments:
In @.script/remote-status:
- Around line 1-5: 在第 5 行包含复杂嵌套引号和 sed 表达式的 ssh-remote 命令(ssh-remote "bash -lc
'source ~/env_setup.bash && msg=$(ros2 service call /rmcs/service/status
std_srvs/srv/Trigger \"{}\" | sed -n \"s/.*message='\\''\\(.*\\)'\\'')/\\1/p\")
&& printf \"%b\\n\" \"$msg\"'") 上方或行内添加一行简短注释,说明该 sed 表达式的目标(从 ros2 服务调用的输出中提取
message 字段并去除外层引号)以及为何需要双重/转义引号,便于后续维护与调试;保留现有命令不变,仅增注释以提高可读性。

In `@rmcs_ws/src/rmcs_bringup/config/sentry.yaml`:
- Around line 33-34: The current config hardcodes device-specific serials in the
keys board_serial_top_board and board_serial_bottom_board; change these to
placeholder values (e.g., empty strings or "UNKNOWN") in the base sentry.yaml
and remove the hardcoded "d4-1d2b" and "d4-041d" values, then update the
device-specific launch/overlay mechanism to inject the real serials via launch
arguments or the machine-specific overlay so each device gets its own
board_serial_top_board and board_serial_bottom_board at runtime.

In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp`:
- Around line 138-139: The code constructs pitch = {yaw_axis->z(),
yaw_axis->x()} then calls pitch.normalize() which can divide by zero if that
vector is (near) zero; add a defensive check in the block that builds pitch
(referencing pitch, yaw_axis and yaw_axis_filtered_) to compute the vector norm
and only call normalize() when norm > epsilon (e.g. 1e-8); if the norm is <=
epsilon, set pitch to a safe fallback (for example reuse a previous valid pitch,
or compute a different orthogonal vector such as a unit vector along a canonical
axis) and normalize that fallback, and optionally emit a debug/warning log so
the rare case is visible.

ℹ️ Review info
⚙️ Run configuration

Configuration used: Organization UI

Review profile: CHILL

Plan: Pro

Run ID: 5ef8bfb0-d456-4f8e-88a9-4827652f1f19

📥 Commits

Reviewing files that changed from the base of the PR and between a5f2820 and 4f6da7b.

📒 Files selected for processing (18)
  • .clang-format
  • .gitmodules
  • .script/complete/_save-map-once
  • .script/remote-status
  • .script/save-map-once
  • Dockerfile
  • rmcs_ws/src/hikcamera
  • rmcs_ws/src/rmcs_bringup/config/sentry.yaml
  • rmcs_ws/src/rmcs_core/package.xml
  • rmcs_ws/src/rmcs_core/plugins.xml
  • rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp
  • rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_controller.cpp
  • rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp
  • rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp
  • rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp
  • rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp
  • rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp
  • rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp
💤 Files with no reviewable changes (2)
  • .gitmodules
  • rmcs_ws/src/hikcamera

Comment on lines +143 to +154
// Navigation Control
auto nav_velocity_odom = Eigen::Vector2d{Eigen::Vector2d::Zero()};
if (navigation_command_velocity_.ready()) {
auto raw_command = *navigation_command_velocity_;
if (std::isfinite(raw_command.x()) && std::isfinite(raw_command.y())) {
nav_velocity_odom.x() = raw_command.x() * navigation_velocity_scale_;
nav_velocity_odom.y() = raw_command.y() * navigation_velocity_scale_;
}
}
auto nav_velocity = Eigen::Rotation2Dd{*gimbal_yaw_angle_} * nav_velocity_odom;

return translational_velocity + nav_velocity;
Copy link

Choose a reason for hiding this comment

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

⚠️ Potential issue | 🟠 Major

导航速度叠加后要重新限幅。

这里先把手动输入限到 translational_velocity_max,再直接叠加导航速度并返回。手动和导航同时生效时,最终输出可以明显超过 10.0,直接绕过这里定义的最大平移速度。

🔧 建议修改
-        auto nav_velocity = Eigen::Rotation2Dd{*gimbal_yaw_angle_} * nav_velocity_odom;
-
-        return translational_velocity + nav_velocity;
+        translational_velocity += Eigen::Rotation2Dd{*gimbal_yaw_angle_} * nav_velocity_odom;
+        if (const auto norm = translational_velocity.norm();
+            norm > translational_velocity_max) {
+            translational_velocity *= translational_velocity_max / norm;
+        }
+        return translational_velocity;
📝 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.

Suggested change
// Navigation Control
auto nav_velocity_odom = Eigen::Vector2d{Eigen::Vector2d::Zero()};
if (navigation_command_velocity_.ready()) {
auto raw_command = *navigation_command_velocity_;
if (std::isfinite(raw_command.x()) && std::isfinite(raw_command.y())) {
nav_velocity_odom.x() = raw_command.x() * navigation_velocity_scale_;
nav_velocity_odom.y() = raw_command.y() * navigation_velocity_scale_;
}
}
auto nav_velocity = Eigen::Rotation2Dd{*gimbal_yaw_angle_} * nav_velocity_odom;
return translational_velocity + nav_velocity;
// Navigation Control
auto nav_velocity_odom = Eigen::Vector2d{Eigen::Vector2d::Zero()};
if (navigation_command_velocity_.ready()) {
auto raw_command = *navigation_command_velocity_;
if (std::isfinite(raw_command.x()) && std::isfinite(raw_command.y())) {
nav_velocity_odom.x() = raw_command.x() * navigation_velocity_scale_;
nav_velocity_odom.y() = raw_command.y() * navigation_velocity_scale_;
}
}
translational_velocity += Eigen::Rotation2Dd{*gimbal_yaw_angle_} * nav_velocity_odom;
if (const auto norm = translational_velocity.norm();
norm > translational_velocity_max) {
translational_velocity *= translational_velocity_max / norm;
}
return translational_velocity;
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp` around
lines 143 - 154, The code currently clamps only translational_velocity but then
adds nav_velocity (computed from navigation_command_velocity_ and
navigation_velocity_scale_) which can push the result over
translational_velocity_max; update the return path (after computing nav_velocity
and before returning translational_velocity + nav_velocity) to enforce the
translational magnitude limit by computing the combined vector
(translational_velocity + nav_velocity) and, if its norm exceeds
translational_velocity_max, scale it down to translational_velocity_max (e.g.,
combined *= translational_velocity_max / combined.norm()); use the existing
symbols translational_velocity, nav_velocity (and nav_velocity_odom),
navigation_command_velocity_, navigation_velocity_scale_, gimbal_yaw_angle_, and
translational_velocity_max to locate and implement the clamp.

Comment on lines +84 to +89
double yaw_angle = *top_yaw_angle_ + *bottom_yaw_angle_;
if (yaw_angle < 0)
yaw_angle += 2 * std::numbers::pi;
else if (yaw_angle > 2 * std::numbers::pi)
yaw_angle -= 2 * std::numbers::pi;
*yaw_angle_ = yaw_angle;
Copy link

Choose a reason for hiding this comment

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

⚠️ Potential issue | 🟡 Minor

角度归一化仅处理一次环绕,存在越界边界情况

在 Line 84-88,当前只做一次 ±2π 修正;当输入和超过一圈(例如 > 4π< -2π)时,yaw_angle 仍可能不在目标区间内,导致下游姿态使用异常。

建议修复(支持多圈输入)
-        double yaw_angle = *top_yaw_angle_ + *bottom_yaw_angle_;
-        if (yaw_angle < 0)
-            yaw_angle += 2 * std::numbers::pi;
-        else if (yaw_angle > 2 * std::numbers::pi)
-            yaw_angle -= 2 * std::numbers::pi;
+        double yaw_angle = *top_yaw_angle_ + *bottom_yaw_angle_;
+        while (yaw_angle < 0)
+            yaw_angle += 2 * std::numbers::pi;
+        while (yaw_angle > 2 * std::numbers::pi)
+            yaw_angle -= 2 * std::numbers::pi;
         *yaw_angle_ = yaw_angle;
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp` around
lines 84 - 89, The current normalization of yaw_angle computed from
*top_yaw_angle_ + *bottom_yaw_angle_ only subtracts/adds 2π once and can still
be out of range for multi-turn inputs; update the normalization logic around the
local variable yaw_angle (computed from top_yaw_angle_ and bottom_yaw_angle_) so
it repeatedly wraps into [0, 2π) (or uses std::fmod with corrective
adds/subtracts) until yaw_angle is within bounds, then assign the normalized
value to *yaw_angle_; ensure you modify the block that reads top_yaw_angle_,
bottom_yaw_angle_ and writes yaw_angle_ so all multi-revolution inputs are
correctly folded into the target interval.

Comment on lines +74 to +80
// Navigation Control
if (navigation_command_velocity_.ready()) {
auto yaw_speed = navigation_command_velocity_->z();
if (std::isfinite(yaw_speed)) {
yaw_shift += yaw_speed * control_dt_;
}
}
Copy link

Choose a reason for hiding this comment

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

⚠️ Potential issue | 🟠 Major

不要把导航 yaw 的积分步长写死成 1 ms。

yaw_speed 在这里是速度,但 Line 78 把它乘上了固定的 1e-3rmcs_executor::Executor 的循环周期来自 update_rate 参数,不是固定 1000 Hz;只要实际更新频率变化,这里的导航 yaw 增量就会系统性偏大或偏小。请改成使用真实更新周期,或者把 dt 显式从执行器传进来。

Also applies to: 88-88

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

In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp`
around lines 74 - 80, The code multiplies navigation yaw_speed by a hardcoded 1
ms instead of the real controller timestep, causing incorrect yaw_shift scaling
when the executor update_rate differs; update the navigation integration to use
the actual control timestep variable (control_dt_) or accept dt from
rmcs_executor::Executor and replace the fixed 1e-3 multiplier where
navigation_command_velocity_, yaw_speed, and yaw_shift are updated (also update
the similar occurrence noted around the second usage) so yaw_shift += yaw_speed
* actual_dt.

Comment on lines +1 to +10
#include <cstddef>
#include <format>
#include <memory>
#include <print>
#include <ranges>
#include <span>
#include <sstream>
#include <string_view>
#include <tuple>
#include <utility>
Copy link

Choose a reason for hiding this comment

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

⚠️ Potential issue | 🔴 Critical

🧩 Analysis chain

🌐 Web query:

Does C++23 <print> provide std::println overloads for std::ostringstream/std::basic_ostream, or only for stdout and FILE*?

💡 Result:

C++23 provides both kinds of std::println overloads, but they’re split across headers:

  • <print> declares the std::println overloads that write to stdout (no stream parameter) and to a FILE* (first parameter is FILE*). [1]
  • The overloads that write to a C++ iostream are declared in <ostream>: std::println(std::ostream& os, /*format*/, args...) (and similarly for wide output). This works with std::ostringstream too, since it’s an std::ostream. [2]

So: not only stdout/FILE*; there is an std::ostream& overload—but it’s in <ostream>, not <print>. [1][2]


🏁 Script executed:

# First, let's find and examine the sentry.cpp file
find . -name "sentry.cpp" -type f

Repository: Alliance-Algorithm/RMCS

Length of output: 114


🏁 Script executed:

# Let's check the content of the file at lines 1-130 to see includes and the problematic code
cat -n rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | head -130

Repository: Alliance-Algorithm/RMCS

Length of output: 5674


缺少必要的头文件导致编译失败。

代码在第 99-117 行使用 std::println(feedback_message, ...) 将内容写入 std::ostringstream,但 <print> 头文件只提供写入 stdout 和 FILE* 的重载。对 std::ostream 的重载声明在 <ostream> 头文件中,需要添加 #include <ostream>

此外,第 114 行使用 std::array 但文件未包含 <array> 头文件。

建议添加 #include <ostream>#include <array>,或改用 std::format() 结合 ostringstream 的方式。

🧰 Tools
🪛 Clang (14.0.6)

[error] 1-1: 'cstddef' file not found

(clang-diagnostic-error)

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

In `@rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp` around lines 1 - 10, The file
is missing headers causing compile failures: add `#include` <ostream> to provide
the std::ostream overloads used by std::println in the code that writes into
std::ostringstream (the block around the feedback_message usage) and add
`#include` <array> to cover the std::array usage at/near line 114; alternatively,
replace the std::println(...) into the ostringstream with a std::format(...) +
ostringstream.write if you prefer not to add headers—locate the code that
constructs feedback_message and the std::array instance to apply the fix.

Comment on lines +82 to +90
void update() override {
top_board_->update();
bottom_board_->update();
}

void command_update() {
top_board_->command_update();
bottom_board_->command_update();
}
Copy link

Choose a reason for hiding this comment

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

⚠️ Potential issue | 🔴 Critical

把 ROS 回调和硬件更新串行化。

这些 service/subscription 回调直接读写 TopBoard / BottomBoard 里的电机对象,而 rmcs_executor::Executor 会在独立线程里调用 Sentry::update()SentryCommand::update() 也会继续触发 command_update()。现在 last_raw_angle()calibrate_zero_point()update_status()generate_command() 都可能并发访问同一批状态,属于未同步的数据竞争;校准请求还可能正好插进一次控制帧中间。至少要把回调改成“投递请求,由更新线程处理”,或者用统一的互斥锁把 board 访问串行化。

Also applies to: 93-148, 150-158

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

In `@rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp` around lines 82 - 90, 当前 ROS
回调与硬件更新在不同线程并发访问 TopBoard/BottomBoard
导致数据竞争(相关符号:Sentry::update、SentryCommand::update、command_update、TopBoard/BottomBoard,以及成员方法
last_raw_angle(), calibrate_zero_point(), update_status(),
generate_command());请将所有会读写 board 的 ROS callbacks/service handlers
改为“投递请求到更新线程”或在 Sentry 内用一个单一互斥锁保护对 top_board_ 和 bottom_board_ 的所有访问(包含
update(), command_update() 与回调处理路径),确保回调只 enqueue 请求结构并返回,由更新线程在
Sentry::update()/SentryCommand::update() 中串行消费并调用相应的 board 方法,从而消除数据竞态与校准穿插问题。

@creeper5820 creeper5820 requested a review from qzhhhi March 15, 2026 05:42
Copy link

@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: 1

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

Inline comments:
In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp`:
- Around line 68-81: The top yaw loop uses
top_yaw_angle_pid_.update(-true_angle) directly as the velocity-PID input,
missing real velocity feedback; change the velocity input to (angle_setpoint -
actual_velocity) similar to bottom yaw by calling the IMU-derived velocity
(e.g., top_yaw_velocity_imu()) — compute auto angle_setpoint =
top_yaw_angle_pid_.update(-true_angle), then pass angle_setpoint -
top_yaw_velocity_imu() into top_yaw_velocity_pid_.update(...) and assign the
resulting torque to top_yaw_control_torque_.

ℹ️ Review info
⚙️ Run configuration

Configuration used: Organization UI

Review profile: CHILL

Plan: Pro

Run ID: a0b39e10-1546-4132-b2f9-8212a885f19e

📥 Commits

Reviewing files that changed from the base of the PR and between 4f6da7b and d998808.

📒 Files selected for processing (2)
  • rmcs_ws/src/rmcs_bringup/config/sentry.yaml
  • rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp
🚧 Files skipped from review as they are similar to previous changes (1)
  • rmcs_ws/src/rmcs_bringup/config/sentry.yaml

Comment on lines +68 to +81

/// @FIXME:
/// The implement of dual yaw controlling is not completed
/// Let it stable as a common gimbal
{
auto true_angle = *top_yaw_angle_;
if (true_angle > std::numbers::pi)
true_angle -= 2 * std::numbers::pi;

const auto velocity = top_yaw_angle_pid_.update(-true_angle);
const auto torque = top_yaw_velocity_pid_.update(velocity);

*top_yaw_control_torque_ = torque;
}
Copy link

Choose a reason for hiding this comment

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

⚠️ Potential issue | 🟠 Major

Top yaw 速度环缺少实际速度反馈

对比第 83-85 行的 bottom yaw 控制实现,它使用 pid_angle.update(error) - bottom_yaw_velocity_imu() 作为速度环输入,即速度设定值减去实际速度。

但 top yaw 在第 77-78 行直接将角度 PID 输出作为速度 PID 输入,缺少实际速度反馈。这会导致速度环实际上是开环工作的,无法有效抑制扰动。

建议修复:添加速度反馈
             {
                 auto true_angle = *top_yaw_angle_;
                 if (true_angle > std::numbers::pi)
                     true_angle -= 2 * std::numbers::pi;

-                const auto velocity = top_yaw_angle_pid_.update(-true_angle);
-                const auto torque = top_yaw_velocity_pid_.update(velocity);
+                const auto velocity_setpoint = top_yaw_angle_pid_.update(-true_angle);
+                // 使用 IMU 或编码器速度反馈
+                const auto torque = top_yaw_velocity_pid_.update(
+                    velocity_setpoint - *gimbal_yaw_velocity_imu_);

                 *top_yaw_control_torque_ = torque;
             }
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp` around
lines 68 - 81, The top yaw loop uses top_yaw_angle_pid_.update(-true_angle)
directly as the velocity-PID input, missing real velocity feedback; change the
velocity input to (angle_setpoint - actual_velocity) similar to bottom yaw by
calling the IMU-derived velocity (e.g., top_yaw_velocity_imu()) — compute auto
angle_setpoint = top_yaw_angle_pid_.update(-true_angle), then pass
angle_setpoint - top_yaw_velocity_imu() into top_yaw_velocity_pid_.update(...)
and assign the resulting torque to top_yaw_control_torque_.

Copy link

@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

♻️ Duplicate comments (2)
rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp (2)

103-108: ⚠️ Potential issue | 🟡 Minor

yaw_angle 归一化仍只处理一次环绕

Line 103-107 只做一次 ±2π 修正;多圈输入时可能仍越界。该问题在历史评论中已指出,当前实现仍未闭环。

建议修复
         double yaw_angle = *top_yaw_angle_ + *bottom_yaw_angle_;
-        if (yaw_angle < 0)
-            yaw_angle += 2 * std::numbers::pi;
-        else if (yaw_angle > 2 * std::numbers::pi)
-            yaw_angle -= 2 * std::numbers::pi;
+        while (yaw_angle < 0)
+            yaw_angle += 2 * std::numbers::pi;
+        while (yaw_angle >= 2 * std::numbers::pi)
+            yaw_angle -= 2 * std::numbers::pi;
         *yaw_angle_ = yaw_angle;
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp` around
lines 103 - 108, 当前对 yaw_angle 的归一化只做了一次 ±2π 修正,无法处理多圈输入;在
dual_yaw_controller.cpp 中计算完 double yaw_angle = *top_yaw_angle_ +
*bottom_yaw_angle_; 后,改为将角度完整归一化到 [0, 2π)(例如使用 std::fmod 或循环:yaw_angle =
std::fmod(yaw_angle, 2*std::numbers::pi); if (yaw_angle < 0) yaw_angle +=
2*std::numbers::pi;),然后再写回 *yaw_angle_,确保针对任意倍数 2π 的输入都能正确映射。

82-87: ⚠️ Potential issue | 🟠 Major

Top yaw 速度环仍缺少实际速度反馈

Line 84-85 仍是“角度环输出直接喂给速度环”,没有减去实际角速度,速度环等效开环,和 bottom yaw 控制结构不一致。这个问题在历史评论中已提过,当前提交仍存在。

建议修复
-                const auto velocity = top_yaw_angle_pid_.update(-true_angle);
-                const auto torque = top_yaw_velocity_pid_.update(velocity);
+                const auto velocity_setpoint = top_yaw_angle_pid_.update(-true_angle);
+                const auto torque = top_yaw_velocity_pid_.update(
+                    velocity_setpoint - *gimbal_yaw_velocity_imu_);
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp` around
lines 82 - 87, The angle loop currently feeds its output directly into the
velocity PID (top_yaw_angle_pid_.update -> top_yaw_velocity_pid_.update)
creating an open-loop velocity controller; change it to compute a velocity
setpoint from top_yaw_angle_pid_.update, then subtract the measured top yaw
angular velocity (use the actual sensor/state variable, e.g. *top_yaw_velocity_
or whatever the project uses for measured rate) to form a velocity error, pass
that error into top_yaw_velocity_pid_.update, and then write the resulting
torque into *top_yaw_control_torque_; this makes the top yaw control structure
consistent with bottom yaw.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.

Inline comments:
In `@rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp`:
- Line 153: The variable nav_velocity_odom is misleading because the code
applies Eigen::Rotation2Dd{*gimbal_yaw_angle_} to it (the same rotation used for
joystick/local inputs), so confirm the coordinate frame emitted by
/rmcs_navigation/command_velocity and either (a) rename nav_velocity_odom to a
clearer name (e.g., nav_velocity_local or nav_velocity_cmd) that matches the
actual frame, or (b) add a concise comment near the use of nav_velocity_odom and
gimbal_yaw_angle_ stating the expected source frame and why the rotation is
applied; if the frame is actually odom but the rotation is wrong, adjust the
rotation logic accordingly to convert from the true source frame to the chassis
frame.

In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp`:
- Around line 63-68: The code currently binds chassis_output_status_ to true
when the referee input is missing, which bypasses the safety check that triggers
on !*chassis_output_status_; change the fallback to fail-safe by calling
chassis_output_status_.bind_directly(false) instead of true, and apply the same
change to any other similar fallback bindings in this file (the analogous block
at lines 72-75) so missing referee inputs default to false and allow the
protection branch to run.

---

Duplicate comments:
In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp`:
- Around line 103-108: 当前对 yaw_angle 的归一化只做了一次 ±2π 修正,无法处理多圈输入;在
dual_yaw_controller.cpp 中计算完 double yaw_angle = *top_yaw_angle_ +
*bottom_yaw_angle_; 后,改为将角度完整归一化到 [0, 2π)(例如使用 std::fmod 或循环:yaw_angle =
std::fmod(yaw_angle, 2*std::numbers::pi); if (yaw_angle < 0) yaw_angle +=
2*std::numbers::pi;),然后再写回 *yaw_angle_,确保针对任意倍数 2π 的输入都能正确映射。
- Around line 82-87: The angle loop currently feeds its output directly into the
velocity PID (top_yaw_angle_pid_.update -> top_yaw_velocity_pid_.update)
creating an open-loop velocity controller; change it to compute a velocity
setpoint from top_yaw_angle_pid_.update, then subtract the measured top yaw
angular velocity (use the actual sensor/state variable, e.g. *top_yaw_velocity_
or whatever the project uses for measured rate) to form a velocity error, pass
that error into top_yaw_velocity_pid_.update, and then write the resulting
torque into *top_yaw_control_torque_; this makes the top yaw control structure
consistent with bottom yaw.

ℹ️ Review info
⚙️ Run configuration

Configuration used: Organization UI

Review profile: CHILL

Plan: Pro

Run ID: 40e57a0a-69e4-4358-9b80-54fb8bb99c4f

📥 Commits

Reviewing files that changed from the base of the PR and between d998808 and 2e39c71.

📒 Files selected for processing (3)
  • rmcs_ws/src/rmcs_bringup/config/sentry.yaml
  • rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp
  • rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp
✅ Files skipped from review due to trivial changes (1)
  • rmcs_ws/src/rmcs_bringup/config/sentry.yaml

nav_velocity_odom.y() = raw_command.y() * navigation_velocity_scale_;
}
}
auto nav_velocity = Eigen::Rotation2Dd{*gimbal_yaw_angle_} * nav_velocity_odom;
Copy link

Choose a reason for hiding this comment

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

⚠️ Potential issue | 🟡 Minor

🧩 Analysis chain

🏁 Script executed:

#!/bin/bash
# 查找导航命令发布者,确认其输出坐标系
rg -n -A 10 "command_velocity" --type cpp

Repository: Alliance-Algorithm/RMCS

Length of output: 15750


🏁 Script executed:

# Search for navigation-related files to understand output frame
find . -name "*navigation*" -type f | grep -E "\.(cpp|hpp|h|py)$" | head -20

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Look for frame-related documentation or constants
rg -n "frame\|Frame\|FRAME" --type cpp -i | grep -E "(odom|body|chassis|gimbal)" | head -30

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Check for any comments or documentation about coordinate frames in chassis_controller
sed -n '140,160p' rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp

Repository: Alliance-Algorithm/RMCS

Length of output: 957


🏁 Script executed:

# Look at the context around chassis_angle_ calculation to understand frame relationships
rg -n "chassis_angle_" rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp -B 2 -A 2

Repository: Alliance-Algorithm/RMCS

Length of output: 1421


🏁 Script executed:

# Search for rmcs_navigation package files
find . -path "*rmcs_navigation*" -type f | grep -E "\.(cpp|hpp|h|py|md|txt)$" | head -20

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Search for documentation or README files about coordinate frames
find . -type f \( -name "README*" -o -name "*.md" \) | xargs grep -l "frame\|coordinate" 2>/dev/null | head -10

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Look for vector/velocity definitions or comments in navigation context
rg -n "nav.*velocity\|navigation.*frame\|coordinate" --type cpp -i | head -30

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Check the gimbal angle source to understand what frame it represents
rg -n "gimbal.*yaw.*angle\|/gimbal/yaw" --type cpp -B 3 -A 3 | head -50

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Look for simple_gimbal_controller to understand gimbal angle context better
cat -n rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp | head -100

Repository: Alliance-Algorithm/RMCS

Length of output: 4693


🏁 Script executed:

# Check the entire chassis_controller update function to understand the flow
sed -n '66,210p' rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp

Repository: Alliance-Algorithm/RMCS

Length of output: 6142


🏁 Script executed:

# Search for any test files or configuration that might clarify frame conventions
find . -type f \( -name "*test*" -o -name "*config*" \) -name "*.cpp" -o -name "*.hpp" | xargs grep -l "gimbal_yaw\|nav_velocity" 2>/dev/null | head -10

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Look for geometry or transform related includes/comments
rg -n "Rotation\|Transform\|frame" rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp -B 1 -A 1

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Look for navigation-related ROS package or files in the workspace
find . -type d -name "*navigation*" | head -20

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Search for any comments or documentation about gimbal_yaw_angle frame reference
rg -n "gimbal.*angle\|yaw.*angle" rmcs_ws/src/rmcs_core/src/controller -B 3 -A 3 --type cpp | head -60

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Look at the two_axis_gimbal_solver to understand gimbal angle semantics
find . -name "*gimbal_solver*" -type f

Repository: Alliance-Algorithm/RMCS

Length of output: 220


🏁 Script executed:

# Examine gimbal solver to understand gimbal angle frame
cat -n rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp | head -80

Repository: Alliance-Algorithm/RMCS

Length of output: 3293


🏁 Script executed:

# Look at the full chassis_controller header to see type definitions
find . -name "chassis_controller.hpp" -o -name "chassis_controller.h" | xargs cat -n 2>/dev/null | head -50

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Search for OdomImu or frame definitions in tf_description
find . -name "*tf_description*" -type f | head -5

Repository: Alliance-Algorithm/RMCS

Length of output: 195


🏁 Script executed:

# Look for more context about how gimbal_yaw_angle is used in relation to frame transformations
rg -n "gimbal_yaw_angle_\|gimbal.*angle.*odom\|frame\|Frame" rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp -B 2 -A 2 | head -80

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Examine the tf_description header to understand frame definitions
cat -n rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp | head -150

Repository: Alliance-Algorithm/RMCS

Length of output: 6071


🏁 Script executed:

# Look at the chassis_controller to see the complete picture around nav_velocity transformation
sed -n '1,50p' rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp

Repository: Alliance-Algorithm/RMCS

Length of output: 2084


🏁 Script executed:

# Search for any documentation or README about coordinate systems
find . -type f \( -name "README*" -o -name "*.md" \) -exec grep -l "gimbal\|navigation\|frame" {} \; 2>/dev/null | head -5

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Look for any navigation package or documentation
find . -type f -name "*.cpp" -o -name "*.hpp" | xargs grep -l "rmcs_navigation" | head -10

Repository: Alliance-Algorithm/RMCS

Length of output: 211


🏁 Script executed:

# Search for comments or documentation about what gimbal_yaw_angle represents
rg -n "gimbal_yaw_angle|yaw.*angle" rmcs_ws/src/rmcs_core/src -B 5 -A 5 --type cpp | grep -E "(//|/\*|comment|frame|transform|rotate)" | head -40

Repository: Alliance-Algorithm/RMCS

Length of output: 1897


🏁 Script executed:

# Look at the full context of how translational_velocity is computed to understand the pattern
sed -n '110,160p' rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp

Repository: Alliance-Algorithm/RMCS

Length of output: 1995


🏁 Script executed:

# Look for the complete update_translational_velocity_control to see both transformations side-by-side
sed -n '115,156p' rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp

Repository: Alliance-Algorithm/RMCS

Length of output: 1738


🏁 Script executed:

# Check if there are any integration tests or example configs that clarify the navigation frame
find . -type f \( -name "*test*" -o -name "*.yaml" -o -name "*.xml" \) | xargs grep -l "nav.*velocity\|command_velocity" 2>/dev/null | head -10

Repository: Alliance-Algorithm/RMCS

Length of output: 112


🏁 Script executed:

# Check the sentry.yaml config to understand navigation setup
cat -n rmcs_ws/src/rmcs_bringup/config/sentry.yaml

Repository: Alliance-Algorithm/RMCS

Length of output: 5612


🏁 Script executed:

# Look for any comments or documentation in the entire chassis controller file
cat -n rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp | grep -E "(//|/\*|\*)" | head -40

Repository: Alliance-Algorithm/RMCS

Length of output: 2636


🏁 Script executed:

# Search for any information about rmcs_navigation Brain component or command_velocity output
rg -n "command_velocity|Brain" --type cpp --type hpp | grep -i "nav\|brain" | head -30

Repository: Alliance-Algorithm/RMCS

Length of output: 94


🏁 Script executed:

# Look for any documentation about coordinate frames in comments
rg -n "frame.*nav\|nav.*frame\|odom.*nav\|nav.*odom" --type cpp -i | head -20

Repository: Alliance-Algorithm/RMCS

Length of output: 49


🏁 Script executed:

# Check if there are imports or submodule references to rmcs_navigation
find . -name ".gitmodules" -o -name "CMakeLists.txt" | xargs grep -l "rmcs_navigation" 2>/dev/null

Repository: Alliance-Algorithm/RMCS

Length of output: 51


🏁 Script executed:

# Look at the complete picture - compare joystick and nav velocity handling side by side
sed -n '133,156p' rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp | cat -n

Repository: Alliance-Algorithm/RMCS

Length of output: 1336


变量命名 nav_velocity_odom 可能有误导性,需要澄清导航系统的输出坐标系。

变量名称 nav_velocity_odom 暗示速度来自里程计坐标系,但根据代码逻辑,它应用的旋转变换与摇杆输入完全相同(都乘以 Eigen::Rotation2Dd{*gimbal_yaw_angle_})。由于摇杆输入是本地控制信号而非里程计坐标系,这表明导航命令速度可能来自不同的坐标系约定。

建议确认 /rmcs_navigation/command_velocity 的输出坐标系约定,并相应地重命名变量或添加注释以提高代码的可读性。

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

In `@rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp` at line
153, The variable nav_velocity_odom is misleading because the code applies
Eigen::Rotation2Dd{*gimbal_yaw_angle_} to it (the same rotation used for
joystick/local inputs), so confirm the coordinate frame emitted by
/rmcs_navigation/command_velocity and either (a) rename nav_velocity_odom to a
clearer name (e.g., nav_velocity_local or nav_velocity_cmd) that matches the
actual frame, or (b) add a concise comment near the use of nav_velocity_odom and
gimbal_yaw_angle_ stating the expected source frame and why the rotation is
applied; if the frame is actually odom but the rotation is wrong, adjust the
rotation logic accordingly to convert from the true source frame to the chassis
frame.

Comment on lines +63 to +68
if (!chassis_output_status_.ready()) {
RCLCPP_WARN(
get_logger(),
"Failed to fetch \"/referee/chassis/output_status\", fallback to enabled.");
chassis_output_status_.bind_directly(true);
}
Copy link

Choose a reason for hiding this comment

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

⚠️ Potential issue | 🟠 Major

缺失裁判系统状态时回退为 true 会绕过失效保护

Line 63-68 在输入缺失时绑定 true,但 Line 72 依赖 !*chassis_output_status_ 进入保护分支;这会导致输入丢失时仍继续输出控制(fail-open)。建议改为 fail-safe(回退 false)。

建议修复
         if (!chassis_output_status_.ready()) {
             RCLCPP_WARN(
                 get_logger(),
-                "Failed to fetch \"/referee/chassis/output_status\", fallback to enabled.");
-            chassis_output_status_.bind_directly(true);
+                "Failed to fetch \"/referee/chassis/output_status\", fallback to disabled.");
+            chassis_output_status_.bind_directly(false);
         }

Also applies to: 72-75

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

In `@rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp` around
lines 63 - 68, The code currently binds chassis_output_status_ to true when the
referee input is missing, which bypasses the safety check that triggers on
!*chassis_output_status_; change the fallback to fail-safe by calling
chassis_output_status_.bind_directly(false) instead of true, and apply the same
change to any other similar fallback bindings in this file (the analogous block
at lines 72-75) so missing referee inputs default to false and allow the
protection branch to run.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

Status: Todo

Development

Successfully merging this pull request may close these issues.

3 participants