Conversation
Walkthrough新增 rmcs_core 的 Sentry 硬件组件实现并注册为插件;为底盘与云台控制器接入导航速度输入与若干参数化改动;移除 hikcamera 子模块;新增多项运维脚本与 zsh 完成;添加大量 Sentry 启动配置与少量设备访问器。 Changes
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)或服务响应
Estimated code review effort🎯 5 (Critical) | ⏱️ ~120 minutes Poem
🚥 Pre-merge checks | ✅ 2 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (2 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
📝 Coding Plan
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. Comment Tip You can customize the tone of the review comments and chat replies.Configure the |
There was a problem hiding this comment.
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
📒 Files selected for processing (18)
.clang-format.gitmodules.script/complete/_save-map-once.script/remote-status.script/save-map-onceDockerfilermcs_ws/src/hikcamerarmcs_ws/src/rmcs_bringup/config/sentry.yamlrmcs_ws/src/rmcs_core/package.xmlrmcs_ws/src/rmcs_core/plugins.xmlrmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpprmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_controller.cpprmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpprmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpprmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpprmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpprmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpprmcs_ws/src/rmcs_core/src/hardware/sentry.cpp
💤 Files with no reviewable changes (2)
- .gitmodules
- rmcs_ws/src/hikcamera
| // 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; |
There was a problem hiding this comment.
导航速度叠加后要重新限幅。
这里先把手动输入限到 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.
| // 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.
| 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; |
There was a problem hiding this comment.
角度归一化仅处理一次环绕,存在越界边界情况
在 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.
| // 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_; | ||
| } | ||
| } |
There was a problem hiding this comment.
不要把导航 yaw 的积分步长写死成 1 ms。
yaw_speed 在这里是速度,但 Line 78 把它乘上了固定的 1e-3。rmcs_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.
| #include <cstddef> | ||
| #include <format> | ||
| #include <memory> | ||
| #include <print> | ||
| #include <ranges> | ||
| #include <span> | ||
| #include <sstream> | ||
| #include <string_view> | ||
| #include <tuple> | ||
| #include <utility> |
There was a problem hiding this comment.
🧩 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 thestd::printlnoverloads that write tostdout(no stream parameter) and to aFILE*(first parameter isFILE*). [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 withstd::ostringstreamtoo, since it’s anstd::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 fRepository: 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 -130Repository: 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.
| void update() override { | ||
| top_board_->update(); | ||
| bottom_board_->update(); | ||
| } | ||
|
|
||
| void command_update() { | ||
| top_board_->command_update(); | ||
| bottom_board_->command_update(); | ||
| } |
There was a problem hiding this comment.
把 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 方法,从而消除数据竞态与校准穿插问题。
There was a problem hiding this comment.
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
📒 Files selected for processing (2)
rmcs_ws/src/rmcs_bringup/config/sentry.yamlrmcs_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
|
|
||
| /// @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; | ||
| } |
There was a problem hiding this comment.
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_.
There was a problem hiding this comment.
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 | 🟠 MajorTop 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
📒 Files selected for processing (3)
rmcs_ws/src/rmcs_bringup/config/sentry.yamlrmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpprmcs_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; |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# 查找导航命令发布者,确认其输出坐标系
rg -n -A 10 "command_velocity" --type cppRepository: 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 -20Repository: 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 -30Repository: 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.cppRepository: 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 2Repository: 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 -20Repository: 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 -10Repository: 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 -30Repository: 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 -50Repository: 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 -100Repository: 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.cppRepository: 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 -10Repository: 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 1Repository: 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 -20Repository: 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 -60Repository: 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 fRepository: 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 -80Repository: 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 -50Repository: 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 -5Repository: 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 -80Repository: 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 -150Repository: 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.cppRepository: 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 -5Repository: 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 -10Repository: 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 -40Repository: 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.cppRepository: 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.cppRepository: 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 -10Repository: 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.yamlRepository: 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 -40Repository: 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 -30Repository: 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 -20Repository: 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/nullRepository: 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 -nRepository: 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.
| 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); | ||
| } |
There was a problem hiding this comment.
缺失裁判系统状态时回退为 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.
哨兵机器人迁移(dev/migrate-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 中导出。导航与控制器集成
数值与实现改进
硬件接口增强
配置与依赖
脚本与补全
子模块与样式
变更量与审查建议