Open
Conversation
|
Note Reviews pausedIt looks like this branch is under active development. To avoid overwhelming you with review comments due to an influx of new commits, CodeRabbit has automatically paused this review. You can configure this behavior by changing the Use the following commands to manage reviews:
Use the checkboxes below for quick actions:
Walkthrough该PR引入了Dart制导系统的完整实现，包括新硬件驱动、控制器组件、配置管理和消息定义；将项目迁移至ROS Jazzy；重构设备驱动架构，新增端序处理和CAN数据包工具库；停用旧版机器人硬件模块。 Changes
预估代码审查工作量🎯 4 (Complex) | ⏱️ ~60 minutes 可能相关的PR
建议审阅人
诗
🚥 Pre-merge checks | ✅ 1 | ❌ 2❌ Failed checks (1 warning, 1 inconclusive)
✅ Passed checks (1 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches🧪 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 CodeRabbit can use Trivy to scan for security misconfigurations and secrets in Infrastructure as Code files.Add a .trivyignore file to your project to customize which findings Trivy reports. |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 18
Note
Due to the large number of review comments, Critical 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/plugins.xml (1)
1-128:⚠️ Potential issue | 🟠 Major请把
GimbalTfPublisher条目补回plugins.xml。当前整个文件里已经看不到这个插件导出；如果启动配置还会加载它，pluginlib 会在运行时直接找不到类。你这次虽然只是追加了新的 Dart 插件，但这里仍然需要把原仓库里的
GimbalTfPublisher原样保留下来。Based on learnings: The
GimbalTfPublisherentry in rmcs_ws/src/rmcs_core/plugins.xml is from the original repository and should be preserved as-is, even though it lacks the namespace prefix used by other entries.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_core/plugins.xml` around lines 1 - 128, The plugins.xml is missing the GimbalTfPublisher export which will break pluginlib at runtime; re-add the original <class> entry for GimbalTfPublisher into rmcs_core's <library> section exactly as it appeared in the upstream repo (preserve its original type/name and description and do not add the namespace prefix), so that rmcs_core::controller::gimbal::GimbalTfPublisher (the unique symbol) is registered along with the other classes in plugins.xml.
🟠 Major comments (17)
.vscode/settings.default.json-5-12 (1)
5-12:⚠️ Potential issue | 🟠 MajorROS Jazzy 容器中的 Python 版本路径错误
ROS Jazzy 使用 Ubuntu 24.04 (Noble) 基础镜像，其主要平台配置使用 Python 3.12，而不是配置中的 Python 3.10。这会导致指定的路径不存在，使 VS Code 的自动补全和代码分析功能失效。
修复
"python.autoComplete.extraPaths": [ - "/opt/ros/jazzy/lib/python3.10/site-packages", - "/opt/ros/jazzy/local/lib/python3.10/dist-packages" + "/opt/ros/jazzy/lib/python3.12/site-packages", + "/opt/ros/jazzy/local/lib/python3.12/dist-packages" ], "python.analysis.extraPaths": [ - "/opt/ros/jazzy/lib/python3.10/site-packages", - "/opt/ros/jazzy/local/lib/python3.10/dist-packages" + "/opt/ros/jazzy/lib/python3.12/site-packages", + "/opt/ros/jazzy/local/lib/python3.12/dist-packages" ],🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In @.vscode/settings.default.json around lines 5 - 12, The configured VS Code Python extraPaths ("python.autoComplete.extraPaths" and "python.analysis.extraPaths") point to Python 3.10 site-packages for the ROS Jazzy container; update those entries to the correct Python 3.12 paths (e.g., replace occurrences of "python3.10" with "python3.12") and ensure both "/opt/ros/jazzy/lib/python3.12/site-packages" and "/opt/ros/jazzy/local/lib/python3.12/dist-packages" (or the actual container paths) are present; after updating, validate the container’s Python interpreter path and adjust paths if the environment uses a different virtualenv or layout.rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp-119-128 (1)
119-128:⚠️ Potential issue | 🟠 Major
calculateAngleDelta()的环绕修正现在实际上不会生效。
delta在进入判断前已经被截成int16_t，所以delta > 32767/delta < -32768这两个分支永远不会命中。当前实现只是在返回截断后的原始差值，编码器过零时会把同步统计打成假跳变。🤖 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/dart/debug_info.cpp` around lines 119 - 128, The wrap-around correction in calculateAngleDelta is ineffective because delta is cast to int16_t too early; compute the raw difference in a wider signed type (e.g., int32_t or int) from current_angle and last_angle (or compute as uint32_t and convert), apply the wrap logic against 65536/32768 thresholds on that wider value, then finally cast the adjusted result to int16_t before returning; update the function calculateAngleDelta(last_angle, current_angle) to perform the math in the wider type so the delta > 32767 / delta < -32768 checks can actually trigger.rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp-177-217 (1)
177-217:⚠️ Potential issue | 🟠 Major容错分支不要在未通过本机 ID / 校验时仍更新角度。
这里把
uart_data[2]读成了id，但从未和id_比较；同时 checksum 错误也只是告警不返回失败。多舵机共用同一 UART 时，别的舵机回包或损坏帧都会照样覆盖current_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/hardware/device/trigger_servo.hpp` around lines 177 - 217, In fixable_calibrate_current_angle: don't update out_angle or current_angle_ unless the packet is for this servo and checksum passes; after parsing id (from uart_data[2]) compare it with the member id_ and if they differ return {false,0x0000} immediately; when computing the checksum (calculated_checksum) treat a mismatch as a fatal validation failure (return {false,0x0000}) instead of only logging; only after both id == id_ and checksum match, extract angle, set out_angle and current_angle_ (keep the same memory_order) and then log/return success.rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp-171-195 (1)
171-195:⚠️ Potential issue | 🟠 Major稳定区超时计数没有在恢复稳定后清零。
现在
stable_region_timeout_counter_只在首次进入稳定区时清零；中间哪怕已经重新稳定，之前累积的不稳定点也会继续保留。离散噪声会被累计成“超时退出”，长期稳定区会被误判。修复示例
if (is_stable) { + stable_region_timeout_counter_ = 0; if (!is_in_stable_region_) { is_in_stable_region_ = true; - current_stable_start_point_ = angle_window_[angle_window_.size() - 1]; - stable_region_timeout_counter_ = 0; + current_stable_start_point_ = angle_window_.back(); // RCLCPP_DEBUG(logger_, "Enter stable region: slope=%.4f, rms=%.4f", b, rms_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/controller/dart/debug_info.cpp` around lines 171 - 195, The stable-region timeout counter (stable_region_timeout_counter_) is only reset when entering a stable region for the first time (inside the is_stable && !is_in_stable_region_ block), so intermittent re-stabilizations keep prior unstable counts and can trigger false exits; change the logic so that whenever is_stable is true you reset stable_region_timeout_counter_ (not just on the first entry) — i.e., in the is_stable branch ensure stable_region_timeout_counter_ = 0 (and preserve the existing handling of is_in_stable_region_, current_stable_start_point_, and current_stable_total_points_/current_stable_at_control_angle_ as before).rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp-1-37 (1)
1-37:⚠️ Potential issue | 🟠 Major这个头文件需要 include guard /
#pragma once。它现在定义了两个
enum class，同一翻译单元只要经由两条路径重复包含这个.hpp，这里就会直接重定义。建议像同目录的force_sensor_mode.hpp一样在文件头加#pragma once。修复示例
+#pragma once + namespace rmcs_msgs {🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp` around lines 1 - 37, Add a header guard to prevent double inclusion by placing either `#pragma once` at the very top of the header or traditional include guards around the whole file; specifically update rmcs_msgs::DartLaunchStages and rmcs_msgs::DartFillingStages declarations in dart_launch_stage.hpp so they are protected from multiple inclusion (e.g., same pattern used in force_sensor_mode.hpp), ensuring no redefinition of the enum classes when the header is included via multiple translation paths.rmcs_ws/src/rmcs_bringup/config/dart.yaml-4-11 (1)
4-11:⚠️ Potential issue | 🟠 Major
force_pid_controller这段配置已经损坏。Line 89-91 重复声明了
setpoint、control和kp；常见 YAML 解析结果要么是后值覆盖前值，要么直接拒绝加载这个参数文件。并且它现在也没有出现在 Line 4-11 的components列表里，所以修好重复键之前/之后都不会按预期生效。建议修改
force_pid_controller: ros__parameters: measurement: /dart/force_control_motor/velocity setpoint: /force/control/velocity control: /dart/force_control_motor/control_torque kp: 1.0 - setpoint: /dart/yaw_motor/control_velocity - control: /dart/yaw_motor/control_torque - kp: 0.0 ki: 0.0 kd: 0.0如果这个控制环需要启用，还要把
rmcs_core::controller::pid::PidController -> force_pid_controller补进components。Also applies to: 83-93
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_bringup/config/dart.yaml` around lines 4 - 11, 配置中名为 force_pid_controller 的 PID 节点存在重复键（重复声明 setpoint、control、kp），导致 YAML 解析冲突并且该控制器未列在开头的 components 中；请在 dart.yaml 中为 force_pid_controller 只保留一组唯一的键值（合并或移除重复的 setpoint/control/kp 定义，确保每个参数只有一处声明），并在文件开头的 components 列表中添加条目 rmcs_core::controller::pid::PidController -> force_pid_controller 以启用该控制环（同时检查并修复文件中其它相同问题的块，如原评论提到的类似重复段落）。rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp-52-54 (1)
52-54:⚠️ Potential issue | 🟠 Major统一 force 电机的话题名。
这里注册的是
/dart/force_control_motor，但同一 PR 里的rmcs_ws/src/rmcs_bringup/config/dart.yamlLine 16-17、Line 65-69 和rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cppLine 46 都在使用/dart/force_screw_motor/...。按现在的命名，速度环和手动设定都不会接到这个电机上。🤖 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/catapult_dart.cpp` around lines 52 - 54, The motor is registered under the topic "/dart/force_control_motor" but other parts of the PR (config/dart.yaml and dart_setttings.cpp) use "/dart/force_screw_motor", so subscribers (speed loop and manual set) won't reach this motor; change the topic string in the force_control_motor_ construction to "/dart/force_screw_motor" (referencing force_control_motor_ and device::DjiMotor::Config in catapult_dart.cpp) so it matches the existing config and dart_setttings usage, and ensure any publishers/subscribers elsewhere remain consistent with that topic name.rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp-46-48 (1)
46-48:⚠️ Potential issue | 🟠 Majoryaw/pitch 手动控制的话题接不到 PID。
rmcs_ws/src/rmcs_bringup/config/dart.yamlLine 42-44 和 Line 76-78 把两个速度环的setpoint配成/pitch/control/velocity、/yaw/control/velocity，这里却发布到/dart/pitch_motor/control_velocity、/dart/yaw_motor/control_velocity。如果没有额外桥接，遥控调角不会进入闭环。建议修改
- register_output("/dart/yaw_motor/control_velocity", yaw_control_velocity_); - register_output("/dart/pitch_motor/control_velocity", pitch_control_velocity_); + register_output("/yaw/control/velocity", yaw_control_velocity_); + register_output("/pitch/control/velocity", pitch_control_velocity_);如果
/dart/...才是规范命名，就反过来把dart.yaml里的两个setpoint一并统一过去。🤖 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/dartlauncher/dart_setttings.cpp` around lines 46 - 48, The yaw/pitch manual control topics published by register_output currently use "/dart/yaw_motor/control_velocity" and "/dart/pitch_motor/control_velocity" so the PID setpoints from the bringup config (which use "/yaw/control/velocity" and "/pitch/control/velocity") never arrive; update the two register_output calls that publish yaw_control_velocity_ and pitch_control_velocity_ to use the exact topic names used in the config ("/yaw/control/velocity" and "/pitch/control/velocity"), or alternatively update the config setpoint keys to the "/dart/..." names so both sides match; ensure you change the register_output topic strings referencing yaw_control_velocity_ and pitch_control_velocity_ (or the corresponding setpoint keys in the YAML) consistently and verify no other components rely on the old "/dart/..." topic names.rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp-108-123 (1)
108-123:⚠️ Potential issue | 🟠 Major同步升降命令漏掉了右侧单独变化。
Line 111 只比较左侧目标角。如果右侧目标变化而左侧没变，这个分支不会发送同步命令，右舵机角度会停留在旧值。
建议修改
- if (current_target_left != last_lifting_left_angle_) { + if (current_target_left != last_lifting_left_angle_ + || current_target_right != last_lifting_right_angle_) { size_t uart_data_length; uint16_t runtime_left = 0; uint16_t runtime_right = 0; @@ const auto lifting_table_uart_data_ptr = command_buffer.get(); transmit_buffer_.add_uart2_transmission(lifting_table_uart_data_ptr, uart_data_length); last_lifting_left_angle_ = current_target_left; + last_lifting_right_angle_ = current_target_right; }同时补一个
uint16_t last_lifting_right_angle_ = 0xFFFF;成员。🤖 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/dart_filling_test_hardware.cpp` around lines 108 - 123, The sync-send branch only checks last_lifting_left_angle_, so changes to lifting_right_ are missed; add a member uint16_t last_lifting_right_angle_ (initialized to 0xFFFF) and change the condition in the block that currently references lifting_left_.get_target_angle()/last_lifting_left_angle_ to check if either current_target_left != last_lifting_left_angle_ OR current_target_right != last_lifting_right_angle_; when sending via device::TriggerServo::generate_sync_run_command and transmit_buffer_.add_uart2_transmission, update both last_lifting_left_angle_ and last_lifting_right_angle_ to the current targets so subsequent comparisons work correctly.rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp-176-190 (1)
176-190:⚠️ Potential issue | 🟠 Major同步升降命令漏掉了右侧单独变化。
Line 179 只比较
current_target_left。如果右侧目标变化而左侧不变，这里不会发任何同步命令，右舵机就会停在旧角度。建议修改
- if (current_target_left != last_lifting_left_angle_) { + if (current_target_left != last_lifting_left_angle_ + || current_target_right != last_lifting_right_angle_) { size_t uart_data_length; uint16_t runtime_left = 0; uint16_t runtime_right = 0; @@ const auto lifting_table_uart_data_ptr = command_buffer.get(); transmit_buffer_.add_uart2_transmission(lifting_table_uart_data_ptr, uart_data_length); last_lifting_left_angle_ = current_target_left; + last_lifting_right_angle_ = current_target_right; }同时补一个
uint16_t last_lifting_right_angle_ = 0xFFFF;成员。🤖 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/catapult_dart.cpp` around lines 176 - 190, The code only checks for changes to current_target_left (comparing to last_lifting_left_angle_) so a change on the right side alone won't send a sync command; update the conditional to send a sync command when either current_target_left != last_lifting_left_angle_ OR current_target_right != last_lifting_right_angle_, call device::TriggerServo::generate_sync_run_command and transmit_buffer_.add_uart2_transmission as before, and after sending update both last_lifting_left_angle_ and last_lifting_right_angle_; also add and initialize a member uint16_t last_lifting_right_angle_ = 0xFFFF to mirror last_lifting_left_angle_.rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp-41-43 (1)
41-43:⚠️ Potential issue | 🟠 Major
b_kp/b_ki/b_kd当前完全不生效。这里构造了
drive_belt_pid_calculator_，但 Line 217 直接把速度误差映射成力矩，没有调用 PID 计算器。现在调 YAML 里的 belt PID 参数不会改变任何输出。Also applies to: 203-223
🤖 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/dart/launch_controller.cpp` around lines 41 - 43, The belt PID parameters are unused because you compute torque by directly mapping velocity error instead of using drive_belt_pid_calculator_; replace the direct error->torque mapping in the belt control logic (the block around where velocity error is turned into torque in the drive/belt path and the repeated section covering the same logic) with a call to the PID calculator instance (drive_belt_pid_calculator_) to compute the torque correction from the velocity error, passing the error and timestep (or delta time) and using the PID output as the commanded torque; ensure you remove the old direct scaling and wire the PID output into wherever torque/effort is published so changes to b_kp/b_ki/b_kd in YAML take effect.rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp-225-233 (1)
225-233:⚠️ Potential issue | 🟠 Major阻塞计数需要在恢复时清零。
这里只有命中阻塞时才递增，正常周期不会归零，所以零散的低速/大力矩采样也会一路累计，最后在 Line 232 被误判成真正卡滞。
🧩 一个最小修正
if ((abs(*left_drive_belt_velocity_) < 0.5 && abs(*left_drive_belt_control_torque_) > 0.5) || (abs(*right_drive_belt_velocity_) < 0.5 && abs(*right_drive_belt_control_torque_) > 0.5)) { drive_belt_block_count_++; + } else { + drive_belt_block_count_ = 0; } return drive_belt_block_count_ > 1000 ? true : false;🤖 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/dart/launch_controller.cpp` around lines 225 - 233, The blocking_detection() routine currently only increments drive_belt_block_count_ on detected blockage and never resets it, causing sporadic low-speed/high-torque samples to accumulate into a false positive; modify blocking_detection() so that when neither left nor right belt meets the blockage condition (the two abs(...) checks on left_drive_belt_velocity_/left_drive_belt_control_torque_ and right_drive_belt_velocity_/right_drive_belt_control_torque_), reset drive_belt_block_count_ to zero (or a suitable decay) instead of leaving it unchanged, then return drive_belt_block_count_ > 1000 as before; update any related comments and ensure the reset uses the same counter variable drive_belt_block_count_.rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor_runtime.hpp-16-17 (1)
16-17:⚠️ Potential issue | 🟠 Major
std::numeric_limits<int>::quiet_NaN()对整型无效，无法用作缺失数据的哨兵值。
channel_1_weight_/channel_2_weight_是OutputInterface<int>类型。根据 C++ 标准库，std::numeric_limits<int>::has_quiet_NaN对整型恒为false，std::numeric_limits<int>::quiet_NaN()仅返回0，而非真正的 NaN。这意味着在启动阶段（传感器未上报数据）与实际读数 0 之间无法区分。需要改用
std::optional<int>、std::expected或额外的有效性标志来表达"缺失"状态。Also applies to: 42-58
🤖 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/device/force_sensor_runtime.hpp` around lines 16 - 17, The outputs channel_1_weight_ and channel_2_weight_ are declared as OutputInterface<int> but use std::numeric_limits<int>::quiet_NaN() which is invalid for ints; change the output type or signaling so missing data is representable: either make the outputs OutputInterface<std::optional<int>> (or OutputInterface<int> plus a separate OutputInterface<bool> valid flag), update all uses of channel_1_weight_ / channel_2_weight_ and their register_output calls to match the new type, and adjust any readers/consumers to check the optional/valid flag before using the int value; ensure register_output is given a matching sentinel (e.g. std::nullopt) or a default invalid state instead of quiet_NaN().rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp-17-18 (1)
17-18:⚠️ Potential issue | 🟠 MajorNaN 默认值在整型输出上不成立——实际返回的是
0
weight_ch1_和weight_ch2_是OutputInterface<int>类型，但其默认值使用了std::numeric_limits<int>::quiet_NaN()。对整型而言，
std::numeric_limits<int>::has_quiet_NaN为false，std::numeric_limits<int>::quiet_NaN()仅返回0，无法提供真实的 NaN 值。这样做无法区分"未收到数据"和"读数为 0"的状态。建议：
- 改用浮点类型输出（如
OutputInterface<double>），或- 添加独立的有效性标志（valid flag），或
- 使用
std::optional<int>或其他机制明确表达缺失值也适用于：40-56 行
🤖 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/device/force_sensor.hpp` around lines 17 - 18, weight_ch1_ and weight_ch2_ are declared as OutputInterface<int> but are being initialized with std::numeric_limits<int>::quiet_NaN() which is not meaningful for ints; update the outputs to convey missing data explicitly: either change the outputs to OutputInterface<double> (so status_component.register_output("/force_sensor/channel_1/weight", weight_ch1_, nan_) and channel_2 use a real NaN), or keep OutputInterface<int> and add a paired validity flag (e.g., weight_ch1_valid_, weight_ch2_valid_) or switch to optional semantics (std::optional<int>) so you can distinguish "no data" from 0; apply the same change for the other occurrences noted around lines 40-56 and update any producers/consumers of weight_ch1_/weight_ch2_ (and their register_output calls) accordingly.rmcs_ws/src/rmcs_core/src/hardware/force_sensor_test.cpp-80-86 (1)
80-86:⚠️ Potential issue | 🟠 Major不要在
0x302接收回调里按帧打印INFO。
force-sensor.yaml当前实际加载的就是这个组件，而且 executor 频率是1000Hz。这里每收到一帧就打两条RCLCPP_INFO，很容易把日志刷爆，并把 I/O 抖动带进控制路径。建议改成带节流的调试日志，或者只在显式测试开关打开时启用。🤖 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/force_sensor_test.cpp` around lines 80 - 86, The two unconditional RCLCPP_INFO calls inside the can_id == 0x302 branch (where force_sensor_.store_status and sensor_.store are called) flood logs at 1000Hz; change them to either throttled debug logs or gated by a test/debug flag: replace RCLCPP_INFO(...) with RCLCPP_DEBUG_THROTTLE(get_logger(), period_ms, ...) or wrap the INFO calls in a runtime check (e.g., a bool member like debug_force_sensor_logs_ or a ROS parameter) so the prints only occur when the explicit test switch is enabled; keep the existing sensor_.store and force_sensor_.store_status behavior unchanged.rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp-88-92 (1)
88-92:⚠️ Potential issue | 🟠 Major自动俯仰模式现在实际上没有输出。
pitch_control()只更新 setpoint 和 PID 参数，真正写*pitch_control_velocity_的语句还在注释里。所以is_auto_pitch_control_mode_ == 1时不会产生新的 pitch 指令，只会进入一个“看起来启用、实际上不控制”的分支。要么把反馈链路接完并写出输出，要么在功能未完成前禁用这个模式。Also applies to: 124-131
🤖 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/dart/dart_settings.cpp` around lines 88 - 92, The auto-pitch branch (when is_auto_pitch_control_mode_ == 1) only calls pitch_control() which updates setpoints/PID but does not write the actual actuator output because the line that assigns *pitch_control_velocity_ is commented out; update the branch so pitch_control() is followed by computing and writing the actual output to *pitch_control_velocity_ (re-implement the commented feedback/output assignment using the computed setpoint / PID result and max_transform_rate_), or if the output path is not ready, disable this mode by removing/guarding the is_auto_pitch_control_mode_ == 1 branch so it cannot be selected; also apply the same fix for the similar code area referenced around the 124-131 region.rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp-1-1 (1)
1-1:⚠️ Potential issue | 🟠 Major需要显式配置 Eigen3 依赖，确保包含方式与 CMake 配置一致。
当前代码使用
<eigen3/Eigen/Dense>但 rmcs_core/package.xml 中未声明 Eigen3 依赖，CMakeLists.txt 也无相应的find_package(Eigen3)配置。这导致静态分析报"file not found"错误。虽然代码可能在系统已安装 Eigen3 时编译通过，但这种隐式依赖违反了 ROS 2 最佳实践，不利于跨平台移植。需要选择以下方案之一：
推荐方案：在 rmcs_core/package.xml 中添加
<depend>eigen3</depend>，在 CMakeLists.txt 中添加find_package(Eigen3 REQUIRED)和ament_target_dependencies(rmcs_core Eigen3)，保持<eigen3/Eigen/Dense>包含方式。备选方案：将所有源文件的
#include <eigen3/Eigen/Dense>改为#include <Eigen/Dense>，并在 CMakeLists.txt 中通过find_package(Eigen3 REQUIRED)和target_link_libraries(rmcs_core Eigen3::Eigen)显式链接 Eigen3::Eigen 目标。当前 20+ 个源文件都使用相同的
<eigen3/Eigen/Dense>风格，建议统一处理。🤖 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/dart/dart_settings.cpp` at line 1, 当前源文件（例如 dart_settings.cpp 中的 `#include` <eigen3/Eigen/Dense>）使用了 Eigen3 但未在包配置中声明或在 CMake 中查找，导致静态分析报错；请选择方案一（推荐）或方案二并统一所有 20+ 个源文件：方案一——在 rmcs_core/package.xml 添加 <depend>eigen3</depend>，在 CMakeLists.txt 添加 find_package(Eigen3 REQUIRED) 并将 Eigen3 加入 ament_target_dependencies(rmcs_core Eigen3)；方案二——将所有 `#include` <eigen3/Eigen/Dense> 改为 `#include` <Eigen/Dense>，在 CMakeLists.txt 添加 find_package(Eigen3 REQUIRED) 并对目标使用 target_link_libraries(rmcs_core Eigen3::Eigen)；确保修改覆盖 dart_settings.cpp 和其它所有相同包含样式的源文件以保持一致性。
🧹 Nitpick comments (2)
rmcs_ws/src/rmcs_dart_guidance (1)
1-1: 完善子模块工作流文档子模块提交哈希已正确引入。建议补充以下操作指南到项目文档中，确保团队成员能够正确处理子模块：
- 初次克隆：使用
git clone --recursive或在克隆后执行git submodule update --init --recursive- 更新子模块：当引用更新时，执行
git submodule update --remote- CI/CD 配置：确保流水线正确初始化子模块（如 GitHub Actions 中配置
submodules: true）- 访问权限：确认团队成员都有访问子模块仓库的权限
在 README 或开发文档中记录这些步骤有助于降低新成员的学习成本。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_dart_guidance` at line 1, 在项目文档（例如 README 或 CONTRIBUTING，以及现有的 rmcs_dart_guidance）中补充子模块工作流指南：说明初次克隆时需递归初始化子模块（或在克隆后执行子模块初始化和更新）、说明当子模块引用更新时如何执行远端更新以同步子模块、指示在 CI/CD 配置（如 GitHub Actions）中启用或正确初始化子模块以确保流水线能访问子模块，以及提醒检查并记录子模块仓库的访问权限要求以便团队成员能正常拉取子模块。确保将这些步骤按“初次克隆、更新子模块、CI 配置、访问权限”四项列出并放入 rmcs_dart_guidance 或 README 中以便团队查阅。rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml (1)
22-24: 把存图/标定目录改成稳定路径。
./saved_images、./debug_saved和./calibration_results都依赖进程当前工作目录；在 launch 或 systemd 下它通常不是包目录。尤其 Line 61 默认把标定打开了，部署后很容易把结果写到不可预期的位置，或者直接因为权限问题失败。Also applies to: 37-38, 60-61
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml` around lines 22 - 24, 配置文件中 image_save_directory（当前为 "./saved_images"）以及其它相对路径如 "./debug_saved"、"./calibration_results" 不应依赖进程工作目录；请把这些默认值改为稳定的绝对路径（例如一个包级或系统级目录如 /var/lib/rmcs_bringup/... 或由环境变量/launch 参数注入的目录），并在程序启动时确保目录存在且具有正确权限；具体修改 target 字段包括 enable_image_saving、image_save_directory、image_save_interval_ms 和 calibration 保存相关的配置条目（即当前使用 "./debug_saved"、"./calibration_results" 的配置），并在启动逻辑中添加 mkdir -p 权限检查/创建步骤或文档说明如何通过环境/launch 覆盖该路径。
🤖 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/dart/dart_filling_test.cpp`:
- Around line 60-65: 测试里在写入 launch_stage_ 前没有先注册它：只有 filling_stage_ 被
register_output(...) 注册，但代码后面（写入 launch_stage_ 的地方，如在写入的函数或测试行）直接写入
launch_stage_，导致发布失败或首次解引用崩溃。请在与 filling_stage_ 相同的注册块中为 launch_stage_ 调用
register_output("/dart/launch/stage", launch_stage_)（或与现有命名一致的 topic 名称），确保
register_output 对象名为 launch_stage_ 并在任何对 launch_stage_ 写入（引用 launch_stage_
的代码，如在行 ~78 和 ~107 的写操作）之前完成注册。
- Around line 84-100: The function update() uses last_switch_right_ for edge
detection before it's ever initialized (e.g., checks like if (last_switch_right_
== Switch::MIDDLE && *switch_right_ == Switch::DOWN) ), causing random behavior
on first run; fix this by initializing last_switch_right_ to the current switch
state before any edge checks (either set last_switch_right_ = *switch_right_ at
the start of update() or initialize it in the class constructor), and apply the
same initialization/guard for the other occurrences noted (around the checks at
the later block flagged, e.g., the checks near the 156-157 region) so that
edge-detection only runs after last_switch_right_ holds a defined value.
In `@rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp`:
- Around line 77-111: 当前拨杆分支只有部分分支会写入
*yaw_control_velocity_、*pitch_control_velocity_、*force_control_
等输出，未命中分支时会把上一帧速度“锁存”下来；在 dart_settings.cpp 的该 do/while(false) 分支处理里，先统一复位输出（调用
reset_all_controls() 或显式把
*yaw_control_velocity_、*pitch_control_velocity_、*force_control_ 设为
0），然后在各激活分支（检查 switch_left/switch_right 的 UP/MIDDLE/DOWN 分支、以及
is_auto_pitch_control_mode_/is_auto_force_control_mode_ 分支）覆盖写入对应值（例如
joystick_left_->y(), joystick_right_->x(), average_force_ 处理和
transmit_distance_to_force()/pitch_control()/force_control()
调用），确保未匹配状态不会保留上帧命令。
In `@rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp`:
- Around line 15-16: The Node is currently constructed with
automatically_declare_parameters_from_overrides(true) while the code later calls
declare_parameter("fit_window_size", ...) etc., causing
ParameterAlreadyDeclaredException; fix by disabling automatic declaration in the
Node options and keep the explicit declare_parameter calls (change/remove
automatically_declare_parameters_from_overrides(true) in the Node initializer)
OR if you prefer automatic declaration, remove the declare_parameter(...) calls
and use get_parameter(...) to read those parameters instead (edit usages around
declare_parameter/get_parameter in debug_info.cpp accordingly). Ensure you
reference the Node constructor initializer that sets
automatically_declare_parameters_from_overrides and the
declare_parameter("fit_window_size", ...) calls when making the change.
In `@rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp`:
- Around line 73-81: The constructor registers many outputs but forgot to
register launch_stage_, so update() writes *launch_stage_ without it being
registered; add a register_output call for launch_stage_ (e.g.
register_output("/dart/launch/stage", launch_stage_, 0)) in the same block where
filling_stage_ and the other control_angle outputs are registered so
launch_stage_ is registered before update() dereferences it.
- Around line 107-115: The update() method reads uninitialized history variables
(e.g., last_switch_right_, last_launch_stage_) before they are set, causing
undefined behavior; fix by initializing these history/state members
(last_switch_right_, last_switch_left_ if present, last_launch_stage_) to
sensible defaults (e.g., Switch::MIDDLE and DartLaunchStages::INIT) either in
the class constructor or at the top of update() before any comparisons, or by
adding a guard that on first call copies
*switch_right_/*switch_left_/*launch_stage_ into the last_* vars and returns;
apply the same initialization/guard pattern to the other occurrences noted (the
variables referenced around the second site analogous to lines 279-280).
- Around line 119-126: 连续调用 delay_and_execute 导致后一个任务被丢弃且禁用后旧的延时任务会被回放：在
delay_and_execute 的实现和使用处（参见 delay_and_execute(), is_delaying_, 以及调用方中的
loading_process(),
lifting_left_control_angle/lifting_right_control_angle/filling_stage_
设置）中改为支持多重延时任务或为每次调用创建独立可取消的定时器对象，确保 is_delaying_ 不会阻止合法的后续调度；同时在控制器切换到 DISABLE
时（timer 清理处，原 250-267
区域对应的停机处理逻辑）取消并销毁所有未执行的延时定时器并清空待执行任务队列，重置相关状态标志以防止停机后回放旧动作。 Ensure
delay_and_execute returns/records a handle so callers can cancel, and use that
handle in the disable path to cancel pending tasks.
In `@rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp`:
- Around line 35-36: 构造函数中未读取/初始化成员 angle_control_max_velocity_
导致其未定义就被用于计算输出（在使用该成员的函数/表达式中如 yaw/pitch 速度乘法处），请在 dart_setttings.cpp 的构造函数中调用
get_parameter("angle_control_max_velocity") 并赋值给
angle_control_max_velocity_（提供合理默认值或处理参数缺失的情况），同时在配置文件 dart.yaml 的
dart_settings.ros__parameters 中添加 angle_control_max_velocity 参数以确保运行时可配置；确保所有使用
angle_control_max_velocity_ 的函数（例如生成角速度命令的那段逻辑）在该成员被初始化后才运行。
In `@rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp`:
- Around line 28-209: This PR contains a duplicate implementation of
rmcs_core::controller::dart::DartLaunchController (the class and its
PLUGINLIB_EXPORT_CLASS registration) which breaks ODR and plugin registration;
delete the duplicate source that defines DartLaunchController in the
"dartlauncher" compilation unit so only the single implementation remains, and
ensure the retained implementation is the one in
controller/dart/launch_controller.cpp (keep its class definition and
PLUGINLIB_EXPORT_CLASS and remove the other file from the repository/CMake
build).
In `@rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp`:
- Around line 67-68: The event_thread_ is being started in the constructor
initializer list which can run handle_events() before outputs, subscriptions and
tf_broadcaster_ are initialized (and before members declared after
event_thread_), exposing the object in a half-constructed state; move thread
startup out of the initializer list and instead initialize event_thread_ (or
call a start method) at the end of the constructor body after you've set up
output registration, created subscriptions, initialized tf_broadcaster_ and any
members referenced by handle_events(); locate event_thread_, handle_events(),
transmit_buffer_, tf_broadcaster_ and the output/subscription initialization
code and change to construct all members first then create the std::thread with
the [this] lambda.
- Around line 84-99: The subscriptions capture the UART pointer members by value
at subscription creation, reading uninitialized members
(trigger_servo_uart_data_ptr, limiting_servo_uart_data_ptr,
lifting_left_uart_data_ptr, lifting_right_uart_data_ptr) and later assignments
write only to the lambda parameter copy; fix by capturing this in the lambdas
and passing the member pointer expression (e.g.
this->trigger_servo_uart_data_ptr) into
trigger_servo_calibrate_subscription_callback so the callback reads the live
member at call time (or initialize those member pointers before creating the
subscriptions); update all four subscription lambdas
(trigger_calibrate_subscription_, limiting_calibrate_subscription_,
lifting_left_calibrate_subscription_, lifting_right_calibrate_subscription_) to
use a this-capture and the member pointer names when calling
trigger_servo_calibrate_subscription_callback.
In `@rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp`:
- Around line 43-58: The subscription lambdas capture raw member pointer
variables by value (e.g., trigger_servo_uart_data_ptr,
limiting_servo_uart_data_ptr, lifting_left_uart_data_ptr,
lifting_right_uart_data_ptr) before those members are initialized and later
assignments only modify the lambda-captured copy, causing UB and no persistence;
fix by either initializing these uart data pointers before creating the
subscriptions or change the lambdas to access the members via this (e.g.,
capture [this] and call
trigger_servo_calibrate_subscription_callback(trigger_servo_, std::move(msg),
this->trigger_servo_uart_data_ptr)) or use shared_ptr members and capture a
shared_ptr by value to ensure lifetime; ensure
trigger_servo_calibrate_subscription_callback is invoked with the actual member
(not a stale copy) so assignments after line 253 update the same object the
callbacks use.
- Around line 38-39: The constructor currently starts event_thread_ in the
member initializer list which runs handle_events() before the rest of the
constructor body and before outputs that are declared after event_thread_;
change event_thread_ so it is not started in the initializer list (e.g.,
initialize it default-constructed or as an empty std::thread) and then start the
thread from the constructor body after you finish registering
outputs/subscriptions and initializing transmit_buffer_ and other output
members; specifically modify the usage of event_thread_ and handle_events() in
the ctor (mirror the CatapultDart pattern) so the thread launch happens after
all output members are ready.
In `@rmcs_ws/src/rmcs_core/src/hardware/device/lifting_left.hpp`:
- Around line 91-123: calibrate_current_angle currently returns bool but callers
(e.g., the use in catapult_dart.cpp) expect a std::pair<bool,uint16_t> like
trigger_servo.hpp; change the signature of calibrate_current_angle(const
rclcpp::Logger&, const std::byte*, size_t) to return std::pair<bool,uint16_t>,
keep all existing validation (length, header, checksum using
ReadAngleReceiveData and package.calculate_checksum()), and on success return
{true, current_angle} (use the computed current_angle variable) and on any
failure return {false, 0}; update any call sites (e.g.,
lifting_left_.calibrate_current_angle(...)) to consume .first/.second instead of
a plain bool.
In `@rmcs_ws/src/rmcs_core/src/hardware/device/lifting_right.hpp`:
- Around line 91-123: The function calibrate_current_angle currently returns
bool but callers expect a std::pair<bool,uint16_t>; change
calibrate_current_angle(const rclcpp::Logger&, const std::byte*, size_t) to
return std::pair<bool,uint16_t>, keep all current validation logic but on
success return {true, current_angle} (use the parsed uint16_t current_angle
variable) and on failure return {false, 0}; mirror the behavior/return shape
used by trigger_servo.hpp (the logic around header/checksum/current_angle
parsing in calibrate_current_angle should remain the same) and update any
callers (e.g., in catapult_dart.cpp) to use result.first/result.second.
In `@rmcs_ws/src/rmcs_core/src/hardware/device/limiting_servo.hpp`:
- Around line 91-120: calibrate_current_angle is currently declared as static
bool and drops the parsed current_angle, breaking callers that expect a result
pair; change LimitingServo::calibrate_current_angle from a static bool to an
instance method returning std::pair<bool,uint16_t>, populate and return {true,
current_angle} on success and {false, 0} on failure, remove static usage so it
can access instance if needed, and update all call sites that expect
result.first/result.second to use the new return type (e.g., callers that treat
the return as result.first/result.second should receive and unpack the pair).
In `@rmcs_ws/src/rmcs_core/src/hardware/force_sensor_setting.cpp`:
- Around line 36-39: 在 ForceSensorSetting 的 command_update() 中当前错误地调用了
force_sensor_.generate_command()（返回 0x00）并通过
transmit_buffer_.add_can1_transmission(0x301, ...) 发送，导致未发送校零指令；请将调用改为
force_sensor_.generate_zero_calibration_command() 并替换为设备协议要求的 CAN ID（不要再使用
generate_command()），同时保持 transmit_buffer_.add_can1_transmission(...)
的数据类型转换/封装一致以确保消息格式不变。
---
Outside diff comments:
In `@rmcs_ws/src/rmcs_core/plugins.xml`:
- Around line 1-128: The plugins.xml is missing the GimbalTfPublisher export
which will break pluginlib at runtime; re-add the original <class> entry for
GimbalTfPublisher into rmcs_core's <library> section exactly as it appeared in
the upstream repo (preserve its original type/name and description and do not
add the namespace prefix), so that
rmcs_core::controller::gimbal::GimbalTfPublisher (the unique symbol) is
registered along with the other classes in plugins.xml.
---
Major comments:
In @.vscode/settings.default.json:
- Around line 5-12: The configured VS Code Python extraPaths
("python.autoComplete.extraPaths" and "python.analysis.extraPaths") point to
Python 3.10 site-packages for the ROS Jazzy container; update those entries to
the correct Python 3.12 paths (e.g., replace occurrences of "python3.10" with
"python3.12") and ensure both "/opt/ros/jazzy/lib/python3.12/site-packages" and
"/opt/ros/jazzy/local/lib/python3.12/dist-packages" (or the actual container
paths) are present; after updating, validate the container’s Python interpreter
path and adjust paths if the environment uses a different virtualenv or layout.
In `@rmcs_ws/src/rmcs_bringup/config/dart.yaml`:
- Around line 4-11: 配置中名为 force_pid_controller 的 PID 节点存在重复键（重复声明
setpoint、control、kp），导致 YAML 解析冲突并且该控制器未列在开头的 components 中；请在 dart.yaml 中为
force_pid_controller 只保留一组唯一的键值（合并或移除重复的 setpoint/control/kp
定义，确保每个参数只有一处声明），并在文件开头的 components 列表中添加条目
rmcs_core::controller::pid::PidController -> force_pid_controller
以启用该控制环（同时检查并修复文件中其它相同问题的块，如原评论提到的类似重复段落）。
In `@rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp`:
- Around line 88-92: The auto-pitch branch (when is_auto_pitch_control_mode_ ==
1) only calls pitch_control() which updates setpoints/PID but does not write the
actual actuator output because the line that assigns *pitch_control_velocity_ is
commented out; update the branch so pitch_control() is followed by computing and
writing the actual output to *pitch_control_velocity_ (re-implement the
commented feedback/output assignment using the computed setpoint / PID result
and max_transform_rate_), or if the output path is not ready, disable this mode
by removing/guarding the is_auto_pitch_control_mode_ == 1 branch so it cannot be
selected; also apply the same fix for the similar code area referenced around
the 124-131 region.
- Line 1: 当前源文件（例如 dart_settings.cpp 中的 `#include` <eigen3/Eigen/Dense>）使用了 Eigen3
但未在包配置中声明或在 CMake 中查找，导致静态分析报错；请选择方案一（推荐）或方案二并统一所有 20+ 个源文件：方案一——在
rmcs_core/package.xml 添加 <depend>eigen3</depend>，在 CMakeLists.txt 添加
find_package(Eigen3 REQUIRED) 并将 Eigen3 加入 ament_target_dependencies(rmcs_core
Eigen3)；方案二——将所有 `#include` <eigen3/Eigen/Dense> 改为 `#include` <Eigen/Dense>，在
CMakeLists.txt 添加 find_package(Eigen3 REQUIRED) 并对目标使用
target_link_libraries(rmcs_core Eigen3::Eigen)；确保修改覆盖 dart_settings.cpp
和其它所有相同包含样式的源文件以保持一致性。
In `@rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp`:
- Around line 119-128: The wrap-around correction in calculateAngleDelta is
ineffective because delta is cast to int16_t too early; compute the raw
difference in a wider signed type (e.g., int32_t or int) from current_angle and
last_angle (or compute as uint32_t and convert), apply the wrap logic against
65536/32768 thresholds on that wider value, then finally cast the adjusted
result to int16_t before returning; update the function
calculateAngleDelta(last_angle, current_angle) to perform the math in the wider
type so the delta > 32767 / delta < -32768 checks can actually trigger.
- Around line 171-195: The stable-region timeout counter
(stable_region_timeout_counter_) is only reset when entering a stable region for
the first time (inside the is_stable && !is_in_stable_region_ block), so
intermittent re-stabilizations keep prior unstable counts and can trigger false
exits; change the logic so that whenever is_stable is true you reset
stable_region_timeout_counter_ (not just on the first entry) — i.e., in the
is_stable branch ensure stable_region_timeout_counter_ = 0 (and preserve the
existing handling of is_in_stable_region_, current_stable_start_point_, and
current_stable_total_points_/current_stable_at_control_angle_ as before).
In `@rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp`:
- Around line 41-43: The belt PID parameters are unused because you compute
torque by directly mapping velocity error instead of using
drive_belt_pid_calculator_; replace the direct error->torque mapping in the belt
control logic (the block around where velocity error is turned into torque in
the drive/belt path and the repeated section covering the same logic) with a
call to the PID calculator instance (drive_belt_pid_calculator_) to compute the
torque correction from the velocity error, passing the error and timestep (or
delta time) and using the PID output as the commanded torque; ensure you remove
the old direct scaling and wire the PID output into wherever torque/effort is
published so changes to b_kp/b_ki/b_kd in YAML take effect.
- Around line 225-233: The blocking_detection() routine currently only
increments drive_belt_block_count_ on detected blockage and never resets it,
causing sporadic low-speed/high-torque samples to accumulate into a false
positive; modify blocking_detection() so that when neither left nor right belt
meets the blockage condition (the two abs(...) checks on
left_drive_belt_velocity_/left_drive_belt_control_torque_ and
right_drive_belt_velocity_/right_drive_belt_control_torque_), reset
drive_belt_block_count_ to zero (or a suitable decay) instead of leaving it
unchanged, then return drive_belt_block_count_ > 1000 as before; update any
related comments and ensure the reset uses the same counter variable
drive_belt_block_count_.
In `@rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp`:
- Around line 46-48: The yaw/pitch manual control topics published by
register_output currently use "/dart/yaw_motor/control_velocity" and
"/dart/pitch_motor/control_velocity" so the PID setpoints from the bringup
config (which use "/yaw/control/velocity" and "/pitch/control/velocity") never
arrive; update the two register_output calls that publish yaw_control_velocity_
and pitch_control_velocity_ to use the exact topic names used in the config
("/yaw/control/velocity" and "/pitch/control/velocity"), or alternatively update
the config setpoint keys to the "/dart/..." names so both sides match; ensure
you change the register_output topic strings referencing yaw_control_velocity_
and pitch_control_velocity_ (or the corresponding setpoint keys in the YAML)
consistently and verify no other components rely on the old "/dart/..." topic
names.
In `@rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp`:
- Around line 52-54: The motor is registered under the topic
"/dart/force_control_motor" but other parts of the PR (config/dart.yaml and
dart_setttings.cpp) use "/dart/force_screw_motor", so subscribers (speed loop
and manual set) won't reach this motor; change the topic string in the
force_control_motor_ construction to "/dart/force_screw_motor" (referencing
force_control_motor_ and device::DjiMotor::Config in catapult_dart.cpp) so it
matches the existing config and dart_setttings usage, and ensure any
publishers/subscribers elsewhere remain consistent with that topic name.
- Around line 176-190: The code only checks for changes to current_target_left
(comparing to last_lifting_left_angle_) so a change on the right side alone
won't send a sync command; update the conditional to send a sync command when
either current_target_left != last_lifting_left_angle_ OR current_target_right
!= last_lifting_right_angle_, call
device::TriggerServo::generate_sync_run_command and
transmit_buffer_.add_uart2_transmission as before, and after sending update both
last_lifting_left_angle_ and last_lifting_right_angle_; also add and initialize
a member uint16_t last_lifting_right_angle_ = 0xFFFF to mirror
last_lifting_left_angle_.
In `@rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp`:
- Around line 108-123: The sync-send branch only checks
last_lifting_left_angle_, so changes to lifting_right_ are missed; add a member
uint16_t last_lifting_right_angle_ (initialized to 0xFFFF) and change the
condition in the block that currently references
lifting_left_.get_target_angle()/last_lifting_left_angle_ to check if either
current_target_left != last_lifting_left_angle_ OR current_target_right !=
last_lifting_right_angle_; when sending via
device::TriggerServo::generate_sync_run_command and
transmit_buffer_.add_uart2_transmission, update both last_lifting_left_angle_
and last_lifting_right_angle_ to the current targets so subsequent comparisons
work correctly.
In `@rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor_runtime.hpp`:
- Around line 16-17: The outputs channel_1_weight_ and channel_2_weight_ are
declared as OutputInterface<int> but use std::numeric_limits<int>::quiet_NaN()
which is invalid for ints; change the output type or signaling so missing data
is representable: either make the outputs OutputInterface<std::optional<int>>
(or OutputInterface<int> plus a separate OutputInterface<bool> valid flag),
update all uses of channel_1_weight_ / channel_2_weight_ and their
register_output calls to match the new type, and adjust any readers/consumers to
check the optional/valid flag before using the int value; ensure register_output
is given a matching sentinel (e.g. std::nullopt) or a default invalid state
instead of quiet_NaN().
In `@rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp`:
- Around line 17-18: weight_ch1_ and weight_ch2_ are declared as
OutputInterface<int> but are being initialized with
std::numeric_limits<int>::quiet_NaN() which is not meaningful for ints; update
the outputs to convey missing data explicitly: either change the outputs to
OutputInterface<double> (so
status_component.register_output("/force_sensor/channel_1/weight", weight_ch1_,
nan_) and channel_2 use a real NaN), or keep OutputInterface<int> and add a
paired validity flag (e.g., weight_ch1_valid_, weight_ch2_valid_) or switch to
optional semantics (std::optional<int>) so you can distinguish "no data" from 0;
apply the same change for the other occurrences noted around lines 40-56 and
update any producers/consumers of weight_ch1_/weight_ch2_ (and their
register_output calls) accordingly.
In `@rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp`:
- Around line 177-217: In fixable_calibrate_current_angle: don't update
out_angle or current_angle_ unless the packet is for this servo and checksum
passes; after parsing id (from uart_data[2]) compare it with the member id_ and
if they differ return {false,0x0000} immediately; when computing the checksum
(calculated_checksum) treat a mismatch as a fatal validation failure (return
{false,0x0000}) instead of only logging; only after both id == id_ and checksum
match, extract angle, set out_angle and current_angle_ (keep the same
memory_order) and then log/return success.
In `@rmcs_ws/src/rmcs_core/src/hardware/force_sensor_test.cpp`:
- Around line 80-86: The two unconditional RCLCPP_INFO calls inside the can_id
== 0x302 branch (where force_sensor_.store_status and sensor_.store are called)
flood logs at 1000Hz; change them to either throttled debug logs or gated by a
test/debug flag: replace RCLCPP_INFO(...) with
RCLCPP_DEBUG_THROTTLE(get_logger(), period_ms, ...) or wrap the INFO calls in a
runtime check (e.g., a bool member like debug_force_sensor_logs_ or a ROS
parameter) so the prints only occur when the explicit test switch is enabled;
keep the existing sensor_.store and force_sensor_.store_status behavior
unchanged.
In `@rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp`:
- Around line 1-37: Add a header guard to prevent double inclusion by placing
either `#pragma once` at the very top of the header or traditional include
guards around the whole file; specifically update rmcs_msgs::DartLaunchStages
and rmcs_msgs::DartFillingStages declarations in dart_launch_stage.hpp so they
are protected from multiple inclusion (e.g., same pattern used in
force_sensor_mode.hpp), ensuring no redefinition of the enum classes when the
header is included via multiple translation paths.
---
Nitpick comments:
In `@rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml`:
- Around line 22-24: 配置文件中 image_save_directory（当前为 "./saved_images"）以及其它相对路径如
"./debug_saved"、"./calibration_results"
不应依赖进程工作目录；请把这些默认值改为稳定的绝对路径（例如一个包级或系统级目录如 /var/lib/rmcs_bringup/...
或由环境变量/launch 参数注入的目录），并在程序启动时确保目录存在且具有正确权限；具体修改 target 字段包括
enable_image_saving、image_save_directory、image_save_interval_ms 和 calibration
保存相关的配置条目（即当前使用 "./debug_saved"、"./calibration_results" 的配置），并在启动逻辑中添加 mkdir -p
权限检查/创建步骤或文档说明如何通过环境/launch 覆盖该路径。
In `@rmcs_ws/src/rmcs_dart_guidance`:
- Line 1: 在项目文档（例如 README 或 CONTRIBUTING，以及现有的
rmcs_dart_guidance）中补充子模块工作流指南：说明初次克隆时需递归初始化子模块（或在克隆后执行子模块初始化和更新）、说明当子模块引用更新时如何执行远端更新以同步子模块、指示在
CI/CD 配置（如 GitHub
Actions）中启用或正确初始化子模块以确保流水线能访问子模块，以及提醒检查并记录子模块仓库的访问权限要求以便团队成员能正常拉取子模块。确保将这些步骤按“初次克隆、更新子模块、CI
配置、访问权限”四项列出并放入 rmcs_dart_guidance 或 README 中以便团队查阅。
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: c7f80caf-3c21-421d-ad5c-23a72e0f37c3
📒 Files selected for processing (30)
.gitmodules.vscode/settings.default.jsonrmcs_ws/src/rmcs_bringup/config/dart-filling-test.yamlrmcs_ws/src/rmcs_bringup/config/dart-guidance.yamlrmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yamlrmcs_ws/src/rmcs_bringup/config/dart.yamlrmcs_ws/src/rmcs_bringup/config/force-sensor.yamlrmcs_ws/src/rmcs_core/plugins.xmlrmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpprmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpprmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpprmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpprmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpprmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpprmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpprmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpprmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpprmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpprmcs_ws/src/rmcs_core/src/hardware/device/force_sensor_runtime.hpprmcs_ws/src/rmcs_core/src/hardware/device/hojo_board.hpprmcs_ws/src/rmcs_core/src/hardware/device/lifting_left.hpprmcs_ws/src/rmcs_core/src/hardware/device/lifting_right.hpprmcs_ws/src/rmcs_core/src/hardware/device/limiting_servo.hpprmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpprmcs_ws/src/rmcs_core/src/hardware/force_sensor_receive.cpprmcs_ws/src/rmcs_core/src/hardware/force_sensor_setting.cpprmcs_ws/src/rmcs_core/src/hardware/force_sensor_test.cpprmcs_ws/src/rmcs_dart_guidancermcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpprmcs_ws/src/rmcs_msgs/include/rmcs_msgs/force_sensor_mode.hpp
rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp
Outdated
Show resolved
Hide resolved
rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp
Outdated
Show resolved
Hide resolved
Comment on lines
+77
to
+111
| do {if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) | ||
| || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { | ||
| reset_all_controls(); | ||
| break; | ||
| } | ||
| if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) | ||
| || (switch_left == Switch::UP && switch_right == Switch::MIDDLE)) { | ||
| if (is_auto_pitch_control_mode_ == 0) { // manual control mode | ||
| *yaw_control_velocity_ = joystick_left_->y() * max_transform_rate_; | ||
| *pitch_control_velocity_ = joystick_right_->x() * max_transform_rate_; | ||
| break; | ||
| } else if (is_auto_pitch_control_mode_ == 1){ // double loop pid | ||
| // double yaw_angle = (*yaw_pitch_angle_)[0]; | ||
| // *yaw_control_velocity_ = yaw_angle * max_transform_rate_; | ||
| pitch_control(); | ||
| break; | ||
| } | ||
| } | ||
| if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) | ||
| || (switch_left == Switch::UP && switch_right == Switch::DOWN)) { | ||
| if (is_auto_force_control_mode_ == 0) { | ||
| *force_control_ = joystick_right_->x() * max_transform_rate_ * 5; | ||
| break; | ||
| } else if (is_auto_force_control_mode_ == 1) { | ||
| *average_force_ = (*force_sensor_ch1_data_ + *force_sensor_ch2_data_) * 0.5; | ||
| transmit_distance_to_force(); | ||
| force_control(); | ||
| log_count_++; | ||
| if (log_count_ == 100) { | ||
| log_count_ = 0; | ||
| RCLCPP_INFO(logger_, "ch1:%d, ch2:%d", *force_sensor_ch1_data_, *force_sensor_ch2_data_); | ||
| } | ||
| } | ||
| } | ||
| } while(false); |
There was a problem hiding this comment.
未命中的拨杆状态会把上一帧速度指令锁存下来。
这里只有少数分支会写 yaw/pitch/force 输出；切回“双中”或任何未覆盖状态时，函数什么都不写，旧速度会继续保留。对速度控制来说，这会导致退出设置模式后机构仍按上一帧命令继续运动。建议先统一复位，再只在激活分支里覆写输出。
建议改法
void update() override {
using namespace rmcs_msgs;
+ reset_all_controls();
auto switch_right = *switch_right_;
auto switch_left = *switch_left_;
do {if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN)
- || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) {
- reset_all_controls();
+ || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) {
break;
} 🤖 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/dart/dart_settings.cpp` around lines 77
- 111, 当前拨杆分支只有部分分支会写入
*yaw_control_velocity_、*pitch_control_velocity_、*force_control_
等输出，未命中分支时会把上一帧速度“锁存”下来；在 dart_settings.cpp 的该 do/while(false) 分支处理里，先统一复位输出（调用
reset_all_controls() 或显式把
*yaw_control_velocity_、*pitch_control_velocity_、*force_control_ 设为
0），然后在各激活分支（检查 switch_left/switch_right 的 UP/MIDDLE/DOWN 分支、以及
is_auto_pitch_control_mode_/is_auto_force_control_mode_ 分支）覆盖写入对应值（例如
joystick_left_->y(), joystick_right_->x(), average_force_ 处理和
transmit_distance_to_force()/pitch_control()/force_control()
调用），确保未匹配状态不会保留上帧命令。
Comment on lines
+73
to
+81
| register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); | ||
| register_output("/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); | ||
|
|
||
| register_output("/dart/trigger_servo/control_angle", trigger_control_angle); | ||
| register_output("/dart/limiting_servo/control_angle", limiting_control_angle); | ||
| register_output("/dart/lifting_left/control_angle", lifting_left_control_angle); | ||
| register_output("/dart/lifting_right/control_angle", lifting_right_control_angle); | ||
|
|
||
| register_output("/dart/filling/stage", filling_stage_); |
There was a problem hiding this comment.
补上 launch_stage_ 的输出注册。
构造函数里只注册了 filling_stage_，但 update() 从 Line 96 开始一直在写 *launch_stage_。这样发射阶段至少不会被发布；如果 OutputInterface 依赖注册后才能解引用，首次写入就会直接失效。
🤖 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/dart/launch_controller.cpp` around lines
73 - 81, The constructor registers many outputs but forgot to register
launch_stage_, so update() writes *launch_stage_ without it being registered;
add a register_output call for launch_stage_ (e.g.
register_output("/dart/launch/stage", launch_stage_, 0)) in the same block where
filling_stage_ and the other control_angle outputs are registered so
launch_stage_ is registered before update() dereferences it.
Comment on lines
+43
to
+58
| trigger_calibrate_subscription_ = create_subscription<std_msgs::msg::Int32>( | ||
| "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { | ||
| trigger_servo_calibrate_subscription_callback(trigger_servo_, std::move(msg), trigger_servo_uart_data_ptr); | ||
| }); | ||
| limiting_calibrate_subscription_ = create_subscription<std_msgs::msg::Int32>( | ||
| "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { | ||
| trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg), limiting_servo_uart_data_ptr); | ||
| }); | ||
| lifting_left_calibrate_subscription_ = create_subscription<std_msgs::msg::Int32>( | ||
| "/lifting_left/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { | ||
| trigger_servo_calibrate_subscription_callback(lifting_left_, std::move(msg), lifting_left_uart_data_ptr); | ||
| }); | ||
| lifting_right_calibrate_subscription_ = create_subscription<std_msgs::msg::Int32>( | ||
| "/lifting_right/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { | ||
| trigger_servo_calibrate_subscription_callback(lifting_right_, std::move(msg), lifting_right_uart_data_ptr); | ||
| }); |
There was a problem hiding this comment.
标定回调会先读取未初始化的成员指针。
Line 43-58 把 trigger_servo_uart_data_ptr 等成员按值传给回调，但它们在 Line 213-216 从未初始化；Line 253 的赋值也只改了形参副本。这个模式既有 UB，也没有任何持久化意义。
建议修改
- trigger_servo_calibrate_subscription_callback(trigger_servo_, std::move(msg), trigger_servo_uart_data_ptr);
+ trigger_servo_calibrate_subscription_callback(trigger_servo_, std::move(msg));
@@
- trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg), limiting_servo_uart_data_ptr);
+ trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg));
@@
- trigger_servo_calibrate_subscription_callback(lifting_left_, std::move(msg), lifting_left_uart_data_ptr);
+ trigger_servo_calibrate_subscription_callback(lifting_left_, std::move(msg));
@@
- trigger_servo_calibrate_subscription_callback(lifting_right_, std::move(msg), lifting_right_uart_data_ptr);
+ trigger_servo_calibrate_subscription_callback(lifting_right_, std::move(msg));
@@
- std::byte * trigger_servo_uart_data_ptr;
- std::byte * limiting_servo_uart_data_ptr;
- std::byte * lifting_left_uart_data_ptr;
- std::byte * lifting_right_uart_data_ptr;
-
void trigger_servo_calibrate_subscription_callback(device::TriggerServo& servo_
- , std_msgs::msg::Int32::UniquePtr msg
- , std::byte* servo_uart_data_ptr) {
+ , std_msgs::msg::Int32::UniquePtr msg) {
@@
- servo_uart_data_ptr = command_buffer.get();
- transmit_buffer_.add_uart2_transmission(servo_uart_data_ptr, command_length);
+ transmit_buffer_.add_uart2_transmission(command_buffer.get(), command_length);Also applies to: 213-220, 253-254
🤖 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/dart_filling_test_hardware.cpp` around
lines 43 - 58, The subscription lambdas capture raw member pointer variables by
value (e.g., trigger_servo_uart_data_ptr, limiting_servo_uart_data_ptr,
lifting_left_uart_data_ptr, lifting_right_uart_data_ptr) before those members
are initialized and later assignments only modify the lambda-captured copy,
causing UB and no persistence; fix by either initializing these uart data
pointers before creating the subscriptions or change the lambdas to access the
members via this (e.g., capture [this] and call
trigger_servo_calibrate_subscription_callback(trigger_servo_, std::move(msg),
this->trigger_servo_uart_data_ptr)) or use shared_ptr members and capture a
shared_ptr by value to ensure lifetime; ensure
trigger_servo_calibrate_subscription_callback is invoked with the actual member
(not a stale copy) so assignments after line 253 update the same object the
callbacks use.
Comment on lines
+36
to
+39
| count_++; | ||
| if (count_ == 100) { | ||
| count_ = 0; | ||
| transmit_buffer_.add_can1_transmission(0x301, std::bit_cast<uint64_t>(force_sensor_.generate_command())); |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
set -euo pipefail
echo "== All usages of zero calibration helper =="
rg -n -C2 'generate_zero_calibration_command\s*\(' --glob '*.hpp' --glob '*.cpp'
echo
echo "== command_update in ForceSensorSetting =="
sed -n '34,43p' rmcs_ws/src/rmcs_core/src/hardware/force_sensor_setting.cpp
echo
echo "== command_update in ForceSensorTest =="
sed -n '61,68p' rmcs_ws/src/rmcs_core/src/hardware/force_sensor_test.cppRepository: Alliance-Algorithm/RMCS
Length of output: 1807
确认：ForceSensorSetting 应调用 generate_zero_calibration_command() 而非 generate_command()
组件设计上应负责发送校零命令。经代码检查确认，generate_zero_calibration_command() 返回 0x0F，但在整个仓库中无任何活跃调用点（仅存在于注释代码）。当前 ForceSensorSetting.command_update() 发送 generate_command()（返回 0x00），行为与 ForceSensorTest 完全相同，实际上校零命令路径未实装。建议修改为调用 generate_zero_calibration_command()，同时更新对应的 CAN ID（需参考设备协议）。
🤖 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/force_sensor_setting.cpp` around lines 36
- 39, 在 ForceSensorSetting 的 command_update() 中当前错误地调用了
force_sensor_.generate_command()（返回 0x00）并通过
transmit_buffer_.add_can1_transmission(0x301, ...) 发送，导致未发送校零指令；请将调用改为
force_sensor_.generate_zero_calibration_command() 并替换为设备协议要求的 CAN ID（不要再使用
generate_command()），同时保持 transmit_buffer_.add_can1_transmission(...)
的数据类型转换/封装一致以确保消息格式不变。
There was a problem hiding this comment.
Actionable comments posted: 18
🧹 Nitpick comments (8)
rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp (2)
11-45: 可考虑使用编译器内置函数优化字节交换。当前手动实现是正确的，但可以考虑使用编译器内置函数（如 GCC/Clang 的
__builtin_bswap16/32/64）来获得更好的性能，编译器通常会将其优化为单条指令。另外，第 41 行的
return 0是死代码，因为static_assert已确保只有 2、4、8 字节类型能通过编译，但这不影响正确性。♻️ 可选优化：使用内置函数
template <typename T> requires(std::is_integral_v<T> || std::is_floating_point_v<T>) [[nodiscard]] inline T swap_endian(const T& value) noexcept { static_assert( sizeof(T) == 2 || sizeof(T) == 4 || sizeof(T) == 8, "Endian swap is only defined for 2, 4, and 8-byte types"); - T result; - - const auto* value_bytes = reinterpret_cast<const std::byte*>(&value); - auto* result_bytes = reinterpret_cast<std::byte*>(&result); - - if constexpr (sizeof(T) == 8) { // 64-bit - result_bytes[0] = value_bytes[7]; - result_bytes[1] = value_bytes[6]; - result_bytes[2] = value_bytes[5]; - result_bytes[3] = value_bytes[4]; - result_bytes[4] = value_bytes[3]; - result_bytes[5] = value_bytes[2]; - result_bytes[6] = value_bytes[1]; - result_bytes[7] = value_bytes[0]; - } else if constexpr (sizeof(T) == 4) { // 32-bit - result_bytes[0] = value_bytes[3]; - result_bytes[1] = value_bytes[2]; - result_bytes[2] = value_bytes[1]; - result_bytes[3] = value_bytes[0]; - } else if constexpr (sizeof(T) == 2) { // 16-bit - result_bytes[0] = value_bytes[1]; - result_bytes[1] = value_bytes[0]; - } else { - return 0; - } - - return result; + if constexpr (std::is_integral_v<T>) { + if constexpr (sizeof(T) == 8) { + return static_cast<T>(__builtin_bswap64(static_cast<uint64_t>(value))); + } else if constexpr (sizeof(T) == 4) { + return static_cast<T>(__builtin_bswap32(static_cast<uint32_t>(value))); + } else { + return static_cast<T>(__builtin_bswap16(static_cast<uint16_t>(value))); + } + } else { + // For floating point, use bit_cast (C++20) with integral swap + if constexpr (sizeof(T) == 8) { + auto bits = std::bit_cast<uint64_t>(value); + return std::bit_cast<T>(__builtin_bswap64(bits)); + } else { + auto bits = std::bit_cast<uint32_t>(value); + return std::bit_cast<T>(__builtin_bswap32(bits)); + } + } }🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp` around lines 11 - 45, The swap_endian template manually reverses bytes which is correct but can be optimized: modify swap_endian to use compiler built-ins (__builtin_bswap16, __builtin_bswap32, __builtin_bswap64) when available for sizeof(T)==2/4/8, preserving the existing requires/ static_assert and noexcept; for integral types call the appropriate built-in on the underlying unsigned integer representation (e.g., reinterpret/cast to std::make_unsigned_t<T> or std::uint16_t/32_t/64_t), then reinterpret/cast back to T, and remove the unreachable else branch that returns 0 (or replace it with a clear unreachable path) while keeping behavior for floating-point types by bit-casting to an unsigned integer, bswapping, and bit-casting back so swap_endian<float/double> uses the built-ins too.
49-49:[[gnu::packed]]是 GNU 扩展，不可移植到 MSVC。如果项目仅针对 GCC/Clang（如 ROS2 环境），这是可以接受的。若需跨平台支持，可考虑使用条件编译或
#pragma pack。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp` at line 49, The struct EndianContainer uses the GNU-only attribute [[gnu::packed]], which is not portable to MSVC; update the declaration to conditionally apply packing: detect compilers (e.g., __GNUC__ / __clang__ vs _MSC_VER) and for GCC/Clang keep the [[gnu::packed]] attribute (or __attribute__((packed))), while for MSVC wrap the struct with `#pragma` pack(push,1) before and `#pragma` pack(pop) after (or use MSVC-specific declspec if preferred); ensure EndianContainer remains packed on all targets and add an explanatory comment and compile-time static_assert (sizeof check) if possible.rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp (3)
68-71: 四元数访问器返回可变引用。
q0()至q3()返回double&，允许外部代码直接修改内部状态，破坏了封装性。从上下文代码片段来看，这些方法仅用于读取。建议同时提供 const 版本，或改为仅返回值。♻️ 建议添加 const 访问器
- double& q0() { return q0_; } - double& q1() { return q1_; } - double& q2() { return q2_; } - double& q3() { return q3_; } + double q0() const { return q0_; } + double q1() const { return q1_; } + double q2() const { return q2_; } + double q3() const { return q3_; }如果确实需要外部设置四元数（例如重置姿态），建议提供显式的 setter 方法：
void set_quaternion(double q0, double q1, double q2, double q3) { q0_ = q0; q1_ = q1; q2_ = q2; q3_ = q3; }🤖 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/device/bmi088.hpp` around lines 68 - 71, The q0()/q1()/q2()/q3() accessors currently return mutable double& allowing external mutation of internal quaternion state; change them to non-mutating accessors (either return double by value or provide const overloads like double q0() const) and, if external mutation is required, add an explicit setter such as set_quaternion(double q0, double q1, double q2, double q3) to centralize state changes; update references to q0()/q1()/q2()/q3() accordingly and remove any external code that relied on writing via the returned references.
44-47: 建议为魔法数字添加命名常量。
32767.0、6.0、2000.0是 BMI088 传感器特定的配置值（int16 最大值、±6g 加速度量程、±2000°/s 陀螺仪量程）。为提高可读性和可维护性，可以考虑定义为命名常量。💡 示例
private: static constexpr double kInt16Max = 32767.0; static constexpr double kAccelRange = 6.0; // ±6g static constexpr double kGyroRange = 2000.0; // ±2000 deg/s🤖 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/device/bmi088.hpp` around lines 44 - 47, The magic numbers in the BMI088 conversion lambdas (solve_acc and solve_gyro) should be replaced with well-named constants for clarity and maintainability: add constants such as kInt16Max = 32767.0, kAccelRange = 6.0, kGyroRange = 2000.0 and a kDegToRad = std::numbers::pi / 180.0 (or similar) in the containing class/namespace, then update solve_acc to use value / kInt16Max * kAccelRange and update solve_gyro to use value / kInt16Max * kGyroRange * kDegToRad so the conversions are self-documenting and easy to adjust.
150-154: 原子成员变量未显式初始化。
accelerometer_data_和gyroscope_data_使用默认初始化，在store_*_status()被调用之前，底层的ImuData值是不确定的。🔧 建议显式初始化
- std::atomic<ImuData> accelerometer_data_, gyroscope_data_; + std::atomic<ImuData> accelerometer_data_{ImuData{0, 0, 0}}; + std::atomic<ImuData> gyroscope_data_{ImuData{0, 0, 0}};🤖 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/device/bmi088.hpp` around lines 150 - 154, The atomic ImuData members accelerometer_data_ and gyroscope_data_ are default-initialized and thus hold indeterminate values until store_*_status() is called; explicitly initialize them to a known ImuData (e.g., {0,0,0}) either in the BMI088 class constructor initialization list or via inline member initialization (std::atomic<ImuData>{ImuData{0,0,0}}) so reads are well-defined; keep the ImuData struct and the static_assert on std::atomic<ImuData>::is_always_lock_free unchanged.rmcs_ws/src/rmcs_core/src/hardware/tunnel_infantry.cpp (1)
1-308: 不要把整份实现保留成注释块。现在这个文件既没有任何行为，又留下了 300 多行失效实现，后续很难判断这是临时屏蔽还是正式废弃。若确实下线该组件，直接删文件并同步清理相关构建/文档会更清晰；若只是临时停用，建议保留最小 stub 并附上 issue/TODO 链接。
🤖 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/tunnel_infantry.cpp` around lines 1 - 308, The file contains a fully commented-out implementation of TunnelInfantry (class TunnelInfantry, its methods, related members, and PLUGINLIB_EXPORT_CLASS) leaving a 300+ line inert block; either remove the file and clean up build/registration or replace the large comment with a minimal stub component that documents the reason and links an issue/TODO. Fix by either (A) deleting this file and removing any references/registration (e.g., PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::TunnelInfantry, rmcs_executor::Component)) from the build and docs, or (B) restore a small stub implementation for TunnelInfantry that inherits rmcs_executor::Component and rclcpp::Node with no-op constructors/destructors and an explanatory comment plus an issue/TODO link, and keep or re-add a matching PLUGINLIB_EXPORT_CLASS registration so the build and plugin registration remain correct.rmcs_ws/src/rmcs_core/src/hardware/device/pwm_servo.hpp (1)
3-6: 补齐头文件的直接依赖。
Line 13用到了std::string，Line 16、Line 22和Line 25用到了uint16_t，但这里没有直接包含<string>和<cstdint>。现在能编过只是在依赖传递包含，后面很容易因为包含顺序变化变成脆弱构建。♻️ 建议修改
`#include` <algorithm> +#include <cstdint> `#include` <cmath> `#include` <limits> +#include <string> `#include` <rmcs_executor/component.hpp>🤖 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/device/pwm_servo.hpp` around lines 3 - 6, The header is missing direct includes for types it uses: add `#include` <string> and `#include` <cstdint> to pwm_servo.hpp so std::string and uint16_t are provided directly (fix the usages referenced where std::string is used and the declarations/parameters using uint16_t), preventing reliance on transitive includes and fragile build breaks.rmcs_ws/src/rmcs_core/src/hardware/device/can_packet.hpp (1)
35-38:as_bytes()应添加const限定符。此方法返回
const std::byte的 span，不修改对象状态，应标记为const成员函数。♻️ 建议的修复
- constexpr std::span<const std::byte, sizeof(data)> as_bytes() noexcept { + constexpr std::span<const std::byte, sizeof(data)> as_bytes() const noexcept { return std::span<const std::byte, sizeof(data)>{ reinterpret_cast<const std::byte*>(&data), sizeof(data)}; }🤖 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/device/can_packet.hpp` around lines 35 - 38, The member function as_bytes() should be marked const because it does not modify object state; change its declaration/definition to be a const member (e.g., add the trailing const so the signature becomes constexpr std::span<const std::byte, sizeof(data)> as_bytes() const noexcept) and keep the existing reinterpret_cast to const std::byte* of &data so it compiles in const contexts; update any declarations/overloads accordingly.
🤖 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/CMakeLists.txt`:
- Around line 18-22: 摘要：为 FetchContent_Declare 中的 librmcs 添加 URL_HASH SHA256
校验以提高供应链完整性和可复现性。 修复方法：在 rmcs_core/CMakeLists.txt 的 FetchContent_Declare(...)
块（标识符 librmcs）中为该 URL 增加一个 URL_HASH 参数，格式为 URL_HASH SHA256=<实际的 sha256 值>；先对下载的
archiv 文件或远程 URL 使用 sha256sum 或 curl|sha256sum 计算正确的 SHA256，然后将该散列字符串填入 URL_HASH
字段，确保 CMake 在下载时校验完整性并在不匹配时失败。
- Around line 6-7: The CMake minimum required version is too low for the
features used; update the cmake_minimum_required call to VERSION 3.24 so it
matches features like CMAKE_CXX_STANDARD 23, FetchContent,
FetchContent_MakeAvailable(), and DOWNLOAD_EXTRACT_TIMESTAMP; locate the
cmake_minimum_required(...) invocation and change its VERSION to 3.24 to resolve
the mismatch.
In `@rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp`:
- Line 56: update() only calls force_sensor_.update_status() and therefore never
publishes the motor states populated by can2_receive_callback() ->
store_status(), causing stale motor feedback; modify update() to, after
force_sensor_.update_status(), iterate over the stored motor status entries
(from store_status()) and invoke the existing publish routines for those
statuses (e.g., publish_motor_status()/publish_all_motor_states() or the
project’s equivalent publish methods) so all motor states are emitted to the
controller.
In `@rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp`:
- Around line 26-31: The CatapultDart plugin is currently commented out so no
plugin symbol is emitted; either restore the implementation or remove its
registration: uncomment the CatapultDart class declaration/definition (including
the CatapultDart() constructor and any methods) and re-enable the
PLUGINLIB_EXPORT_CLASS(...) macro that exports rmcs_core::hardware::CatapultDart
as an rmcs_executor::Component so the shared object contains the plugin symbol,
or if the implementation is not ready remove the plugin entry (and any
PLUGINLIB_EXPORT_CLASS(...) lines) so plugins.xml / runtime won't attempt to
load rmcs_core::hardware::CatapultDart (also check the other commented export at
the referenced lines 558-559 and apply the same fix).
In `@rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp`:
- Line 156: 构造函数中未初始化成员 ax_, ay_, az_, gx_, gy_, gz_; 在类 BMI088（或其构造函数
BMI088::BMI088）中将这些成员初始化为明确的初始值（例如 0.0），可以在构造函数的成员初始化列表中设置 ax_(0.0), ay_(0.0),
az_(0.0), gx_(0.0), gy_(0.0), gz_(0.0) 或在声明处给出默认值，以确保在调用 getter 或
update_status() 之前不会读取未定义的值。
In `@rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp`:
- Around line 51-63: The DjiMotor constructor currently only initializes angle_,
velocity_, and torque_, leaving can_data_, encoder_zero_point_, last_raw_angle_,
multi_turn_angle_enabled_, angle_multi_turn_, all coefficient variables,
max_torque_, and temperature_ uninitialized which can cause UB if
update_status() runs before configure(); fix by initializing these members in
the DjiMotor(...) initializer list (or via brace-or-equal defaults in the class)
— ensure can_data_ is default-constructed (e.g., atomic holding a zeroed
CanPacket8), set encoder_zero_point_ and last_raw_angle_ to 0, disable
multi_turn_angle_enabled_ and zero angle_multi_turn_, initialize all coefficient
variables and max_torque_/temperature_ to safe defaults, and keep
update_status()/store_status() usage unchanged (so constructor invariants
prevent reading uninitialized data).
In `@rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp`:
- Around line 22-24: store_status currently only stores raw CAN bytes into
can_data_ (via CanPacket8) but update_status reads/decodes from
force_sensor_status_, so incoming 0x302 packets never reach the decoder and
published values stay at defaults; fix by making the two paths consistent:
either have store_status decode the incoming span (using the same decoding logic
as update_status) and store the decoded result into force_sensor_status_ (and
update any atomic/store ordering as needed), or change update_status to
load/parse can_data_ (CanPacket8) and then write the decoded values into
force_sensor_status_; update references to can_data_, CanPacket8, store_status,
update_status, and force_sensor_status_ accordingly so the decoded data is
actually published.
In `@rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp`:
- Around line 336-345: The velocity limit conversion uses the signed coefficient
velocity_to_command_velocity_coefficient_ so when config.reversed is true a
positive velocity_limit can become negative and be clamped to 0 before casting;
in the function that builds the A4/A8 command (the velocity limit path shown)
use the absolute value (or an unsigned conversion coefficient) when computing
velocity_limit (e.g. apply std::abs to the scaled value or use a separate
unsigned coefficient), clamp the absolute/scaled value to uint16_t range, then
static_cast to uint16_t for command.velocity_limit; apply the same fix in
generate_angle_shift_command() where the unsigned limit is computed.
- Around line 80-119: The code incorrectly uses raw_angle_max_ = 65535 while
masking encoder_zero_point_ with (raw_angle_max_ - 1) which assumes a
power-of-two period; separate the concepts by introducing a raw_angle_period_
(e.g., 65536 for a 16-bit encoder) and keep raw_angle_max_ as the maximum index
if needed, then replace the mask/bitwise operations (encoder_zero_point_ =
config.encoder_zero_point & (raw_angle_max_ - 1)) with encoder_zero_point_ =
config.encoder_zero_point & (raw_angle_period_ - 1) and update all wrap/diff
logic (the code around the diff calculation referenced at lines ~177-179) to use
raw_angle_period_ for modular arithmetic; ensure raw_angle_period_ is a power of
two or validate it and document the change in the motor-type switch
(Type::kMG5010Ei10, Type::kMG4010Ei10, etc.).
- Around line 508-512: The fields angle_, velocity_, torque_, temperature_ are
not initialized and can be read as indeterminate before the first
update_status(); initialize them to sensible defaults (e.g., 0.0) to match the
constructor's exported port defaults—either by setting them in the lk_motor
constructor initializer list or by providing in-class default initializers for
angle_, velocity_, torque_, temperature_ (and max_torque_ if desired) so any
getter returns defined values prior to the first update_status().
In `@rmcs_ws/src/rmcs_core/src/hardware/device/pwm_servo.hpp`:
- Around line 12-25: 构造函数 PWMServo 在初始化列表里直接用
pwm_cycle_time、min_position_high_time、max_position_high_time 做除法和缩放（赋值到
duty_offset_ / duty_scale_），但这些参数可能无效导致负值、inf 或越界并在 generate_duty_cycle() 转为
uint16_t 时静默截断；请在使用这些值前对入参做显式校验（确保 pwm_cycle_time > 0、min_position_high_time >=
0、max_position_high_time >= min_position_high_time、max_position_high_time <=
pwm_cycle_time）并在不满足时返回/抛出 std::invalid_argument；为简单实现，把 duty_offset_ 和
duty_scale_ 的计算从初始化列表移到构造函数体中（或通过一个验证并返回已校验值的静态 helper），并在 generate_duty_cycle()
中对计算出的 duty 做最后一层保护性 clamp 到 [0, std::numeric_limits<uint16_t>::max()] 后再
static_cast<uint16_t>，以避免把非法占空比传给 gpio_analog_transmit。
In `@rmcs_ws/src/rmcs_core/src/hardware/hero.cpp`:
- Around line 1-480: The Hero component implementation in hero.cpp is entirely
commented out leaving only references to rmcs_core::hardware::Hero (including
the PLUGINLIB_EXPORT_CLASS macro) which causes pluginlib instantiation failures;
either fully restore/uncomment and compile the Hero class and its
PLUGINLIB_EXPORT_CLASS export in hero.cpp (ensuring class Hero and its
TopBoard/BottomBoard implementations are present), or permanently remove the
component by deleting the PLUGINLIB_EXPORT_CLASS and the Hero class from the
build and also remove the rmcs_core::hardware::Hero registration from the plugin
registry and the Hero entry in the mecanum-hero configuration so no runtime code
references a missing symbol.
In `@rmcs_ws/src/rmcs_core/src/hardware/infantry.cpp`:
- Around line 1-315: The Infantry implementation (class Infantry and
PLUGINLIB_EXPORT_CLASS) is completely commented out but remains registered in
plugins.xml (rmcs_core::hardware::Infantry), causing pluginlib load failures; do
one of two fixes: either restore the implementation by uncommenting/re-adding
the Infantry class and its PLUGINLIB_EXPORT_CLASS macro (look for symbols
Infantry and PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Infantry,
rmcs_executor::Component)) or remove the plugin registration entries from
plugins.xml for rmcs_core::hardware::Infantry (and the analogous entries for
rmcs_core::hardware::SteeringInfantry and rmcs_core::hardware::TunnelInfantry)
and delete the corresponding commented-out implementation files; ensure that for
any retained plugin registration the corresponding class definition and export
macro are present and compile.
In `@rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp`:
- Around line 1-528: The SteeringHero implementation and its
PLUGINLIB_EXPORT_CLASS registration were fully commented out, but plugins.xml
still declares rmcs_core::hardware::SteeringHero, causing runtime
plugin-not-found errors; either restore a compilable SteeringHero class and
re-enable PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero,
rmcs_executor::Component) (ensure class name SteeringHero and the export macro
are present and compile), or remove the SteeringHero entry and any references
from plugins.xml (and any launch/config that attempts to load it) so the plugin
declaration and the code stay in sync.
In `@rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp`:
- Around line 1-443: The SteeringInfantry plugin class is commented out but
still referenced by the plugin loader (PLUGINLIB_EXPORT_CLASS and the
SteeringInfantry class are disabled) which causes "class not registered" at
runtime; either remove the plugin registration and yaml reference or restore the
implementation: 1) If you mean to delete the component, remove the
rmcs_core::hardware::SteeringInfantry entry from plugins.xml and the reference
in steering-infantry.yaml; or 2) If you intend to keep it, uncomment the entire
steering-infantry.cpp implementation including the
PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringInfantry,
rmcs_executor::Component) macro so the class and export match the
plugins.xml/steering-infantry.yaml declarations.
In `@rmcs_ws/src/rmcs_core/src/hardware/tunnel_infantry.cpp`:
- Around line 19-308: TunnelInfantry 类的全部实现和插件导出 PLUGINLIB_EXPORT_CLASS
被注释掉，导致组件不再被导出并在运行时找不到；请恢复（取消注释） TunnelInfantry
类定义及其成员实现（构造函数、析构、update、command_update、各回调与私有成员）并恢复最后一行的
PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::TunnelInfantry,
rmcs_executor::Component) 导出，或者如果决定移除该组件，删除/更新所有对 TunnelInfantry
或该插件导出的引用（launch/插件描述文件等）以保持一致。
In `@rmcs_ws/src/rmcs_utility/include/rmcs_utility/as_byte_span.hpp`:
- Around line 1-7: The file as_byte_span.hpp currently only contains an empty
namespace rmcs_utility while its name and included headers suggest a utility to
convert objects to std::byte spans; either implement the intended utility or
remove the file. If you intend to implement it, add a function template (e.g.,
to_byte_span/T to_mutable_byte_span) in namespace rmcs_utility that uses
std::span and std::as_bytes/std::as_writable_bytes or reinterpret_cast with
static_asserts on std::is_trivially_copyable to safely produce
std::span<std::byte> from an object or container; otherwise delete
as_byte_span.hpp and remove any planned includes. Ensure the chosen symbols
(namespace rmcs_utility and the new function names like
to_byte_span/to_mutable_byte_span) match existing style and add unit tests if
implementing.
In `@rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp`:
- Around line 126-149: The assignment operators operator%=, operator&=,
operator|=, operator^=, operator<<= and operator>>= on EndianContainer are
invalid for floating-point T (e.g., le_float32_t, be_float64_t) and cause
compile errors; restrict these operators to integer types by adding a constraint
(e.g., use std::is_integral_v<T> or the std::integral concept in a requires
clause) or guard their bodies with if constexpr(std::is_integral_v<T>) so they
are only instantiated for integral T; update the templates for the methods named
operator%=, operator&=, operator|=, operator^=, operator<<= and operator>>= in
the EndianContainer definition to enforce that constraint.
---
Nitpick comments:
In `@rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp`:
- Around line 68-71: The q0()/q1()/q2()/q3() accessors currently return mutable
double& allowing external mutation of internal quaternion state; change them to
non-mutating accessors (either return double by value or provide const overloads
like double q0() const) and, if external mutation is required, add an explicit
setter such as set_quaternion(double q0, double q1, double q2, double q3) to
centralize state changes; update references to q0()/q1()/q2()/q3() accordingly
and remove any external code that relied on writing via the returned references.
- Around line 44-47: The magic numbers in the BMI088 conversion lambdas
(solve_acc and solve_gyro) should be replaced with well-named constants for
clarity and maintainability: add constants such as kInt16Max = 32767.0,
kAccelRange = 6.0, kGyroRange = 2000.0 and a kDegToRad = std::numbers::pi /
180.0 (or similar) in the containing class/namespace, then update solve_acc to
use value / kInt16Max * kAccelRange and update solve_gyro to use value /
kInt16Max * kGyroRange * kDegToRad so the conversions are self-documenting and
easy to adjust.
- Around line 150-154: The atomic ImuData members accelerometer_data_ and
gyroscope_data_ are default-initialized and thus hold indeterminate values until
store_*_status() is called; explicitly initialize them to a known ImuData (e.g.,
{0,0,0}) either in the BMI088 class constructor initialization list or via
inline member initialization (std::atomic<ImuData>{ImuData{0,0,0}}) so reads are
well-defined; keep the ImuData struct and the static_assert on
std::atomic<ImuData>::is_always_lock_free unchanged.
In `@rmcs_ws/src/rmcs_core/src/hardware/device/can_packet.hpp`:
- Around line 35-38: The member function as_bytes() should be marked const
because it does not modify object state; change its declaration/definition to be
a const member (e.g., add the trailing const so the signature becomes constexpr
std::span<const std::byte, sizeof(data)> as_bytes() const noexcept) and keep the
existing reinterpret_cast to const std::byte* of &data so it compiles in const
contexts; update any declarations/overloads accordingly.
In `@rmcs_ws/src/rmcs_core/src/hardware/device/pwm_servo.hpp`:
- Around line 3-6: The header is missing direct includes for types it uses: add
`#include` <string> and `#include` <cstdint> to pwm_servo.hpp so std::string and
uint16_t are provided directly (fix the usages referenced where std::string is
used and the declarations/parameters using uint16_t), preventing reliance on
transitive includes and fragile build breaks.
In `@rmcs_ws/src/rmcs_core/src/hardware/tunnel_infantry.cpp`:
- Around line 1-308: The file contains a fully commented-out implementation of
TunnelInfantry (class TunnelInfantry, its methods, related members, and
PLUGINLIB_EXPORT_CLASS) leaving a 300+ line inert block; either remove the file
and clean up build/registration or replace the large comment with a minimal stub
component that documents the reason and links an issue/TODO. Fix by either (A)
deleting this file and removing any references/registration (e.g.,
PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::TunnelInfantry,
rmcs_executor::Component)) from the build and docs, or (B) restore a small stub
implementation for TunnelInfantry that inherits rmcs_executor::Component and
rclcpp::Node with no-op constructors/destructors and an explanatory comment plus
an issue/TODO link, and keep or re-add a matching PLUGINLIB_EXPORT_CLASS
registration so the build and plugin registration remain correct.
In `@rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp`:
- Around line 11-45: The swap_endian template manually reverses bytes which is
correct but can be optimized: modify swap_endian to use compiler built-ins
(__builtin_bswap16, __builtin_bswap32, __builtin_bswap64) when available for
sizeof(T)==2/4/8, preserving the existing requires/ static_assert and noexcept;
for integral types call the appropriate built-in on the underlying unsigned
integer representation (e.g., reinterpret/cast to std::make_unsigned_t<T> or
std::uint16_t/32_t/64_t), then reinterpret/cast back to T, and remove the
unreachable else branch that returns 0 (or replace it with a clear unreachable
path) while keeping behavior for floating-point types by bit-casting to an
unsigned integer, bswapping, and bit-casting back so swap_endian<float/double>
uses the built-ins too.
- Line 49: The struct EndianContainer uses the GNU-only attribute
[[gnu::packed]], which is not portable to MSVC; update the declaration to
conditionally apply packing: detect compilers (e.g., __GNUC__ / __clang__ vs
_MSC_VER) and for GCC/Clang keep the [[gnu::packed]] attribute (or
__attribute__((packed))), while for MSVC wrap the struct with `#pragma`
pack(push,1) before and `#pragma` pack(pop) after (or use MSVC-specific declspec
if preferred); ensure EndianContainer remains packed on all targets and add an
explanatory comment and compile-time static_assert (sizeof check) if possible.
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: ae02b1ab-53d4-4c8e-8e25-276a399ea354
📒 Files selected for processing (21)
rmcs_ws/src/rmcs_bringup/config/pwm_test.yamlrmcs_ws/src/rmcs_core/CMakeLists.txtrmcs_ws/src/rmcs_core/librmcsrmcs_ws/src/rmcs_core/plugins.xmlrmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpprmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpprmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpprmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpprmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpprmcs_ws/src/rmcs_core/src/hardware/device/can_packet.hpprmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpprmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpprmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpprmcs_ws/src/rmcs_core/src/hardware/device/pwm_servo.hpprmcs_ws/src/rmcs_core/src/hardware/hero.cpprmcs_ws/src/rmcs_core/src/hardware/infantry.cpprmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpprmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpprmcs_ws/src/rmcs_core/src/hardware/tunnel_infantry.cpprmcs_ws/src/rmcs_utility/include/rmcs_utility/as_byte_span.hpprmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp
✅ Files skipped from review due to trivial changes (2)
- rmcs_ws/src/rmcs_core/librmcs
- rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp
Comment on lines
+6
to
+7
| set(CMAKE_CXX_STANDARD 23) | ||
| set(CMAKE_CXX_STANDARD_REQUIRED ON) |
There was a problem hiding this comment.
🧩 Analysis chain
🌐 Web query:
``FetchContent_MakeAvailable、DOWNLOAD_EXTRACT_TIMESTAMP`、以及 `CXX_STANDARD` / `CMAKE_CXX_STANDARD` 取值 `23`，分别从哪个 CMake 版本开始支持？`
💡 Result:
| 功能/取值 | 从哪个 CMake 版本开始支持 | 依据 |
|---|---|---|
FetchContent_MakeAvailable() |
3.14 | CMake 3.14 Release Notes 说明 FetchContent 模块新增该命令。[1] |
DOWNLOAD_EXTRACT_TIMESTAMP（用于 ExternalProject_Add() / FetchContent_Declare()） |
3.24 | CMake 3.24 Release Notes：ExternalProject_Add() 新增该选项；并由策略 CMP0135（3.24 引入）配套说明该选项也用于 FetchContent_Declare()。[2][3] |
CXX_STANDARD / CMAKE_CXX_STANDARD 取值 23 |
3.20 | CMake 3.20 Release Notes：支持 C++23（含 CXX_STANDARD / cxx_std_23）；CMake 3.20 的 CXX_STANDARD 文档列出可用值包含 23。[4][5] |
参考来源：
[1] (cmake.org) [2] (cmake.org) [3] (cmake.org) [4] (cmake.org) [5] (cmake.org)
Citations:
- 1: https://cmake.org/cmake/help/latest/release/3.14.html?utm_source=openai
- 2: https://cmake.org/cmake/help/latest/release/3.24.html?utm_source=openai
- 3: https://cmake.org/cmake/help/latest/policy/CMP0135.html?utm_source=openai
- 4: https://cmake.org/cmake/help/latest/release/3.20.html?utm_source=openai
- 5: https://cmake.org/cmake/help/v3.20/prop_tgt/CXX_STANDARD.html?utm_source=openai
🏁 Script executed:
cat -n rmcs_ws/src/rmcs_core/CMakeLists.txt | head -30Repository: Alliance-Algorithm/RMCS
Length of output: 1216
CMake 最低版本声明与实际使用的特性不匹配。
文件声明 cmake_minimum_required(VERSION 3.12)，但使用的功能需要更高版本：
- 第 6 行的
CMAKE_CXX_STANDARD 23需要 CMake 3.20+ - 第 16 行的
FetchContent需要 CMake 3.14+ - 第 21 行的
DOWNLOAD_EXTRACT_TIMESTAMP需要 CMake 3.24+ - 第 23 行的
FetchContent_MakeAvailable()需要 CMake 3.14+
请将第 1 行的最低版本更新为 3.24，以匹配实际使用的最高要求。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@rmcs_ws/src/rmcs_core/CMakeLists.txt` around lines 6 - 7, The CMake minimum
required version is too low for the features used; update the
cmake_minimum_required call to VERSION 3.24 so it matches features like
CMAKE_CXX_STANDARD 23, FetchContent, FetchContent_MakeAvailable(), and
DOWNLOAD_EXTRACT_TIMESTAMP; locate the cmake_minimum_required(...) invocation
and change its VERSION to 3.24 to resolve the mismatch.
Comment on lines
+18
to
+22
| FetchContent_Declare( | ||
| librmcs | ||
| URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0b0/librmcs-sdk-src-3.1.0-beta.0.zip | ||
| DOWNLOAD_EXTRACT_TIMESTAMP TRUE | ||
| ) |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
set -euo pipefail
curl -L -o /tmp/librmcs-sdk-src-3.1.0-beta.0.zip \
https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0b0/librmcs-sdk-src-3.1.0-beta.0.zip
sha256sum /tmp/librmcs-sdk-src-3.1.0-beta.0.zipRepository: Alliance-Algorithm/RMCS
Length of output: 513
🏁 Script executed:
#!/bin/bash
python3 <<'EOFPYTHON'
import hashlib
with open('/tmp/librmcs-sdk-src-3.1.0-beta.0.zip', 'rb') as f:
sha256_hash = hashlib.sha256(f.read()).hexdigest()
print(f"SHA256={sha256_hash}")
EOFPYTHONRepository: Alliance-Algorithm/RMCS
Length of output: 138
🏁 Script executed:
cat rmcs_ws/src/rmcs_core/CMakeLists.txtRepository: Alliance-Algorithm/RMCS
Length of output: 1294
添加 SHA256 校验以增强供应链完整性。
FetchContent_Declare 缺少 URL_HASH 参数。这会导致供应链完整性风险，且使构建结果不可复现。需要添加 SHA256 校验。
🛠️ 建议修改
FetchContent_Declare(
librmcs
URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0b0/librmcs-sdk-src-3.1.0-beta.0.zip
+ URL_HASH SHA256=7f3203e8db8cb7954ae0ac2ee226f93e32b367b3956b1f0bb5c7ae203c2ccbef
DOWNLOAD_EXTRACT_TIMESTAMP TRUE
)📝 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
| FetchContent_Declare( | |
| librmcs | |
| URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0b0/librmcs-sdk-src-3.1.0-beta.0.zip | |
| DOWNLOAD_EXTRACT_TIMESTAMP TRUE | |
| ) | |
| FetchContent_Declare( | |
| librmcs | |
| URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0b0/librmcs-sdk-src-3.1.0-beta.0.zip | |
| URL_HASH SHA256=7f3203e8db8cb7954ae0ac2ee226f93e32b367b3956b1f0bb5c7ae203c2ccbef | |
| DOWNLOAD_EXTRACT_TIMESTAMP TRUE | |
| ) |
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@rmcs_ws/src/rmcs_core/CMakeLists.txt` around lines 18 - 22, 摘要：为
FetchContent_Declare 中的 librmcs 添加 URL_HASH SHA256 校验以提高供应链完整性和可复现性。 修复方法：在
rmcs_core/CMakeLists.txt 的 FetchContent_Declare(...) 块（标识符 librmcs）中为该 URL 增加一个
URL_HASH 参数，格式为 URL_HASH SHA256=<实际的 sha256 值>；先对下载的 archiv 文件或远程 URL 使用
sha256sum 或 curl|sha256sum 计算正确的 SHA256，然后将该散列字符串填入 URL_HASH 字段，确保 CMake
在下载时校验完整性并在不匹配时失败。
|
|
||
| ~CatapultDartforlibrmcsv3() override = default; | ||
|
|
||
| void update() override { force_sensor_.update_status(); } |
There was a problem hiding this comment.
这里漏掉了所有电机状态的发布。
can2_receive_callback() 已经把各路电机帧喂给了 store_status()，但 update() 只调用了 force_sensor_.update_status()。这样控制器侧会一直读到陈旧/默认的电机反馈。
🛠️ 建议修改
- void update() override { force_sensor_.update_status(); }
+ void update() override {
+ pitch_control_motor_.update_status();
+ yaw_control_motor_.update_status();
+ screw_motor_.update_status();
+ left_belt_motor_.update_status();
+ right_belt_motor_.update_status();
+ force_sensor_.update_status();
+ }Also applies to: 101-112
🤖 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/catapult_dart_librmcs_v3.cpp` at line 56,
update() only calls force_sensor_.update_status() and therefore never publishes
the motor states populated by can2_receive_callback() -> store_status(), causing
stale motor feedback; modify update() to, after force_sensor_.update_status(),
iterate over the stored motor status entries (from store_status()) and invoke
the existing publish routines for those statuses (e.g.,
publish_motor_status()/publish_all_motor_states() or the project’s equivalent
publish methods) so all motor states are emitted to the controller.
| std::atomic<ImuData> accelerometer_data_, gyroscope_data_; | ||
| static_assert(std::atomic<ImuData>::is_always_lock_free); | ||
|
|
||
| double ax_, ay_, az_, gx_, gy_, gz_; |
There was a problem hiding this comment.
成员变量未初始化。
ax_, ay_, az_, gx_, gy_, gz_ 未在构造函数中初始化。如果在调用 update_status() 之前调用 getter 方法，将读取到不确定的值。
🔧 建议的修复
- double ax_, ay_, az_, gx_, gy_, gz_;
+ double ax_ = 0.0, ay_ = 0.0, az_ = 0.0, gx_ = 0.0, gy_ = 0.0, gz_ = 0.0;📝 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
| double ax_, ay_, az_, gx_, gy_, gz_; | |
| double ax_ = 0.0, ay_ = 0.0, az_ = 0.0, gx_ = 0.0, gy_ = 0.0, gz_ = 0.0; |
🤖 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/device/bmi088.hpp` at line 156,
构造函数中未初始化成员 ax_, ay_, az_, gx_, gy_, gz_; 在类 BMI088（或其构造函数
BMI088::BMI088）中将这些成员初始化为明确的初始值（例如 0.0），可以在构造函数的成员初始化列表中设置 ax_(0.0), ay_(0.0),
az_(0.0), gx_(0.0), gy_(0.0), gz_(0.0) 或在声明处给出默认值，以确保在调用 getter 或
update_status() 之前不会读取未定义的值。
Comment on lines
+1
to
+528
| // #include <memory> | ||
| // #include <thread> | ||
|
|
||
| // #include <librmcs/client/cboard.hpp> | ||
| // #include <rclcpp/node.hpp> | ||
| // #include <rmcs_description/tf_description.hpp> | ||
| // #include <rmcs_executor/component.hpp> | ||
| // #include <rmcs_msgs/serial_interface.hpp> | ||
| // #include <rmcs_utility/tick_timer.hpp> | ||
| // #include <std_msgs/msg/int32.hpp> | ||
|
|
||
| // #include "hardware/device/benewake.hpp" | ||
| // #include "hardware/device/bmi088.hpp" | ||
| // #include "hardware/device/dji_motor.hpp" | ||
| // #include "hardware/device/dr16.hpp" | ||
| // #include "hardware/device/lk_motor.hpp" | ||
| // #include "hardware/device/supercap.hpp" | ||
|
|
||
| // namespace rmcs_core::hardware { | ||
|
|
||
| // class SteeringHero | ||
| // : public rmcs_executor::Component | ||
| // , public rclcpp::Node { | ||
| // public: | ||
| // SteeringHero() | ||
| // : Node{get_component_name(), | ||
| // rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , | ||
| // command_component_( | ||
| // create_partner_component<SteeringHeroCommand>( | ||
| // get_component_name() + "_command", *this)) { | ||
| // using namespace rmcs_description; | ||
|
|
||
| // register_output("/tf", tf_); | ||
| // tf_->set_transform<PitchLink, CameraLink>(Eigen::Translation3d{0.16, 0.0, 0.15}); | ||
|
|
||
| // gimbal_calibrate_subscription_ = create_subscription<std_msgs::msg::Int32>( | ||
| // "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { | ||
| // gimbal_calibrate_subscription_callback(std::move(msg)); | ||
| // }); | ||
|
|
||
| // top_board_ = std::make_unique<TopBoard>( | ||
| // *this, *command_component_, | ||
| // static_cast<int>(get_parameter("usb_pid_top_board").as_int())); | ||
| // bottom_board_ = std::make_unique<BottomBoard>( | ||
| // *this, *command_component_, | ||
| // static_cast<int>(get_parameter("usb_pid_bottom_board").as_int())); | ||
|
|
||
| // temperature_logging_timer_.reset(1000); | ||
| // } | ||
|
|
||
| // ~SteeringHero() override = default; | ||
|
|
||
| // void update() override { | ||
| // top_board_->update(); | ||
| // bottom_board_->update(); | ||
|
|
||
| // if (temperature_logging_timer_.tick()) { | ||
| // temperature_logging_timer_.reset(1000); | ||
| // RCLCPP_INFO( | ||
| // get_logger(), | ||
| // "Temperature: pitch: %.1f, top_yaw: %.1f, bottom_yaw: %.1f, feeder: %.1f", | ||
| // top_board_->gimbal_pitch_motor_.temperature(), | ||
| // top_board_->gimbal_top_yaw_motor_.temperature(), | ||
| // bottom_board_->gimbal_bottom_yaw_motor_.temperature(), | ||
| // top_board_->gimbal_bullet_feeder_.temperature()); | ||
| // } | ||
| // } | ||
|
|
||
| // void command_update() { | ||
| // top_board_->command_update(); | ||
| // bottom_board_->command_update(); | ||
| // } | ||
|
|
||
| // private: | ||
| // void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", | ||
| // bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[gimbal calibration] New pitch offset: %ld", | ||
| // top_board_->gimbal_pitch_motor_.calibrate_zero_point()); | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[gimbal calibration] New top yaw offset: %ld", | ||
| // top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[gimbal calibration] New viewer offset: %ld", | ||
| // top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[chassis calibration] left front steering offset: %d", | ||
| // bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[chassis calibration] left back steering offset: %d", | ||
| // bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[chassis calibration] right back steering offset: %d", | ||
| // bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[chassis calibration] right front steering offset: %d", | ||
| // bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); | ||
| // } | ||
|
|
||
| // class SteeringHeroCommand : public rmcs_executor::Component { | ||
| // public: | ||
| // explicit SteeringHeroCommand(SteeringHero& hero) | ||
| // : hero_(hero) {} | ||
|
|
||
| // void update() override { hero_.command_update(); } | ||
|
|
||
| // SteeringHero& hero_; | ||
| // }; | ||
| // std::shared_ptr<SteeringHeroCommand> command_component_; | ||
|
|
||
| // class TopBoard final : private librmcs::client::CBoard { | ||
| // public: | ||
| // friend class SteeringHero; | ||
| // explicit TopBoard(SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = | ||
| // -1) | ||
| // : librmcs::client::CBoard(usb_pid) | ||
| // , tf_(hero.tf_) | ||
| // , imu_(1000, 0.2, 0.0) | ||
| // , benewake_(hero, "/gimbal/auto_aim/laser_distance") | ||
| // , gimbal_top_yaw_motor_( | ||
| // hero, hero_command, "/gimbal/top_yaw", | ||
| // device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>( | ||
| // hero.get_parameter("top_yaw_motor_zero_point").as_int()))) | ||
| // , gimbal_pitch_motor_( | ||
| // hero, hero_command, "/gimbal/pitch", | ||
| // device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>(hero.get_parameter("pitch_motor_zero_point").as_int()))) | ||
| // , gimbal_friction_wheels_( | ||
| // {hero, hero_command, "/gimbal/second_left_friction", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, | ||
| // {hero, hero_command, "/gimbal/first_left_friction", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, | ||
| // {hero, hero_command, "/gimbal/first_right_friction", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508} | ||
| // .set_reduction_ratio(1.) | ||
| // .set_reversed()}, | ||
| // {hero, hero_command, "/gimbal/second_right_friction", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508} | ||
| // .set_reduction_ratio(1.) | ||
| // .set_reversed()}) | ||
| // , gimbal_bullet_feeder_( | ||
| // hero, hero_command, "/gimbal/bullet_feeder", | ||
| // device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} | ||
| // .set_reversed() | ||
| // .enable_multi_turn_angle()) | ||
| // , gimbal_scope_motor_( | ||
| // hero, hero_command, "/gimbal/scope", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M2006}) | ||
| // , gimbal_player_viewer_motor_( | ||
| // hero, hero_command, "/gimbal/player_viewer", | ||
| // device::LkMotor::Config{device::LkMotor::Type::MG4005E_I10} | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>(hero.get_parameter("viewer_motor_zero_point").as_int())) | ||
| // .set_reversed()) | ||
| // , transmit_buffer_(*this, 32) | ||
| // , event_thread_([this]() { handle_events(); }) { | ||
|
|
||
| // imu_.set_coordinate_mapping([](double x, double y, double z) { | ||
| // // Get the mapping with the following code. | ||
| // // The rotation angle must be an exact multiple of 90 degrees, otherwise use a | ||
| // // matrix. | ||
|
|
||
| // // Eigen::AngleAxisd pitch_link_to_imu_link{ | ||
| // // std::numbers::pi, Eigen::Vector3d::UnitZ()}; | ||
| // // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; | ||
| // // std::cout << mapping << std::endl; | ||
|
|
||
| // return std::make_tuple(-x, -y, z); | ||
| // }); | ||
|
|
||
| // hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); | ||
| // hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); | ||
| // } | ||
|
|
||
| // ~TopBoard() final { | ||
| // stop_handling_events(); | ||
| // event_thread_.join(); | ||
| // } | ||
|
|
||
| // void update() { | ||
| // imu_.update_status(); | ||
| // Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; | ||
|
|
||
| // tf_->set_transform<rmcs_description::PitchLink, rmcs_description::OdomImu>( | ||
| // gimbal_imu_pose.conjugate()); | ||
|
|
||
| // benewake_.update_status(); | ||
|
|
||
| // *gimbal_yaw_velocity_imu_ = imu_.gz(); | ||
| // *gimbal_pitch_velocity_imu_ = imu_.gy(); | ||
|
|
||
| // gimbal_top_yaw_motor_.update_status(); | ||
|
|
||
| // gimbal_pitch_motor_.update_status(); | ||
| // tf_->set_state<rmcs_description::YawLink, rmcs_description::PitchLink>( | ||
| // gimbal_pitch_motor_.angle()); | ||
|
|
||
| // gimbal_player_viewer_motor_.update_status(); | ||
| // tf_->set_state<rmcs_description::PitchLink, rmcs_description::ViewerLink>( | ||
| // gimbal_player_viewer_motor_.angle()); | ||
|
|
||
| // gimbal_scope_motor_.update_status(); | ||
|
|
||
| // for (auto& motor : gimbal_friction_wheels_) | ||
| // motor.update_status(); | ||
|
|
||
| // gimbal_bullet_feeder_.update_status(); | ||
| // } | ||
|
|
||
| // void command_update() { | ||
| // uint16_t batch_commands[4]{}; | ||
|
|
||
| // for (int i = 0; i < 4; i++) | ||
| // batch_commands[i] = gimbal_friction_wheels_[i].generate_command(); | ||
| // transmit_buffer_.add_can1_transmission(0x200, | ||
| // std::bit_cast<uint64_t>(batch_commands)); | ||
|
|
||
| // transmit_buffer_.add_can1_transmission( | ||
| // 0x141, gimbal_bullet_feeder_.generate_torque_command( | ||
| // gimbal_bullet_feeder_.control_torque())); | ||
|
|
||
| // batch_commands[0] = gimbal_scope_motor_.generate_command(); | ||
| // transmit_buffer_.add_can2_transmission(0x200, | ||
| // std::bit_cast<uint64_t>(batch_commands)); | ||
|
|
||
| // transmit_buffer_.add_can2_transmission( | ||
| // 0x143, gimbal_player_viewer_motor_.generate_velocity_command( | ||
| // gimbal_player_viewer_motor_.control_velocity())); | ||
|
|
||
| // transmit_buffer_.add_can2_transmission(0x141, | ||
| // gimbal_top_yaw_motor_.generate_command()); | ||
| // transmit_buffer_.add_can2_transmission(0x142, | ||
| // gimbal_pitch_motor_.generate_command()); | ||
|
|
||
| // transmit_buffer_.trigger_transmission(); | ||
| // } | ||
|
|
||
| // private: | ||
| // void can1_receive_callback( | ||
| // uint32_t can_id, uint64_t can_data, bool is_extended_can_id, | ||
| // bool is_remote_transmission, uint8_t can_data_length) override { | ||
| // if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| // return; | ||
|
|
||
| // if (can_id == 0x201) { | ||
| // gimbal_friction_wheels_[0].store_status(can_data); | ||
| // } else if (can_id == 0x202) { | ||
| // gimbal_friction_wheels_[1].store_status(can_data); | ||
| // } else if (can_id == 0x203) { | ||
| // gimbal_friction_wheels_[2].store_status(can_data); | ||
| // } else if (can_id == 0x204) { | ||
| // gimbal_friction_wheels_[3].store_status(can_data); | ||
| // } else if (can_id == 0x141) { | ||
| // gimbal_bullet_feeder_.store_status(can_data); | ||
| // } | ||
| // } | ||
|
|
||
| // void can2_receive_callback( | ||
| // uint32_t can_id, uint64_t can_data, bool is_extended_can_id, | ||
| // bool is_remote_transmission, uint8_t can_data_length) override { | ||
| // if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| // return; | ||
|
|
||
| // if (can_id == 0x141) { | ||
| // gimbal_top_yaw_motor_.store_status(can_data); | ||
| // } else if (can_id == 0x142) { | ||
| // gimbal_pitch_motor_.store_status(can_data); | ||
| // } else if (can_id == 0x143) { | ||
| // gimbal_player_viewer_motor_.store_status(can_data); | ||
| // } else if (can_id == 0x201) { | ||
| // gimbal_scope_motor_.store_status(can_data); | ||
| // } | ||
| // } | ||
|
|
||
| // void uart2_receive_callback(const std::byte* data, uint8_t length) override { | ||
| // benewake_.store_status(data, length); | ||
| // } | ||
|
|
||
| // void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| // imu_.store_accelerometer_status(x, y, z); | ||
| // } | ||
|
|
||
| // void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| // imu_.store_gyroscope_status(x, y, z); | ||
| // } | ||
|
|
||
| // OutputInterface<rmcs_description::Tf>& tf_; | ||
|
|
||
| // device::Bmi088 imu_; | ||
| // device::Benewake benewake_; | ||
|
|
||
| // OutputInterface<double> gimbal_yaw_velocity_imu_; | ||
| // OutputInterface<double> gimbal_pitch_velocity_imu_; | ||
|
|
||
| // device::LkMotor gimbal_top_yaw_motor_; | ||
| // device::LkMotor gimbal_pitch_motor_; | ||
|
|
||
| // device::DjiMotor gimbal_friction_wheels_[4]; | ||
| // device::LkMotor gimbal_bullet_feeder_; | ||
|
|
||
| // device::DjiMotor gimbal_scope_motor_; | ||
| // device::LkMotor gimbal_player_viewer_motor_; | ||
|
|
||
| // librmcs::client::CBoard::TransmitBuffer transmit_buffer_; | ||
| // std::thread event_thread_; | ||
| // }; | ||
|
|
||
| // class BottomBoard final : private librmcs::client::CBoard { | ||
| // public: | ||
| // friend class SteeringHero; | ||
| // explicit BottomBoard( | ||
| // SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = -1) | ||
| // : librmcs::client::CBoard(usb_pid) | ||
| // , imu_(1000, 0.2, 0.0) | ||
| // , tf_(hero.tf_) | ||
| // , dr16_(hero) | ||
| // , chassis_steering_motors_( | ||
| // {hero, hero_command, "/chassis/left_front_steering", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>(hero.get_parameter("left_front_zero_point").as_int())) | ||
| // .set_reversed()}, | ||
| // {hero, hero_command, "/chassis/left_back_steering", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>(hero.get_parameter("left_back_zero_point").as_int())) | ||
| // .set_reversed()}, | ||
| // {hero, hero_command, "/chassis/right_back_steering", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>(hero.get_parameter("right_back_zero_point").as_int())) | ||
| // .set_reversed()}, | ||
| // {hero, hero_command, "/chassis/right_front_steering", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>(hero.get_parameter("right_front_zero_point").as_int())) | ||
| // .set_reversed()}) | ||
| // , chassis_wheel_motors_( | ||
| // {hero, hero_command, "/chassis/left_front_wheel", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508} | ||
| // .set_reversed() | ||
| // .set_reduction_ratio(2232. / 169.)}, | ||
| // {hero, hero_command, "/chassis/left_back_wheel", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508} | ||
| // .set_reversed() | ||
| // .set_reduction_ratio(2232. / 169.)}, | ||
| // {hero, hero_command, "/chassis/right_back_wheel", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508} | ||
| // .set_reversed() | ||
| // .set_reduction_ratio(2232. / 169.)}, | ||
| // {hero, hero_command, "/chassis/right_front_wheel", | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508} | ||
| // .set_reversed() | ||
| // .set_reduction_ratio(2232. / 169.)}) | ||
| // , supercap_(hero, hero_command) | ||
| // , gimbal_bottom_yaw_motor_( | ||
| // hero, hero_command, "/gimbal/bottom_yaw", | ||
| // device::LkMotor::Config{device::LkMotor::Type::MG6012E_I8} | ||
| // .set_reversed() | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>( | ||
| // hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))) | ||
| // , transmit_buffer_(*this, 32) | ||
| // , event_thread_([this]() { handle_events(); }) { | ||
|
|
||
| // hero.register_output("/referee/serial", referee_serial_); | ||
| // referee_serial_->read = [this](std::byte* buffer, size_t size) { | ||
| // return referee_ring_buffer_receive_.pop_front_multi( | ||
| // [&buffer](std::byte byte) { *buffer++ = byte; }, size); | ||
| // }; | ||
| // referee_serial_->write = [this](const std::byte* buffer, size_t size) { | ||
| // transmit_buffer_.add_uart1_transmission(buffer, size); | ||
| // return size; | ||
| // }; | ||
|
|
||
| // hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); | ||
|
|
||
| // hero.register_output( | ||
| // "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); | ||
| // hero.register_output( | ||
| // "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); | ||
| // } | ||
|
|
||
| // ~BottomBoard() final { | ||
| // stop_handling_events(); | ||
| // event_thread_.join(); | ||
| // } | ||
|
|
||
| // void update() { | ||
| // imu_.update_status(); | ||
| // dr16_.update_status(); | ||
| // supercap_.update_status(); | ||
|
|
||
| // *chassis_yaw_velocity_imu_ = imu_.gz(); | ||
|
|
||
| // for (auto& motor : chassis_wheel_motors_) | ||
| // motor.update_status(); | ||
| // for (auto& motor : chassis_steering_motors_) | ||
| // motor.update_status(); | ||
| // gimbal_bottom_yaw_motor_.update_status(); | ||
| // tf_->set_state<rmcs_description::GimbalCenterLink, rmcs_description::YawLink>( | ||
| // gimbal_bottom_yaw_motor_.angle()); | ||
| // } | ||
|
|
||
| // void command_update() { | ||
| // uint16_t batch_commands[4]{}; | ||
|
|
||
| // for (int i = 0; i < 4; i++) | ||
| // batch_commands[i] = chassis_wheel_motors_[i].generate_command(); | ||
| // transmit_buffer_.add_can1_transmission(0x200, | ||
| // std::bit_cast<uint64_t>(batch_commands)); | ||
|
|
||
| // batch_commands[3] = supercap_.generate_command(); | ||
| // transmit_buffer_.add_can1_transmission(0x1FE, | ||
| // std::bit_cast<uint64_t>(batch_commands)); | ||
|
|
||
| // for (int i = 0; i < 4; i++) | ||
| // batch_commands[i] = chassis_steering_motors_[i].generate_command(); | ||
| // transmit_buffer_.add_can2_transmission(0x1FE, | ||
| // std::bit_cast<uint64_t>(batch_commands)); | ||
|
|
||
| // transmit_buffer_.add_can2_transmission( | ||
| // 0x141, gimbal_bottom_yaw_motor_.generate_command()); | ||
|
|
||
| // transmit_buffer_.trigger_transmission(); | ||
| // } | ||
|
|
||
| // private: | ||
| // void can1_receive_callback( | ||
| // uint32_t can_id, uint64_t can_data, bool is_extended_can_id, | ||
| // bool is_remote_transmission, uint8_t can_data_length) override { | ||
| // if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| // return; | ||
|
|
||
| // if (can_id == 0x201) { | ||
| // chassis_wheel_motors_[0].store_status(can_data); | ||
| // } else if (can_id == 0x202) { | ||
| // chassis_wheel_motors_[1].store_status(can_data); | ||
| // } else if (can_id == 0x203) { | ||
| // chassis_wheel_motors_[2].store_status(can_data); | ||
| // } else if (can_id == 0x204) { | ||
| // chassis_wheel_motors_[3].store_status(can_data); | ||
| // } else if (can_id == 0x300) { | ||
| // supercap_.store_status(can_data); | ||
| // } | ||
| // } | ||
|
|
||
| // void can2_receive_callback( | ||
| // uint32_t can_id, uint64_t can_data, bool is_extended_can_id, | ||
| // bool is_remote_transmission, uint8_t can_data_length) override { | ||
| // if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| // return; | ||
|
|
||
| // if (can_id == 0x205) { | ||
| // chassis_steering_motors_[0].store_status(can_data); | ||
| // } else if (can_id == 0x206) { | ||
| // chassis_steering_motors_[1].store_status(can_data); | ||
| // } else if (can_id == 0x207) { | ||
| // chassis_steering_motors_[2].store_status(can_data); | ||
| // } else if (can_id == 0x208) { | ||
| // chassis_steering_motors_[3].store_status(can_data); | ||
| // } else if (can_id == 0x141) { | ||
| // gimbal_bottom_yaw_motor_.store_status(can_data); | ||
| // } | ||
| // } | ||
|
|
||
| // void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) | ||
| // override { | ||
| // referee_ring_buffer_receive_.emplace_back_multi( | ||
| // [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); | ||
| // } | ||
|
|
||
| // void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override | ||
| // { | ||
| // dr16_.store_status(uart_data, uart_data_length); | ||
| // } | ||
|
|
||
| // void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| // imu_.store_accelerometer_status(x, y, z); | ||
| // } | ||
|
|
||
| // void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| // imu_.store_gyroscope_status(x, y, z); | ||
| // } | ||
|
|
||
| // device::Bmi088 imu_; | ||
| // OutputInterface<rmcs_description::Tf>& tf_; | ||
|
|
||
| // OutputInterface<double> chassis_yaw_velocity_imu_; | ||
|
|
||
| // OutputInterface<bool> powermeter_control_enabled_; | ||
| // OutputInterface<double> powermeter_charge_power_limit_; | ||
|
|
||
| // device::Dr16 dr16_; | ||
|
|
||
| // device::DjiMotor chassis_steering_motors_[4]; | ||
| // device::DjiMotor chassis_wheel_motors_[4]; | ||
| // device::Supercap supercap_; | ||
|
|
||
| // device::LkMotor gimbal_bottom_yaw_motor_; | ||
|
|
||
| // librmcs::utility::RingBuffer<std::byte> referee_ring_buffer_receive_{256}; | ||
| // OutputInterface<rmcs_msgs::SerialInterface> referee_serial_; | ||
|
|
||
| // librmcs::client::CBoard::TransmitBuffer transmit_buffer_; | ||
| // std::thread event_thread_; | ||
| // }; | ||
|
|
||
| // OutputInterface<rmcs_description::Tf> tf_; | ||
|
|
||
| // rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr gimbal_calibrate_subscription_; | ||
|
|
||
| // std::unique_ptr<TopBoard> top_board_; | ||
| // std::unique_ptr<BottomBoard> bottom_board_; | ||
|
|
||
| // rmcs_utility::TickTimer temperature_logging_timer_; | ||
| // }; | ||
|
|
||
| // } // namespace rmcs_core::hardware | ||
|
|
||
| // #include <pluginlib/class_list_macros.hpp> | ||
|
|
||
| // PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero, rmcs_executor::Component) No newline at end of file |
There was a problem hiding this comment.
恢复 SteeringHero 插件，或同步移除其注册。
这里把整个实现连同 PLUGINLIB_EXPORT_CLASS 一起注释掉后，rmcs_core::hardware::SteeringHero 已经不会被编译或导出；但 rmcs_ws/src/rmcs_core/plugins.xml:5-19 仍然声明了这个插件。只要现有启动配置还去加载它，运行时就会直接报类未注册/不存在。若这是有意下线，请在本次变更里同步删除 plugins.xml 中的 SteeringHero 注册及其所有引用；否则需要恢复一个可编译、可导出的实现。
🤖 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/steering-hero.cpp` around lines 1 - 528,
The SteeringHero implementation and its PLUGINLIB_EXPORT_CLASS registration were
fully commented out, but plugins.xml still declares
rmcs_core::hardware::SteeringHero, causing runtime plugin-not-found errors;
either restore a compilable SteeringHero class and re-enable
PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero,
rmcs_executor::Component) (ensure class name SteeringHero and the export macro
are present and compile), or remove the SteeringHero entry and any references
from plugins.xml (and any launch/config that attempts to load it) so the plugin
declaration and the code stay in sync.
Comment on lines
1
to
+443
|
|
||
| #include <memory> | ||
|
|
||
| #include <rclcpp/logger.hpp> | ||
| #include <rclcpp/logging.hpp> | ||
| #include <rclcpp/node.hpp> | ||
| #include <rmcs_description/tf_description.hpp> | ||
| #include <rmcs_executor/component.hpp> | ||
| #include <rmcs_msgs/serial_interface.hpp> | ||
| #include <std_msgs/msg/int32.hpp> | ||
|
|
||
| #include "hardware/device/bmi088.hpp" | ||
| #include "hardware/device/dji_motor.hpp" | ||
| #include "hardware/device/dr16.hpp" | ||
| #include "hardware/device/lk_motor.hpp" | ||
| #include "hardware/device/supercap.hpp" | ||
| #include "librmcs/client/cboard.hpp" | ||
|
|
||
| namespace rmcs_core::hardware { | ||
|
|
||
| class SteeringInfantry | ||
| : public rmcs_executor::Component | ||
| , public rclcpp::Node { | ||
| public: | ||
| SteeringInfantry() | ||
| : Node( | ||
| get_component_name(), | ||
| rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) | ||
| , command_component_( | ||
| create_partner_component<SteeringInfantryCommand>( | ||
| get_component_name() + "_command", *this)) { | ||
| using namespace rmcs_description; | ||
| register_output("/tf", tf_); | ||
|
|
||
| gimbal_calibrate_subscription_ = create_subscription<std_msgs::msg::Int32>( | ||
| "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { | ||
| gimbal_calibrate_subscription_callback(std::move(msg)); | ||
| }); | ||
| steers_calibrate_subscription_ = create_subscription<std_msgs::msg::Int32>( | ||
| "/steers/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { | ||
| steers_calibrate_subscription_callback(std::move(msg)); | ||
| }); | ||
|
|
||
| top_board_ = std::make_unique<TopBoard>( | ||
| *this, *command_component_, | ||
| static_cast<int>(get_parameter("usb_pid_top_board").as_int())); | ||
|
|
||
| bottom_board_ = std::make_unique<BottomBoard>( | ||
| *this, *command_component_, | ||
| static_cast<int>(get_parameter("usb_pid_bottom_board").as_int())); | ||
|
|
||
| using namespace rmcs_description; | ||
| tf_->set_transform<PitchLink, CameraLink>(Eigen::Translation3d{0.06603, 0.0, 0.082}); | ||
| } | ||
| ~SteeringInfantry() override = default; | ||
|
|
||
| void update() override { | ||
| top_board_->update(); | ||
| bottom_board_->update(); | ||
| } | ||
|
|
||
| void command_update() { | ||
|
|
||
| top_board_->command_update(); | ||
| bottom_board_->command_update(); | ||
| } | ||
|
|
||
| private: | ||
| void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { | ||
| RCLCPP_INFO( | ||
| get_logger(), "[gimbal calibration] New yaw offset: %ld", | ||
| bottom_board_->gimbal_yaw_motor_.calibrate_zero_point()); | ||
| RCLCPP_INFO( | ||
| get_logger(), "[gimbal calibration] New pitch offset: %ld", | ||
| top_board_->gimbal_pitch_motor_.calibrate_zero_point()); | ||
| } | ||
|
|
||
| void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { | ||
| RCLCPP_INFO( | ||
| get_logger(), "[steer calibration] New left front offset: %d", | ||
| bottom_board_->chassis_steer_motors_[0].calibrate_zero_point()); | ||
| RCLCPP_INFO( | ||
| get_logger(), "[steer calibration] New left back offset: %d", | ||
| bottom_board_->chassis_steer_motors_[1].calibrate_zero_point()); | ||
| RCLCPP_INFO( | ||
| get_logger(), "[steer calibration] New right back offset: %d", | ||
| bottom_board_->chassis_steer_motors_[2].calibrate_zero_point()); | ||
| RCLCPP_INFO( | ||
| get_logger(), "[steer calibration] New right front offset: %d", | ||
| bottom_board_->chassis_steer_motors_[3].calibrate_zero_point()); | ||
| } | ||
| class SteeringInfantryCommand : public rmcs_executor::Component { | ||
| public: | ||
| explicit SteeringInfantryCommand(SteeringInfantry& steeringInfantry) | ||
| : steeringInfantry(steeringInfantry) {} | ||
|
|
||
| void update() override { steeringInfantry.command_update(); } | ||
|
|
||
| SteeringInfantry& steeringInfantry; | ||
| }; | ||
|
|
||
| class TopBoard final : private librmcs::client::CBoard { | ||
| public: | ||
| friend class SteeringInfantry; | ||
| explicit TopBoard( | ||
| SteeringInfantry& steeringInfantry, SteeringInfantryCommand& steeringInfantry_command, | ||
| int usb_pid = -1) | ||
| : CBoard(usb_pid) | ||
| , tf_(steeringInfantry.tf_) | ||
| , bmi088_(1000, 0.2, 0.0) | ||
| , gimbal_pitch_motor_(steeringInfantry, steeringInfantry_command, "/gimbal/pitch") | ||
| , gimbal_left_friction_( | ||
| steeringInfantry, steeringInfantry_command, "/gimbal/left_friction") | ||
| , gimbal_right_friction_( | ||
| steeringInfantry, steeringInfantry_command, "/gimbal/right_friction") | ||
| , transmit_buffer_(*this, 32) | ||
| , event_thread_([this]() { handle_events(); }) { | ||
| gimbal_pitch_motor_.configure( | ||
| device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( | ||
| static_cast<int>( | ||
| steeringInfantry.get_parameter("pitch_motor_zero_point").as_int()))); | ||
|
|
||
| gimbal_left_friction_.configure( | ||
| device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)); | ||
| gimbal_right_friction_.configure( | ||
| device::DjiMotor::Config{device::DjiMotor::Type::M3508} | ||
| .set_reduction_ratio(1.) | ||
| .set_reversed()); | ||
|
|
||
| steeringInfantry.register_output( | ||
| "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); | ||
| steeringInfantry.register_output( | ||
| "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); | ||
|
|
||
| bmi088_.set_coordinate_mapping([](double x, double y, double z) { | ||
| // Get the mapping with the following code. | ||
| // The rotation angle must be an exact multiple of 90 degrees, otherwise | ||
| // use a matrix. | ||
|
|
||
| // Eigen::AngleAxisd pitch_link_to_bmi088_link{ | ||
| // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; | ||
| // Eigen::Vector3d mapping = pitch_link_to_bmi088_link * Eigen::Vector3d{1, 2, 3}; | ||
| // std::cout << mapping << std::endl; | ||
|
|
||
| return std::make_tuple(x, y, z); | ||
| }); | ||
| } | ||
| ~TopBoard() { | ||
| stop_handling_events(); | ||
| event_thread_.join(); | ||
| } | ||
| void update() { | ||
| bmi088_.update_status(); | ||
| Eigen::Quaterniond gimbal_bmi088_pose{ | ||
| bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; | ||
|
|
||
| tf_->set_transform<rmcs_description::PitchLink, rmcs_description::OdomImu>( | ||
| gimbal_bmi088_pose.conjugate()); | ||
|
|
||
| *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); | ||
| *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); | ||
|
|
||
| gimbal_pitch_motor_.update_status(); | ||
| gimbal_left_friction_.update_status(); | ||
| gimbal_right_friction_.update_status(); | ||
|
|
||
| tf_->set_state<rmcs_description::YawLink, rmcs_description::PitchLink>( | ||
| gimbal_pitch_motor_.angle()); | ||
| } | ||
|
|
||
| void command_update() { | ||
| uint16_t can_commands[4]; | ||
| can_commands[2] = gimbal_left_friction_.generate_command(); | ||
| can_commands[3] = gimbal_right_friction_.generate_command(); | ||
| transmit_buffer_.add_can1_transmission(0x200, std::bit_cast<uint64_t>(can_commands)); | ||
|
|
||
| transmit_buffer_.add_can2_transmission( | ||
| 0x142, gimbal_pitch_motor_.generate_velocity_command( | ||
| gimbal_pitch_motor_.control_velocity())); | ||
|
|
||
| transmit_buffer_.trigger_transmission(); | ||
| } | ||
|
|
||
| private: | ||
| void can1_receive_callback( | ||
| uint32_t can_id, uint64_t can_data, bool is_extended_can_id, | ||
| bool is_remote_transmission, uint8_t can_data_length) override { | ||
| if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| return; | ||
| if (can_id == 0x203) { | ||
| gimbal_left_friction_.store_status(can_data); | ||
| } else if (can_id == 0x204) { | ||
| gimbal_right_friction_.store_status(can_data); | ||
| } | ||
| } | ||
| void can2_receive_callback( | ||
| uint32_t can_id, uint64_t can_data, bool is_extended_can_id, | ||
| bool is_remote_transmission, uint8_t can_data_length) override { | ||
| if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| return; | ||
| if (can_id == 0x142) | ||
| gimbal_pitch_motor_.store_status(can_data); | ||
| } | ||
| void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| bmi088_.store_accelerometer_status(x, y, z); | ||
| } | ||
| void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| bmi088_.store_gyroscope_status(x, y, z); | ||
| } | ||
| OutputInterface<rmcs_description::Tf>& tf_; | ||
|
|
||
| OutputInterface<double> gimbal_yaw_velocity_bmi088_; | ||
| OutputInterface<double> gimbal_pitch_velocity_bmi088_; | ||
|
|
||
| device::Bmi088 bmi088_; | ||
| device::LkMotor gimbal_pitch_motor_; | ||
| device::DjiMotor gimbal_left_friction_; | ||
| device::DjiMotor gimbal_right_friction_; | ||
|
|
||
| TransmitBuffer transmit_buffer_; | ||
| std::thread event_thread_; | ||
| }; | ||
| class BottomBoard final : private librmcs::client::CBoard { | ||
| public: | ||
| friend class SteeringInfantry; | ||
|
|
||
| explicit BottomBoard( | ||
| SteeringInfantry& steeringInfantry, SteeringInfantryCommand& steeringInfantry_command, | ||
| int usb_pid = -1) | ||
| : librmcs::client::CBoard(usb_pid) | ||
| , imu_(1000, 0.2, 0.0) | ||
| , tf_(steeringInfantry.tf_) | ||
| , dr16_(steeringInfantry) | ||
| , gimbal_yaw_motor_(steeringInfantry, steeringInfantry_command, "/gimbal/yaw") | ||
| , gimbal_bullet_feeder_( | ||
| steeringInfantry, steeringInfantry_command, "/gimbal/bullet_feeder") | ||
| , chassis_wheel_motors_( | ||
| {steeringInfantry, steeringInfantry_command, "/chassis/left_front_wheel"}, | ||
| {steeringInfantry, steeringInfantry_command, "/chassis/left_back_wheel"}, | ||
| {steeringInfantry, steeringInfantry_command, "/chassis/right_back_wheel"}, | ||
| {steeringInfantry, steeringInfantry_command, "/chassis/right_front_wheel"}) | ||
| , chassis_steer_motors_( | ||
| {steeringInfantry, steeringInfantry_command, "/chassis/left_front_steering"}, | ||
| {steeringInfantry, steeringInfantry_command, "/chassis/left_back_steering"}, | ||
| {steeringInfantry, steeringInfantry_command, "/chassis/right_back_steering"}, | ||
| {steeringInfantry, steeringInfantry_command, "/chassis/right_front_steering"}) | ||
| , supercap_(steeringInfantry, steeringInfantry_command) | ||
| , transmit_buffer_(*this, 32) | ||
| , event_thread_([this]() { handle_events(); }) { | ||
|
|
||
| steeringInfantry.register_output("/referee/serial", referee_serial_); | ||
|
|
||
| referee_serial_->read = [this](std::byte* buffer, size_t size) { | ||
| return referee_ring_buffer_receive_.pop_front_multi( | ||
| [&buffer](std::byte byte) { *buffer++ = byte; }, size); | ||
| }; | ||
| referee_serial_->write = [this](const std::byte* buffer, size_t size) { | ||
| transmit_buffer_.add_uart1_transmission(buffer, size); | ||
| return size; | ||
| }; | ||
| gimbal_yaw_motor_.configure( | ||
| device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( | ||
| static_cast<int>( | ||
| steeringInfantry.get_parameter("yaw_motor_zero_point").as_int()))); | ||
|
|
||
| gimbal_bullet_feeder_.configure( | ||
| device::DjiMotor::Config{device::DjiMotor::Type::M2006} | ||
| .enable_multi_turn_angle() | ||
| .set_reversed() | ||
| .set_reduction_ratio(19 * 2)); | ||
|
|
||
| for (auto& motor : chassis_wheel_motors_) | ||
| motor.configure( | ||
| device::DjiMotor::Config{device::DjiMotor::Type::M3508} | ||
|
|
||
| .set_reduction_ratio(11.) | ||
| .enable_multi_turn_angle() | ||
| .set_reversed()); | ||
| chassis_steer_motors_[0].configure( | ||
| device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| .set_reversed() | ||
| .set_encoder_zero_point( | ||
| static_cast<int>( | ||
| steeringInfantry.get_parameter("left_front_zero_point").as_int())) | ||
| .enable_multi_turn_angle()); | ||
| chassis_steer_motors_[1].configure( | ||
| device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| .set_reversed() | ||
| .set_encoder_zero_point( | ||
| static_cast<int>( | ||
| steeringInfantry.get_parameter("left_back_zero_point").as_int())) | ||
| .enable_multi_turn_angle()); | ||
| chassis_steer_motors_[2].configure( | ||
| device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| .set_reversed() | ||
| .set_encoder_zero_point( | ||
| static_cast<int>( | ||
| steeringInfantry.get_parameter("right_back_zero_point").as_int())) | ||
| .enable_multi_turn_angle()); | ||
| chassis_steer_motors_[3].configure( | ||
| device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| .set_reversed() | ||
| .set_encoder_zero_point( | ||
| static_cast<int>( | ||
| steeringInfantry.get_parameter("right_front_zero_point").as_int())) | ||
| .enable_multi_turn_angle()); | ||
| steeringInfantry.register_output( | ||
| "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); | ||
| } | ||
| ~BottomBoard() final { | ||
| stop_handling_events(); | ||
| event_thread_.join(); | ||
| } | ||
| void update() { | ||
| imu_.update_status(); | ||
| *chassis_yaw_velocity_imu_ = imu_.gy(); | ||
| supercap_.update_status(); | ||
|
|
||
| for (auto& motor : chassis_wheel_motors_) | ||
| motor.update_status(); | ||
| for (auto& motor : chassis_steer_motors_) | ||
| motor.update_status(); | ||
|
|
||
| dr16_.update_status(); | ||
| gimbal_yaw_motor_.update_status(); | ||
| tf_->set_state<rmcs_description::GimbalCenterLink, rmcs_description::YawLink>( | ||
| gimbal_yaw_motor_.angle()); | ||
|
|
||
| gimbal_bullet_feeder_.update_status(); | ||
| } | ||
| void command_update() { | ||
| uint16_t batch_commands[4]; | ||
| for (int i = 0; i < 4; i++) | ||
| batch_commands[i] = chassis_wheel_motors_[i].generate_command(); | ||
| transmit_buffer_.add_can1_transmission(0x200, std::bit_cast<uint64_t>(batch_commands)); | ||
| transmit_buffer_.add_can1_transmission(0x1FF, gimbal_bullet_feeder_.generate_command()); | ||
| if (can_transmission_mode) { | ||
| batch_commands[3] = supercap_.generate_command(); | ||
| transmit_buffer_.add_can1_transmission( | ||
| 0x1FE, std::bit_cast<uint64_t>(batch_commands)); | ||
| transmit_buffer_.trigger_transmission(); | ||
| for (int i = 0; i < 4; i++) { | ||
| batch_commands[i] = chassis_steer_motors_[i].generate_command(); | ||
| } | ||
| transmit_buffer_.add_can2_transmission( | ||
| 0x1FE, std::bit_cast<uint64_t>(batch_commands)); | ||
| transmit_buffer_.trigger_transmission(); | ||
| } else { | ||
| transmit_buffer_.add_can2_transmission( | ||
| 0x142, gimbal_yaw_motor_.generate_velocity_command( | ||
| gimbal_yaw_motor_.control_velocity() - imu_.gy())); | ||
| transmit_buffer_.trigger_transmission(); | ||
| } | ||
| can_transmission_mode = !can_transmission_mode; | ||
| } | ||
|
|
||
| void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { | ||
| dr16_.store_status(uart_data, uart_data_length); | ||
| } | ||
| void can1_receive_callback( | ||
| uint32_t can_id, uint64_t can_data, bool is_extended_can_id, | ||
| bool is_remote_transmission, uint8_t can_data_length) override { | ||
| if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| return; | ||
| if (can_id == 0x201) | ||
| chassis_wheel_motors_[0].store_status(can_data); | ||
| else if (can_id == 0x202) | ||
| chassis_wheel_motors_[1].store_status(can_data); | ||
| else if (can_id == 0x203) | ||
| chassis_wheel_motors_[2].store_status(can_data); | ||
| else if (can_id == 0x204) | ||
| chassis_wheel_motors_[3].store_status(can_data); | ||
| else if (can_id == 0x205) | ||
| gimbal_bullet_feeder_.store_status(can_data); | ||
| else if (can_id == 0x300) | ||
| supercap_.store_status(can_data); | ||
| } | ||
| void can2_receive_callback( | ||
| uint32_t can_id, uint64_t can_data, bool is_extended_can_id, | ||
| bool is_remote_transmission, uint8_t can_data_length) override { | ||
| if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| return; | ||
| if (can_id == 0x142) | ||
| gimbal_yaw_motor_.store_status(can_data); | ||
| else if (can_id == 0x205) | ||
| chassis_steer_motors_[0].store_status(can_data); | ||
| else if (can_id == 0x206) | ||
| chassis_steer_motors_[1].store_status(can_data); | ||
| else if (can_id == 0x207) | ||
| chassis_steer_motors_[2].store_status(can_data); | ||
| else if (can_id == 0x208) | ||
| chassis_steer_motors_[3].store_status(can_data); | ||
| } | ||
|
|
||
| void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { | ||
| referee_ring_buffer_receive_.emplace_back_multi( | ||
| [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); | ||
| } | ||
|
|
||
| void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| imu_.store_accelerometer_status(x, y, z); | ||
| } | ||
|
|
||
| void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| imu_.store_gyroscope_status(x, y, z); | ||
| } | ||
|
|
||
| private: | ||
| bool can_transmission_mode = true; | ||
| device::Bmi088 imu_; | ||
| OutputInterface<rmcs_description::Tf>& tf_; | ||
| OutputInterface<bool> powermeter_control_enabled_; | ||
| OutputInterface<double> powermeter_charge_power_limit_; | ||
|
|
||
| device::Dr16 dr16_; | ||
| device::LkMotor gimbal_yaw_motor_; | ||
| device::DjiMotor gimbal_bullet_feeder_; | ||
| device::DjiMotor chassis_wheel_motors_[4]; | ||
| device::DjiMotor chassis_steer_motors_[4]; | ||
| device::Supercap supercap_; | ||
|
|
||
| librmcs::utility::RingBuffer<std::byte> referee_ring_buffer_receive_{256}; | ||
| OutputInterface<rmcs_msgs::SerialInterface> referee_serial_; | ||
| OutputInterface<double> chassis_yaw_velocity_imu_; | ||
|
|
||
| librmcs::client::CBoard::TransmitBuffer transmit_buffer_; | ||
| std::thread event_thread_; | ||
| }; | ||
|
|
||
| OutputInterface<rmcs_description::Tf> tf_; | ||
|
|
||
| std::shared_ptr<SteeringInfantryCommand> command_component_; | ||
| std::shared_ptr<TopBoard> top_board_; | ||
| std::shared_ptr<BottomBoard> bottom_board_; | ||
|
|
||
| rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr gimbal_calibrate_subscription_; | ||
| rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr steers_calibrate_subscription_; | ||
| }; | ||
|
|
||
| } // namespace rmcs_core::hardware | ||
|
|
||
| #include <pluginlib/class_list_macros.hpp> | ||
|
|
||
| PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringInfantry, rmcs_executor::Component) No newline at end of file | ||
| // #include <memory> | ||
|
|
||
| // #include <rclcpp/logger.hpp> | ||
| // #include <rclcpp/logging.hpp> | ||
| // #include <rclcpp/node.hpp> | ||
| // #include <rmcs_description/tf_description.hpp> | ||
| // #include <rmcs_executor/component.hpp> | ||
| // #include <rmcs_msgs/serial_interface.hpp> | ||
| // #include <std_msgs/msg/int32.hpp> | ||
|
|
||
| // #include "hardware/device/bmi088.hpp" | ||
| // #include "hardware/device/dji_motor.hpp" | ||
| // #include "hardware/device/dr16.hpp" | ||
| // #include "hardware/device/lk_motor.hpp" | ||
| // #include "hardware/device/supercap.hpp" | ||
| // #include "librmcs/client/cboard.hpp" | ||
|
|
||
| // namespace rmcs_core::hardware { | ||
|
|
||
| // class SteeringInfantry | ||
| // : public rmcs_executor::Component | ||
| // , public rclcpp::Node { | ||
| // public: | ||
| // SteeringInfantry() | ||
| // : Node( | ||
| // get_component_name(), | ||
| // rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) | ||
| // , command_component_( | ||
| // create_partner_component<SteeringInfantryCommand>( | ||
| // get_component_name() + "_command", *this)) { | ||
| // using namespace rmcs_description; | ||
| // register_output("/tf", tf_); | ||
|
|
||
| // gimbal_calibrate_subscription_ = create_subscription<std_msgs::msg::Int32>( | ||
| // "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { | ||
| // gimbal_calibrate_subscription_callback(std::move(msg)); | ||
| // }); | ||
| // steers_calibrate_subscription_ = create_subscription<std_msgs::msg::Int32>( | ||
| // "/steers/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { | ||
| // steers_calibrate_subscription_callback(std::move(msg)); | ||
| // }); | ||
|
|
||
| // top_board_ = std::make_unique<TopBoard>( | ||
| // *this, *command_component_, | ||
| // static_cast<int>(get_parameter("usb_pid_top_board").as_int())); | ||
|
|
||
| // bottom_board_ = std::make_unique<BottomBoard>( | ||
| // *this, *command_component_, | ||
| // static_cast<int>(get_parameter("usb_pid_bottom_board").as_int())); | ||
|
|
||
| // using namespace rmcs_description; | ||
| // tf_->set_transform<PitchLink, CameraLink>(Eigen::Translation3d{0.06603, 0.0, 0.082}); | ||
| // } | ||
| // ~SteeringInfantry() override = default; | ||
|
|
||
| // void update() override { | ||
| // top_board_->update(); | ||
| // bottom_board_->update(); | ||
| // } | ||
|
|
||
| // void command_update() { | ||
|
|
||
| // top_board_->command_update(); | ||
| // bottom_board_->command_update(); | ||
| // } | ||
|
|
||
| // private: | ||
| // void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[gimbal calibration] New yaw offset: %ld", | ||
| // bottom_board_->gimbal_yaw_motor_.calibrate_zero_point()); | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[gimbal calibration] New pitch offset: %ld", | ||
| // top_board_->gimbal_pitch_motor_.calibrate_zero_point()); | ||
| // } | ||
|
|
||
| // void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[steer calibration] New left front offset: %d", | ||
| // bottom_board_->chassis_steer_motors_[0].calibrate_zero_point()); | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[steer calibration] New left back offset: %d", | ||
| // bottom_board_->chassis_steer_motors_[1].calibrate_zero_point()); | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[steer calibration] New right back offset: %d", | ||
| // bottom_board_->chassis_steer_motors_[2].calibrate_zero_point()); | ||
| // RCLCPP_INFO( | ||
| // get_logger(), "[steer calibration] New right front offset: %d", | ||
| // bottom_board_->chassis_steer_motors_[3].calibrate_zero_point()); | ||
| // } | ||
| // class SteeringInfantryCommand : public rmcs_executor::Component { | ||
| // public: | ||
| // explicit SteeringInfantryCommand(SteeringInfantry& steeringInfantry) | ||
| // : steeringInfantry(steeringInfantry) {} | ||
|
|
||
| // void update() override { steeringInfantry.command_update(); } | ||
|
|
||
| // SteeringInfantry& steeringInfantry; | ||
| // }; | ||
|
|
||
| // class TopBoard final : private librmcs::client::CBoard { | ||
| // public: | ||
| // friend class SteeringInfantry; | ||
| // explicit TopBoard( | ||
| // SteeringInfantry& steeringInfantry, SteeringInfantryCommand& | ||
| // steeringInfantry_command, int usb_pid = -1) : CBoard(usb_pid) , | ||
| // tf_(steeringInfantry.tf_) , bmi088_(1000, 0.2, 0.0) , | ||
| // gimbal_pitch_motor_(steeringInfantry, steeringInfantry_command, "/gimbal/pitch") , | ||
| // gimbal_left_friction_( | ||
| // steeringInfantry, steeringInfantry_command, "/gimbal/left_friction") | ||
| // , gimbal_right_friction_( | ||
| // steeringInfantry, steeringInfantry_command, "/gimbal/right_friction") | ||
| // , transmit_buffer_(*this, 32) | ||
| // , event_thread_([this]() { handle_events(); }) { | ||
| // gimbal_pitch_motor_.configure( | ||
| // device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( | ||
| // static_cast<int>( | ||
| // steeringInfantry.get_parameter("pitch_motor_zero_point").as_int()))); | ||
|
|
||
| // gimbal_left_friction_.configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)); | ||
| // gimbal_right_friction_.configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508} | ||
| // .set_reduction_ratio(1.) | ||
| // .set_reversed()); | ||
|
|
||
| // steeringInfantry.register_output( | ||
| // "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); | ||
| // steeringInfantry.register_output( | ||
| // "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); | ||
|
|
||
| // bmi088_.set_coordinate_mapping([](double x, double y, double z) { | ||
| // // Get the mapping with the following code. | ||
| // // The rotation angle must be an exact multiple of 90 degrees, otherwise | ||
| // // use a matrix. | ||
|
|
||
| // // Eigen::AngleAxisd pitch_link_to_bmi088_link{ | ||
| // // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; | ||
| // // Eigen::Vector3d mapping = pitch_link_to_bmi088_link * Eigen::Vector3d{1, 2, | ||
| // 3}; | ||
| // // std::cout << mapping << std::endl; | ||
|
|
||
| // return std::make_tuple(x, y, z); | ||
| // }); | ||
| // } | ||
| // ~TopBoard() { | ||
| // stop_handling_events(); | ||
| // event_thread_.join(); | ||
| // } | ||
| // void update() { | ||
| // bmi088_.update_status(); | ||
| // Eigen::Quaterniond gimbal_bmi088_pose{ | ||
| // bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; | ||
|
|
||
| // tf_->set_transform<rmcs_description::PitchLink, rmcs_description::OdomImu>( | ||
| // gimbal_bmi088_pose.conjugate()); | ||
|
|
||
| // *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); | ||
| // *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); | ||
|
|
||
| // gimbal_pitch_motor_.update_status(); | ||
| // gimbal_left_friction_.update_status(); | ||
| // gimbal_right_friction_.update_status(); | ||
|
|
||
| // tf_->set_state<rmcs_description::YawLink, rmcs_description::PitchLink>( | ||
| // gimbal_pitch_motor_.angle()); | ||
| // } | ||
|
|
||
| // void command_update() { | ||
| // uint16_t can_commands[4]; | ||
| // can_commands[2] = gimbal_left_friction_.generate_command(); | ||
| // can_commands[3] = gimbal_right_friction_.generate_command(); | ||
| // transmit_buffer_.add_can1_transmission(0x200, std::bit_cast<uint64_t>(can_commands)); | ||
|
|
||
| // transmit_buffer_.add_can2_transmission( | ||
| // 0x142, gimbal_pitch_motor_.generate_velocity_command( | ||
| // gimbal_pitch_motor_.control_velocity())); | ||
|
|
||
| // transmit_buffer_.trigger_transmission(); | ||
| // } | ||
|
|
||
| // private: | ||
| // void can1_receive_callback( | ||
| // uint32_t can_id, uint64_t can_data, bool is_extended_can_id, | ||
| // bool is_remote_transmission, uint8_t can_data_length) override { | ||
| // if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| // return; | ||
| // if (can_id == 0x203) { | ||
| // gimbal_left_friction_.store_status(can_data); | ||
| // } else if (can_id == 0x204) { | ||
| // gimbal_right_friction_.store_status(can_data); | ||
| // } | ||
| // } | ||
| // void can2_receive_callback( | ||
| // uint32_t can_id, uint64_t can_data, bool is_extended_can_id, | ||
| // bool is_remote_transmission, uint8_t can_data_length) override { | ||
| // if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| // return; | ||
| // if (can_id == 0x142) | ||
| // gimbal_pitch_motor_.store_status(can_data); | ||
| // } | ||
| // void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| // bmi088_.store_accelerometer_status(x, y, z); | ||
| // } | ||
| // void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| // bmi088_.store_gyroscope_status(x, y, z); | ||
| // } | ||
| // OutputInterface<rmcs_description::Tf>& tf_; | ||
|
|
||
| // OutputInterface<double> gimbal_yaw_velocity_bmi088_; | ||
| // OutputInterface<double> gimbal_pitch_velocity_bmi088_; | ||
|
|
||
| // device::Bmi088 bmi088_; | ||
| // device::LkMotor gimbal_pitch_motor_; | ||
| // device::DjiMotor gimbal_left_friction_; | ||
| // device::DjiMotor gimbal_right_friction_; | ||
|
|
||
| // TransmitBuffer transmit_buffer_; | ||
| // std::thread event_thread_; | ||
| // }; | ||
| // class BottomBoard final : private librmcs::client::CBoard { | ||
| // public: | ||
| // friend class SteeringInfantry; | ||
|
|
||
| // explicit BottomBoard( | ||
| // SteeringInfantry& steeringInfantry, SteeringInfantryCommand& | ||
| // steeringInfantry_command, int usb_pid = -1) : librmcs::client::CBoard(usb_pid) , | ||
| // imu_(1000, 0.2, 0.0) , tf_(steeringInfantry.tf_) , dr16_(steeringInfantry) , | ||
| // gimbal_yaw_motor_(steeringInfantry, steeringInfantry_command, "/gimbal/yaw") , | ||
| // gimbal_bullet_feeder_( | ||
| // steeringInfantry, steeringInfantry_command, "/gimbal/bullet_feeder") | ||
| // , chassis_wheel_motors_( | ||
| // {steeringInfantry, steeringInfantry_command, "/chassis/left_front_wheel"}, | ||
| // {steeringInfantry, steeringInfantry_command, "/chassis/left_back_wheel"}, | ||
| // {steeringInfantry, steeringInfantry_command, "/chassis/right_back_wheel"}, | ||
| // {steeringInfantry, steeringInfantry_command, "/chassis/right_front_wheel"}) | ||
| // , chassis_steer_motors_( | ||
| // {steeringInfantry, steeringInfantry_command, "/chassis/left_front_steering"}, | ||
| // {steeringInfantry, steeringInfantry_command, "/chassis/left_back_steering"}, | ||
| // {steeringInfantry, steeringInfantry_command, "/chassis/right_back_steering"}, | ||
| // {steeringInfantry, steeringInfantry_command, "/chassis/right_front_steering"}) | ||
| // , supercap_(steeringInfantry, steeringInfantry_command) | ||
| // , transmit_buffer_(*this, 32) | ||
| // , event_thread_([this]() { handle_events(); }) { | ||
|
|
||
| // steeringInfantry.register_output("/referee/serial", referee_serial_); | ||
|
|
||
| // referee_serial_->read = [this](std::byte* buffer, size_t size) { | ||
| // return referee_ring_buffer_receive_.pop_front_multi( | ||
| // [&buffer](std::byte byte) { *buffer++ = byte; }, size); | ||
| // }; | ||
| // referee_serial_->write = [this](const std::byte* buffer, size_t size) { | ||
| // transmit_buffer_.add_uart1_transmission(buffer, size); | ||
| // return size; | ||
| // }; | ||
| // gimbal_yaw_motor_.configure( | ||
| // device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( | ||
| // static_cast<int>( | ||
| // steeringInfantry.get_parameter("yaw_motor_zero_point").as_int()))); | ||
|
|
||
| // gimbal_bullet_feeder_.configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M2006} | ||
| // .enable_multi_turn_angle() | ||
| // .set_reversed() | ||
| // .set_reduction_ratio(19 * 2)); | ||
|
|
||
| // for (auto& motor : chassis_wheel_motors_) | ||
| // motor.configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508} | ||
|
|
||
| // .set_reduction_ratio(11.) | ||
| // .enable_multi_turn_angle() | ||
| // .set_reversed()); | ||
| // chassis_steer_motors_[0].configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| // .set_reversed() | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>( | ||
| // steeringInfantry.get_parameter("left_front_zero_point").as_int())) | ||
| // .enable_multi_turn_angle()); | ||
| // chassis_steer_motors_[1].configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| // .set_reversed() | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>( | ||
| // steeringInfantry.get_parameter("left_back_zero_point").as_int())) | ||
| // .enable_multi_turn_angle()); | ||
| // chassis_steer_motors_[2].configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| // .set_reversed() | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>( | ||
| // steeringInfantry.get_parameter("right_back_zero_point").as_int())) | ||
| // .enable_multi_turn_angle()); | ||
| // chassis_steer_motors_[3].configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| // .set_reversed() | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>( | ||
| // steeringInfantry.get_parameter("right_front_zero_point").as_int())) | ||
| // .enable_multi_turn_angle()); | ||
| // steeringInfantry.register_output( | ||
| // "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); | ||
| // } | ||
| // ~BottomBoard() final { | ||
| // stop_handling_events(); | ||
| // event_thread_.join(); | ||
| // } | ||
| // void update() { | ||
| // imu_.update_status(); | ||
| // *chassis_yaw_velocity_imu_ = imu_.gy(); | ||
| // supercap_.update_status(); | ||
|
|
||
| // for (auto& motor : chassis_wheel_motors_) | ||
| // motor.update_status(); | ||
| // for (auto& motor : chassis_steer_motors_) | ||
| // motor.update_status(); | ||
|
|
||
| // dr16_.update_status(); | ||
| // gimbal_yaw_motor_.update_status(); | ||
| // tf_->set_state<rmcs_description::GimbalCenterLink, rmcs_description::YawLink>( | ||
| // gimbal_yaw_motor_.angle()); | ||
|
|
||
| // gimbal_bullet_feeder_.update_status(); | ||
| // } | ||
| // void command_update() { | ||
| // uint16_t batch_commands[4]; | ||
| // for (int i = 0; i < 4; i++) | ||
| // batch_commands[i] = chassis_wheel_motors_[i].generate_command(); | ||
| // transmit_buffer_.add_can1_transmission(0x200, | ||
| // std::bit_cast<uint64_t>(batch_commands)); | ||
| // transmit_buffer_.add_can1_transmission(0x1FF, | ||
| // gimbal_bullet_feeder_.generate_command()); if (can_transmission_mode) { | ||
| // batch_commands[3] = supercap_.generate_command(); | ||
| // transmit_buffer_.add_can1_transmission( | ||
| // 0x1FE, std::bit_cast<uint64_t>(batch_commands)); | ||
| // transmit_buffer_.trigger_transmission(); | ||
| // for (int i = 0; i < 4; i++) { | ||
| // batch_commands[i] = chassis_steer_motors_[i].generate_command(); | ||
| // } | ||
| // transmit_buffer_.add_can2_transmission( | ||
| // 0x1FE, std::bit_cast<uint64_t>(batch_commands)); | ||
| // transmit_buffer_.trigger_transmission(); | ||
| // } else { | ||
| // transmit_buffer_.add_can2_transmission( | ||
| // 0x142, gimbal_yaw_motor_.generate_velocity_command( | ||
| // gimbal_yaw_motor_.control_velocity() - imu_.gy())); | ||
| // transmit_buffer_.trigger_transmission(); | ||
| // } | ||
| // can_transmission_mode = !can_transmission_mode; | ||
| // } | ||
|
|
||
| // void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override | ||
| // { | ||
| // dr16_.store_status(uart_data, uart_data_length); | ||
| // } | ||
| // void can1_receive_callback( | ||
| // uint32_t can_id, uint64_t can_data, bool is_extended_can_id, | ||
| // bool is_remote_transmission, uint8_t can_data_length) override { | ||
| // if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| // return; | ||
| // if (can_id == 0x201) | ||
| // chassis_wheel_motors_[0].store_status(can_data); | ||
| // else if (can_id == 0x202) | ||
| // chassis_wheel_motors_[1].store_status(can_data); | ||
| // else if (can_id == 0x203) | ||
| // chassis_wheel_motors_[2].store_status(can_data); | ||
| // else if (can_id == 0x204) | ||
| // chassis_wheel_motors_[3].store_status(can_data); | ||
| // else if (can_id == 0x205) | ||
| // gimbal_bullet_feeder_.store_status(can_data); | ||
| // else if (can_id == 0x300) | ||
| // supercap_.store_status(can_data); | ||
| // } | ||
| // void can2_receive_callback( | ||
| // uint32_t can_id, uint64_t can_data, bool is_extended_can_id, | ||
| // bool is_remote_transmission, uint8_t can_data_length) override { | ||
| // if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| // return; | ||
| // if (can_id == 0x142) | ||
| // gimbal_yaw_motor_.store_status(can_data); | ||
| // else if (can_id == 0x205) | ||
| // chassis_steer_motors_[0].store_status(can_data); | ||
| // else if (can_id == 0x206) | ||
| // chassis_steer_motors_[1].store_status(can_data); | ||
| // else if (can_id == 0x207) | ||
| // chassis_steer_motors_[2].store_status(can_data); | ||
| // else if (can_id == 0x208) | ||
| // chassis_steer_motors_[3].store_status(can_data); | ||
| // } | ||
|
|
||
| // void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) | ||
| // override { | ||
| // referee_ring_buffer_receive_.emplace_back_multi( | ||
| // [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); | ||
| // } | ||
|
|
||
| // void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| // imu_.store_accelerometer_status(x, y, z); | ||
| // } | ||
|
|
||
| // void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| // imu_.store_gyroscope_status(x, y, z); | ||
| // } | ||
|
|
||
| // private: | ||
| // bool can_transmission_mode = true; | ||
| // device::Bmi088 imu_; | ||
| // OutputInterface<rmcs_description::Tf>& tf_; | ||
| // OutputInterface<bool> powermeter_control_enabled_; | ||
| // OutputInterface<double> powermeter_charge_power_limit_; | ||
|
|
||
| // device::Dr16 dr16_; | ||
| // device::LkMotor gimbal_yaw_motor_; | ||
| // device::DjiMotor gimbal_bullet_feeder_; | ||
| // device::DjiMotor chassis_wheel_motors_[4]; | ||
| // device::DjiMotor chassis_steer_motors_[4]; | ||
| // device::Supercap supercap_; | ||
|
|
||
| // librmcs::utility::RingBuffer<std::byte> referee_ring_buffer_receive_{256}; | ||
| // OutputInterface<rmcs_msgs::SerialInterface> referee_serial_; | ||
| // OutputInterface<double> chassis_yaw_velocity_imu_; | ||
|
|
||
| // librmcs::client::CBoard::TransmitBuffer transmit_buffer_; | ||
| // std::thread event_thread_; | ||
| // }; | ||
|
|
||
| // OutputInterface<rmcs_description::Tf> tf_; | ||
|
|
||
| // std::shared_ptr<SteeringInfantryCommand> command_component_; | ||
| // std::shared_ptr<TopBoard> top_board_; | ||
| // std::shared_ptr<BottomBoard> bottom_board_; | ||
|
|
||
| // rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr gimbal_calibrate_subscription_; | ||
| // rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr steers_calibrate_subscription_; | ||
| // }; | ||
|
|
||
| // } // namespace rmcs_core::hardware | ||
|
|
||
| // #include <pluginlib/class_list_macros.hpp> | ||
|
|
||
| // PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringInfantry, rmcs_executor::Component) No newline at end of file |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# 检查 plugins.xml 中是否仍然注册了 SteeringInfantry
echo "=== plugins.xml 中的 SteeringInfantry 引用 ==="
rg -n "SteeringInfantry" --glob "*.xml"
echo ""
echo "=== YAML 配置中的 SteeringInfantry 引用 ==="
rg -n "SteeringInfantry" --glob "*.yaml"
echo ""
echo "=== 检查 PLUGINLIB_EXPORT_CLASS 是否被注释 ==="
rg -n "PLUGINLIB_EXPORT_CLASS.*SteeringInfantry" --glob "*.cpp"Repository: Alliance-Algorithm/RMCS
Length of output: 608
插件注册与实现不一致，将导致运行时崩溃。
整个文件被注释掉，包括 PLUGINLIB_EXPORT_CLASS 宏（第 443 行）。但 plugins.xml 仍然在第 14 行声明了 rmcs_core::hardware::SteeringInfantry 作为已注册的插件，steering-infantry.yaml 也在第 5 行引用此组件。
这会导致插件加载器在运行时无法找到该类，抛出 "class not registered" 错误，任何依赖 steering-infantry.yaml 的启动配置都会失败。
必须执行以下之一：
- 若移除此组件，删除
plugins.xml第 14 行的声明和steering-infantry.yaml第 5 行的引用 - 若保留此组件，取消注释整个文件和第 443 行的宏
🤖 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/steering-infantry.cpp` around lines 1 -
443, The SteeringInfantry plugin class is commented out but still referenced by
the plugin loader (PLUGINLIB_EXPORT_CLASS and the SteeringInfantry class are
disabled) which causes "class not registered" at runtime; either remove the
plugin registration and yaml reference or restore the implementation: 1) If you
mean to delete the component, remove the rmcs_core::hardware::SteeringInfantry
entry from plugins.xml and the reference in steering-infantry.yaml; or 2) If you
intend to keep it, uncomment the entire steering-infantry.cpp implementation
including the PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringInfantry,
rmcs_executor::Component) macro so the class and export match the
plugins.xml/steering-infantry.yaml declarations.
Comment on lines
+19
to
+308
| // class TunnelInfantry | ||
| // : public rmcs_executor::Component | ||
| // , public rclcpp::Node | ||
| // , private librmcs::client::CBoard { | ||
| // public: | ||
| // TunnelInfantry() | ||
| // : Node{get_component_name(), | ||
| // rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , | ||
| // librmcs::client::CBoard{static_cast<int>(get_parameter("usb_pid").as_int())} , | ||
| // logger_(get_logger()) , infantry_command_( | ||
| // create_partner_component<InfantryCommand>(get_component_name() + "_command", | ||
| // *this)) | ||
| // , chassis_wheel_motors_( | ||
| // {*this, *infantry_command_, "/chassis/left_front_wheel"}, | ||
| // {*this, *infantry_command_, "/chassis/right_front_wheel"}, | ||
| // {*this, *infantry_command_, "/chassis/right_back_wheel"}, | ||
| // {*this, *infantry_command_, "/chassis/left_back_wheel"}) | ||
| // , supercap_(*this, *infantry_command_) | ||
| // , gimbal_yaw_motor_(*this, *infantry_command_, "/gimbal/yaw") | ||
| // , gimbal_pitch_motor_(*this, *infantry_command_, "/gimbal/pitch") | ||
| // , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") | ||
| // , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") | ||
| // , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") | ||
| // , dr16_{*this} | ||
| // , bmi088_(1000, 0.2, 0.0) | ||
| // , transmit_buffer_(*this, 32) | ||
| // , event_thread_([this]() { handle_events(); }) { | ||
|
|
||
| // for (auto& motor : chassis_wheel_motors_) | ||
| // motor.configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508} | ||
| // .set_reversed() | ||
| // .set_reduction_ratio(13.) | ||
| // .enable_multi_turn_angle()); | ||
|
|
||
| // gimbal_yaw_motor_.configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::GM6020} | ||
| // .set_reversed() | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>(get_parameter("yaw_motor_zero_point").as_int()))); | ||
| // gimbal_pitch_motor_.configure( | ||
| // device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10} | ||
| // .set_encoder_zero_point( | ||
| // static_cast<int>(get_parameter("pitch_motor_zero_point").as_int())) | ||
| // .set_reversed()); | ||
|
|
||
| // gimbal_left_friction_.configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)); | ||
| // gimbal_right_friction_.configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M3508} | ||
| // .set_reversed() | ||
| // .set_reduction_ratio(1.)); | ||
| // gimbal_bullet_feeder_.configure( | ||
| // device::DjiMotor::Config{device::DjiMotor::Type::M2006}.enable_multi_turn_angle()); | ||
|
|
||
| // register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); | ||
| // register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); | ||
| // register_output("/tf", tf_); | ||
|
|
||
| // bmi088_.set_coordinate_mapping([](double x, double y, double z) { | ||
| // // Get the mapping with the following code. | ||
| // // The rotation angle must be an exact multiple of 90 degrees, otherwise use a | ||
| // matrix. | ||
|
|
||
| // // Eigen::AngleAxisd pitch_link_to_imu_link{ | ||
| // // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; | ||
| // // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; | ||
| // // std::cout << mapping << std::endl; | ||
|
|
||
| // return std::make_tuple(-y, x, z); | ||
| // }); | ||
|
|
||
| // using namespace rmcs_description; | ||
| // tf_->set_transform<PitchLink, CameraLink>(Eigen::Translation3d{0.06603, 0.0, 0.082}); | ||
|
|
||
| // constexpr double gimbal_center_height = 0.32059; | ||
| // constexpr double wheel_distance_x = 0.15897, wheel_distance_y = 0.15897; | ||
| // tf_->set_transform<BaseLink, GimbalCenterLink>( | ||
| // Eigen::Translation3d{0, 0, gimbal_center_height}); | ||
| // tf_->set_transform<BaseLink, LeftFrontWheelLink>( | ||
| // Eigen::Translation3d{wheel_distance_x / 2, wheel_distance_y / 2, 0}); | ||
| // tf_->set_transform<BaseLink, LeftBackWheelLink>( | ||
| // Eigen::Translation3d{-wheel_distance_x / 2, wheel_distance_y / 2, 0}); | ||
| // tf_->set_transform<BaseLink, RightBackWheelLink>( | ||
| // Eigen::Translation3d{-wheel_distance_x / 2, -wheel_distance_y / 2, 0}); | ||
| // tf_->set_transform<BaseLink, RightFrontWheelLink>( | ||
| // Eigen::Translation3d{wheel_distance_x / 2, -wheel_distance_y / 2, 0}); | ||
|
|
||
| // gimbal_calibrate_subscription_ = create_subscription<std_msgs::msg::Int32>( | ||
| // "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { | ||
| // gimbal_calibrate_subscription_callback(std::move(msg)); | ||
| // }); | ||
|
|
||
| // register_output("/referee/serial", referee_serial_); | ||
| // referee_serial_->read = [this](std::byte* buffer, size_t size) { | ||
| // return referee_ring_buffer_receive_.pop_front_multi( | ||
| // [&buffer](std::byte byte) { *buffer++ = byte; }, size); | ||
| // }; | ||
| // referee_serial_->write = [this](const std::byte* buffer, size_t size) { | ||
| // transmit_buffer_.add_uart1_transmission(buffer, size); | ||
| // return size; | ||
| // }; | ||
| // } | ||
|
|
||
| // ~TunnelInfantry() override { | ||
| // stop_handling_events(); | ||
| // event_thread_.join(); | ||
| // } | ||
|
|
||
| // void update() override { | ||
| // update_motors(); | ||
| // update_imu(); | ||
| // dr16_.update_status(); | ||
| // supercap_.update_status(); | ||
| // } | ||
|
|
||
| // void command_update() { | ||
| // uint16_t can_commands[4]; | ||
|
|
||
| // can_commands[0] = gimbal_yaw_motor_.generate_command(); | ||
| // can_commands[1] = gimbal_pitch_motor_.generate_command(); | ||
| // can_commands[2] = 0; | ||
| // can_commands[3] = supercap_.generate_command(); | ||
| // transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast<uint64_t>(can_commands)); | ||
|
|
||
| // can_commands[0] = chassis_wheel_motors_[0].generate_command(); | ||
| // can_commands[1] = chassis_wheel_motors_[1].generate_command(); | ||
| // can_commands[2] = chassis_wheel_motors_[2].generate_command(); | ||
| // can_commands[3] = chassis_wheel_motors_[3].generate_command(); | ||
| // transmit_buffer_.add_can1_transmission(0x200, std::bit_cast<uint64_t>(can_commands)); | ||
|
|
||
| // transmit_buffer_.add_can2_transmission(0x142, gimbal_pitch_motor_.generate_command()); | ||
|
|
||
| // can_commands[0] = 0; | ||
| // can_commands[1] = gimbal_bullet_feeder_.generate_command(); | ||
| // can_commands[2] = gimbal_left_friction_.generate_command(); | ||
| // can_commands[3] = gimbal_right_friction_.generate_command(); | ||
| // transmit_buffer_.add_can2_transmission(0x200, std::bit_cast<uint64_t>(can_commands)); | ||
|
|
||
| // transmit_buffer_.trigger_transmission(); | ||
| // } | ||
|
|
||
| // private: | ||
| // void update_motors() { | ||
| // using namespace rmcs_description; | ||
| // for (auto& motor : chassis_wheel_motors_) | ||
| // motor.update_status(); | ||
| // tf_->set_state<BaseLink, LeftFrontWheelLink>(chassis_wheel_motors_[0].angle()); | ||
| // tf_->set_state<BaseLink, RightFrontWheelLink>(chassis_wheel_motors_[1].angle()); | ||
| // tf_->set_state<BaseLink, RightBackWheelLink>(chassis_wheel_motors_[2].angle()); | ||
| // tf_->set_state<BaseLink, LeftBackWheelLink>(chassis_wheel_motors_[3].angle()); | ||
|
|
||
| // gimbal_yaw_motor_.update_status(); | ||
| // tf_->set_state<GimbalCenterLink, YawLink>(gimbal_yaw_motor_.angle()); | ||
| // gimbal_pitch_motor_.update_status(); | ||
| // tf_->set_state<YawLink, PitchLink>(gimbal_pitch_motor_.angle()); | ||
|
|
||
| // gimbal_bullet_feeder_.update_status(); | ||
| // gimbal_left_friction_.update_status(); | ||
| // gimbal_right_friction_.update_status(); | ||
| // } | ||
|
|
||
| // void update_imu() { | ||
| // bmi088_.update_status(); | ||
| // Eigen::Quaterniond gimbal_imu_pose{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), | ||
| // bmi088_.q3()}; tf_->set_transform<rmcs_description::PitchLink, | ||
| // rmcs_description::OdomImu>( | ||
| // gimbal_imu_pose.conjugate()); | ||
|
|
||
| // *gimbal_yaw_velocity_imu_ = bmi088_.gz(); | ||
| // *gimbal_pitch_velocity_imu_ = bmi088_.gy(); | ||
| // } | ||
|
|
||
| // void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { | ||
| // RCLCPP_INFO( | ||
| // logger_, "[gimbal calibration] New yaw offset: %d", | ||
| // gimbal_yaw_motor_.calibrate_zero_point()); | ||
| // RCLCPP_INFO( | ||
| // logger_, "[gimbal calibration] New pitch offset: %ld", | ||
| // gimbal_pitch_motor_.calibrate_zero_point()); | ||
| // } | ||
|
|
||
| // protected: | ||
| // void can1_receive_callback( | ||
| // uint32_t can_id, uint64_t can_data, bool is_extended_can_id, bool is_remote_transmission, | ||
| // uint8_t can_data_length) override { | ||
| // if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| // return; | ||
|
|
||
| // if (can_id == 0x201) { | ||
| // auto& motor = chassis_wheel_motors_[0]; | ||
| // motor.store_status(can_data); | ||
| // } else if (can_id == 0x202) { | ||
| // auto& motor = chassis_wheel_motors_[1]; | ||
| // motor.store_status(can_data); | ||
| // } else if (can_id == 0x203) { | ||
| // auto& motor = chassis_wheel_motors_[2]; | ||
| // motor.store_status(can_data); | ||
| // } else if (can_id == 0x204) { | ||
| // auto& motor = chassis_wheel_motors_[3]; | ||
| // motor.store_status(can_data); | ||
| // } else if (can_id == 0x205) { | ||
| // gimbal_yaw_motor_.store_status(can_data); | ||
| // } else if (can_id == 0x206) { | ||
| // gimbal_pitch_motor_.store_status(can_data); | ||
| // } else if (can_id == 0x300) { | ||
| // supercap_.store_status(can_data); | ||
| // } | ||
| // } | ||
|
|
||
| // void can2_receive_callback( | ||
| // uint32_t can_id, uint64_t can_data, bool is_extended_can_id, bool is_remote_transmission, | ||
| // uint8_t can_data_length) override { | ||
| // if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] | ||
| // return; | ||
|
|
||
| // if (can_id == 0x142) { | ||
| // gimbal_pitch_motor_.store_status(can_data); | ||
| // } else if (can_id == 0x202) { | ||
| // gimbal_bullet_feeder_.store_status(can_data); | ||
| // } else if (can_id == 0x203) { | ||
| // gimbal_left_friction_.store_status(can_data); | ||
| // } else if (can_id == 0x204) { | ||
| // gimbal_right_friction_.store_status(can_data); | ||
| // } | ||
| // } | ||
|
|
||
| // void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { | ||
| // referee_ring_buffer_receive_.emplace_back_multi( | ||
| // [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); | ||
| // } | ||
|
|
||
| // void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { | ||
| // dr16_.store_status(uart_data, uart_data_length); | ||
| // } | ||
|
|
||
| // void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| // bmi088_.store_accelerometer_status(x, y, z); | ||
| // } | ||
|
|
||
| // void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { | ||
| // bmi088_.store_gyroscope_status(x, y, z); | ||
| // } | ||
|
|
||
| // private: | ||
| // rclcpp::Logger logger_; | ||
|
|
||
| // class InfantryCommand : public rmcs_executor::Component { | ||
| // public: | ||
| // explicit InfantryCommand(TunnelInfantry& infantry) | ||
| // : infantry_(infantry) {} | ||
|
|
||
| // void update() override { infantry_.command_update(); } | ||
|
|
||
| // TunnelInfantry& infantry_; | ||
| // }; | ||
| // std::shared_ptr<InfantryCommand> infantry_command_; | ||
|
|
||
| // rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr gimbal_calibrate_subscription_; | ||
|
|
||
| // device::DjiMotor chassis_wheel_motors_[4]; | ||
| // device::Supercap supercap_; | ||
|
|
||
| // device::DjiMotor gimbal_yaw_motor_; | ||
| // device::LkMotor gimbal_pitch_motor_; | ||
|
|
||
| // device::DjiMotor gimbal_left_friction_; | ||
| // device::DjiMotor gimbal_right_friction_; | ||
| // device::DjiMotor gimbal_bullet_feeder_; | ||
|
|
||
| // device::Dr16 dr16_; | ||
| // device::Bmi088 bmi088_; | ||
|
|
||
| // OutputInterface<double> gimbal_yaw_velocity_imu_; | ||
| // OutputInterface<double> gimbal_pitch_velocity_imu_; | ||
|
|
||
| // OutputInterface<rmcs_description::Tf> tf_; | ||
|
|
||
| // librmcs::utility::RingBuffer<std::byte> referee_ring_buffer_receive_{256}; | ||
| // OutputInterface<rmcs_msgs::SerialInterface> referee_serial_; | ||
|
|
||
| // librmcs::client::CBoard::TransmitBuffer transmit_buffer_; | ||
| // std::thread event_thread_; | ||
| // }; | ||
|
|
||
| // } // namespace rmcs_core::hardware | ||
|
|
||
| // #include <pluginlib/class_list_macros.hpp> | ||
|
|
||
| // PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::TunnelInfantry, rmcs_executor::Component) No newline at end of file |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
set -euo pipefail
echo "== Active TunnelInfantry class definitions =="
rg -nP -g '!**/build/**' -g '!**/install/**' -g '!**/log/**' \
'^(?!\s*//)\s*class\s+TunnelInfantry\b'
echo
echo "== Active TunnelInfantry plugin exports =="
rg -nP -g '!**/build/**' -g '!**/install/**' -g '!**/log/**' \
'^(?!\s*//).*PLUGINLIB_EXPORT_CLASS\s*\(\s*rmcs_core::hardware::TunnelInfantry\b'
echo
echo "== Repository references to this plugin =="
rg -n -C2 -g '!**/build/**' -g '!**/install/**' -g '!**/log/**' \
'rmcs_core::hardware::TunnelInfantry|TunnelInfantry|tunnel_infantry'Repository: Alliance-Algorithm/RMCS
Length of output: 112
恢复 TunnelInfantry 的有效实现与插件导出
整个 TunnelInfantry 类实现和 PLUGINLIB_EXPORT_CLASS 都已注释，导致该编译单元不再导出 rmcs_core::hardware::TunnelInfantry 组件。仓库中不存在该类的其他活跃定义或插件导出。若有 launch 文件或插件描述仍引用该组件，运行时将失败。应该要么取消注释恢复实现，要么移除所有对该插件的引用。
🤖 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/tunnel_infantry.cpp` around lines 19 -
308, TunnelInfantry 类的全部实现和插件导出 PLUGINLIB_EXPORT_CLASS
被注释掉，导致组件不再被导出并在运行时找不到；请恢复（取消注释） TunnelInfantry
类定义及其成员实现（构造函数、析构、update、command_update、各回调与私有成员）并恢复最后一行的
PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::TunnelInfantry,
rmcs_executor::Component) 导出，或者如果决定移除该组件，删除/更新所有对 TunnelInfantry
或该插件导出的引用（launch/插件描述文件等）以保持一致。
Comment on lines
+1
to
+7
| #pragma once | ||
|
|
||
| #include <cstddef> | ||
| #include <span> | ||
| #include <type_traits> | ||
|
|
||
| namespace rmcs_utility {} // namespace rmcs_utility No newline at end of file |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# 描述：检查是否有代码引用了 as_byte_span.hpp 或使用了相关功能
# 搜索包含此头文件的代码
echo "=== 搜索包含 as_byte_span.hpp 的文件 ==="
rg -n '#include.*as_byte_span\.hpp'
# 搜索可能使用 as_byte_span 函数的代码
echo -e "\n=== 搜索使用 as_byte_span 的代码 ==="
rg -n 'as_byte_span\s*\('
# 搜索 rmcs_utility 命名空间中的 as_byte_span 引用
echo -e "\n=== 搜索 rmcs_utility::as_byte_span 引用 ==="
rg -n 'rmcs_utility::as_byte_span'Repository: Alliance-Algorithm/RMCS
Length of output: 173
此头文件包含未实现的占位符代码，但无代码依赖它。
文件 as_byte_span.hpp 的名称暗示应包含将对象转换为字节跨度的工具函数，相关标准库头文件也已包含（<cstddef>、<span>、<type_traits>），但实际上只声明了空命名空间，不存在任何实现。
验证显示代码库中无任何地方引用或使用此头文件，因此这不会导致编译错误。请确认此文件的用途：
- 若为计划中的功能，请添加实现代码
- 若为遗留代码，请删除此文件
🧰 Tools
🪛 Clang (14.0.6)
[error] 3-3: '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_utility/include/rmcs_utility/as_byte_span.hpp` around lines
1 - 7, The file as_byte_span.hpp currently only contains an empty namespace
rmcs_utility while its name and included headers suggest a utility to convert
objects to std::byte spans; either implement the intended utility or remove the
file. If you intend to implement it, add a function template (e.g.,
to_byte_span/T to_mutable_byte_span) in namespace rmcs_utility that uses
std::span and std::as_bytes/std::as_writable_bytes or reinterpret_cast with
static_asserts on std::is_trivially_copyable to safely produce
std::span<std::byte> from an object or container; otherwise delete
as_byte_span.hpp and remove any planned includes. Ensure the chosen symbols
(namespace rmcs_utility and the new function names like
to_byte_span/to_mutable_byte_span) match existing style and add unit tests if
implementing.
Comment on lines
+126
to
+149
| EndianContainer& operator%=(const T& value) noexcept { | ||
| *this = T(*this) % value; | ||
| return *this; | ||
| } | ||
| EndianContainer& operator&=(const T& value) noexcept { | ||
| *this = T(*this) & value; | ||
| return *this; | ||
| } | ||
| EndianContainer& operator|=(const T& value) noexcept { | ||
| *this = T(*this) | value; | ||
| return *this; | ||
| } | ||
| EndianContainer& operator^=(const T& value) noexcept { | ||
| *this = T(*this) ^ value; | ||
| return *this; | ||
| } | ||
| EndianContainer& operator<<=(const T& value) noexcept { | ||
| *this = T(T(*this) << value); | ||
| return *this; | ||
| } | ||
| EndianContainer& operator>>=(const T& value) noexcept { | ||
| *this = T(T(*this) >> value); | ||
| return *this; | ||
| } |
There was a problem hiding this comment.
浮点类型不支持模运算和位运算操作符。
operator%=、operator&=、operator|=、operator^=、operator<<=、operator>>= 对浮点类型（float/double）无效。当使用 le_float32_t、be_float64_t 等类型别名并调用这些操作符时，会导致编译错误。
建议使用 requires 约束或 if constexpr 将这些操作符限制为仅整数类型。
🐛 建议修复：添加整数类型约束
- EndianContainer& operator%=(const T& value) noexcept {
+ EndianContainer& operator%=(const T& value) noexcept requires std::is_integral_v<T> {
*this = T(*this) % value;
return *this;
}
- EndianContainer& operator&=(const T& value) noexcept {
+ EndianContainer& operator&=(const T& value) noexcept requires std::is_integral_v<T> {
*this = T(*this) & value;
return *this;
}
- EndianContainer& operator|=(const T& value) noexcept {
+ EndianContainer& operator|=(const T& value) noexcept requires std::is_integral_v<T> {
*this = T(*this) | value;
return *this;
}
- EndianContainer& operator^=(const T& value) noexcept {
+ EndianContainer& operator^=(const T& value) noexcept requires std::is_integral_v<T> {
*this = T(*this) ^ value;
return *this;
}
- EndianContainer& operator<<=(const T& value) noexcept {
+ EndianContainer& operator<<=(const T& value) noexcept requires std::is_integral_v<T> {
*this = T(T(*this) << value);
return *this;
}
- EndianContainer& operator>>=(const T& value) noexcept {
+ EndianContainer& operator>>=(const T& value) noexcept requires std::is_integral_v<T> {
*this = T(T(*this) >> value);
return *this;
}🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp` around
lines 126 - 149, The assignment operators operator%=, operator&=, operator|=,
operator^=, operator<<= and operator>>= on EndianContainer are invalid for
floating-point T (e.g., le_float32_t, be_float64_t) and cause compile errors;
restrict these operators to integer types by adding a constraint (e.g., use
std::is_integral_v<T> or the std::integral concept in a requires clause) or
guard their bodies with if constexpr(std::is_integral_v<T>) so they are only
instantiated for integral T; update the templates for the methods named
operator%=, operator&=, operator|=, operator^=, operator<<= and operator>>= in
the EndianContainer definition to enforce that constraint.
There was a problem hiding this comment.
Actionable comments posted: 15
♻️ Duplicate comments (1)
rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp (1)
77-105:⚠️ Potential issue | 🔴 Critical切模式时旧的速度指令仍会锁存。
角度模式只写
yaw/pitch，拉力模式只写force，未命中的输出在进入本帧时没有先回到安全值。所以从一种模式切到另一种模式后，另外两路会继续沿用上一帧命令，机构会在错误模式下继续运动。建议在update()开头先把三路输出统一写成安全值，再由命中分支覆写。🤖 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/dart/dart_settings.cpp` around lines 77 - 105, 在 update() 函数帧开头先把三路输出统一写成安全值（例如把 *yaw_control_velocity_, *pitch_control_velocity_, *force_control_ 设为 0 或 reset_all_controls() 的等效安全值），然后再进入原来的分支（涉及 switch_left/switch_right、is_auto_pitch_control_mode_、is_auto_force_control_mode_、pitch_control()、force_control() 等）由命中分支覆盖对应通道的值，确保未命中的通道不会沿用上一帧残留命令。
🧹 Nitpick comments (16)
rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp (3)
260-263:rotary_knob初始值可能与协议不一致。其他摇杆通道初始化为 1024（中立位置），但
rotary_knob初始化为 0。根据channel_to_double的逻辑，0 会被转换为超出范围的值并返回 0.0，这在功能上是正确的。但如果 DR16 协议中旋钮的中立值也是 1024，建议统一初始化：
♻️ 统一初始化值
std::atomic<uint32_t> data_part3_ = {std::bit_cast<uint32_t>(Dr16DataPart3{ .keyboard = Keyboard::zero(), - .rotary_knob = 0, + .rotary_knob = 1024, })};🤖 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/device/dr16.hpp` around lines 260 - 263, The rotary_knob field in the Dr16DataPart3 initializer (used to set data_part3_) is currently 0 while other joystick channels use 1024 (midpoint); change the initializer for Dr16DataPart3::rotary_knob to 1024 so it matches the DR16 protocol and the expectations of channel_to_double when converting channel values.
175-183: 建议添加static_assert验证std::bit_cast的类型兼容性。
switch_right()、mouse()和keyboard()使用std::bit_cast在本地类型和rmcs_msgs类型之间转换，假设它们具有相同的二进制表示。如果rmcs_msgs类型定义发生变化，这可能导致静默错误。🔧 建议添加类型兼容性断言
+ static_assert(sizeof(Switch) == sizeof(rmcs_msgs::Switch)); + static_assert(sizeof(Mouse) == sizeof(rmcs_msgs::Mouse)); + static_assert(sizeof(Keyboard) == sizeof(rmcs_msgs::Keyboard)); + rmcs_msgs::Switch switch_right() const { return std::bit_cast<rmcs_msgs::Switch>(switch_right_); }🤖 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/device/dr16.hpp` around lines 175 - 183, Add compile-time checks to ensure the types used with std::bit_cast are compatible: for each conversion function (switch_right, switch_left, mouse, keyboard) add static_asserts that the source and target types are trivially copyable and have equal sizeof() (e.g., std::is_trivially_copyable_v<LocalType> && std::is_trivially_copyable_v<rmcs_msgs::Type> and sizeof(LocalType)==sizeof(rmcs_msgs::Type)). Place these static_asserts in the header (near the functions or in an anonymous namespace at top) so any change to the rmcs_msgs types fails to compile instead of producing silent runtime miscasts.
136-170: 使用[[gnu::packed]]和bool位域存在可移植性限制。
[[gnu::packed]]是 GCC/Clang 特有属性，bool类型的位域行为是实现定义的。对于 RoboMaster 项目（通常使用 ARM Linux + GCC），这是可接受的。static_assert提供了良好的编译时保障。如果未来需要跨编译器支持，可考虑使用
uint8_t/uint16_t位域配合显式的位操作。🤖 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/device/dr16.hpp` around lines 136 - 170, The current use of [[gnu::packed]] with bool bit-fields (structs Mouse and Keyboard and their zero() helpers) is implementation-defined and non-portable; replace the bool bit-fields with fixed-width unsigned integer bit-fields (e.g., use uint8_t for Mouse fields and uint16_t for Keyboard fields, declared as uint8_t left:1; uint8_t right:1; and uint16_t w:1; ... b:1;) and update the zero() helpers to bit_cast from the corresponding zero-width integer (keep constexpr static Mouse zero() { constexpr uint8_t zero = 0; return std::bit_cast<Mouse>(zero); } but with matching underlying type) and remove or document the [[gnu::packed]] usage if portability is required—refer to the Mouse and Keyboard struct definitions and their zero() methods to make these changes.rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp (3)
288-303: 漂移校准采样向量可考虑预分配内存。
yaw_samples_和time_samples_在采样窗口期间持续增长。虽然时间窗口有限（默认 3 秒 × 1000Hz ≈ 3000 样本），但可预分配以避免频繁重新分配：可选优化
在构造函数中预分配：
// 预估最大采样数: (final - first) * update_rate size_t estimated_samples = static_cast<size_t>((final_sample_spot_ - first_sample_spot_) * 1000.0); yaw_samples_.reserve(estimated_samples); time_samples_.reserve(estimated_samples);🤖 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/catapult_dart_v3_full.cpp` around lines 288 - 303, The sampling vectors yaw_samples_ and time_samples_ grow during the calibration window causing reallocations; preallocate their capacity in the class constructor (where the object is initialized) by estimating max samples from (final_sample_spot_ - first_sample_spot_) * update_rate (or 1000.0 if update_rate is implicit) and calling reserve(...) on both yaw_samples_ and time_samples_ to avoid repeated reallocations during the sampling loop in the block that updates yaw_drift_coefficient_.
305-305: 魔法数字0.000512需要文档说明或参数化。偏航漂移补偿中的
0.000512硬编码缺乏上下文说明。建议：
- 添加注释解释该值的来源和物理意义
- 或将其作为可配置参数（类似
yaw_drift_coefficient_）- t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; + // 0.000512 rad/s: 基于 BMI088 IMU 的典型陀螺仪零偏漂移补偿 + constexpr double kYawDriftBiasCorrection = 0.000512; + t_yaw -= (yaw_drift_coefficient_ + kYawDriftBiasCorrection) * elapsed_seconds;🤖 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/catapult_dart_v3_full.cpp` at line 305, The magic constant 0.000512 used in the yaw drift correction (t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds) lacks explanation or configurability; replace it by either adding an inline comment describing its origin/units/physical meaning and why it's added to yaw_drift_coefficient_, or refactor it into a named configurable member (e.g., yaw_drift_offset_ or default_yaw_drift_adjustment_) and use that symbol in the expression alongside yaw_drift_coefficient_ and elapsed_seconds so the value is documented and adjustable at runtime/config.
235-236: 空的uart2_receive_callback需要确认是否为预期行为。如果 UART2 确实不需要接收处理，建议添加注释说明原因，避免后续维护者误解。
void uart2_receive_callback(const librmcs::data::UartDataView& data) override { + // CatapultDartV3Full 不使用 UART2 接收，所有舵机通过 PWM/CAN 控制 + (void)data; }🤖 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/catapult_dart_v3_full.cpp` around lines 235 - 236, The empty uart2_receive_callback(const librmcs::data::UartDataView& data) override is ambiguous—either implement the required UART2 receive handling in uart2_receive_callback or, if no handling is intended, add an inline explanatory comment and silence the unused-parameter warning (e.g. explicitly cast or reference data) so maintainers know this is deliberate; update the body of uart2_receive_callback to contain that comment and the minimal unused-parameter suppression.rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/main.cpp (2)
15-31: 信号处理函数中使用的函数不保证信号安全。
fprintf和exit在 POSIX 标准中不是异步信号安全函数。虽然在段错误场景下这通常可以工作，但严格来说可能导致死锁或未定义行为。对于调试目的，当前实现是可接受的。如果需要更严格的信号安全：
可选改进
void segmentation_fault_handler(int) { void* array[100]; int size = backtrace(array, 100); - fprintf(stderr, "[Fatal] Segmentation fault\n>>> STACK TRACE BEGIN\n"); + const char msg1[] = "[Fatal] Segmentation fault\n>>> STACK TRACE BEGIN\n"; + write(STDERR_FILENO, msg1, sizeof(msg1) - 1); if (size >= 2) backtrace_symbols_fd(array + 2, size - 2, STDERR_FILENO); else backtrace_symbols_fd(array, size, STDERR_FILENO); - fprintf(stderr, "<<< STACK TRACE END\n"); - exit(1); + const char msg2[] = "<<< STACK TRACE END\n"; + write(STDERR_FILENO, msg2, sizeof(msg2) - 1); + _exit(1); // 使用 _exit 而非 exit }🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/main.cpp` around lines 15 - 31, The segmentation_fault_handler currently calls non-signal-safe functions (fprintf, exit); update segmentation_fault_handler to use async-signal-safe APIs instead: replace fprintf calls with write(...) to STDERR_FILENO (compose fixed C-strings) and replace exit(1) with _exit(1); keep backtrace/backtrace_symbols_fd usage (they are commonly used in signal handlers) and avoid any new allocations or non-reentrant APIs in that function.
46-47: 入口函数中抛出异常是可接受的，但建议改进错误消息。当参数缺失时抛出异常会终止程序，这是合理的行为。建议增强错误消息以帮助调试：
if (!executor->get_parameter("components", component_descriptions)) - throw std::runtime_error("Missing parameter 'components' or config is not found"); + throw std::runtime_error( + "Missing required parameter 'components'. " + "Ensure a valid config YAML is loaded via --params-file.");🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/main.cpp` around lines 46 - 47, The thrown runtime_error on missing parameter is too generic; improve diagnostics by including context and the parameter name/value when the call to executor->get_parameter("components", component_descriptions) fails. Update the error handling around executor->get_parameter to construct a clearer message (e.g., indicate the node/executable name or parameter key "components", and whether component_descriptions is empty or malformed) and throw std::runtime_error with that enriched message so callers/getting logs can identify which parameter/config failed.rmcs_ws/src/rmcs_bringup/config/dart-launch.yaml (1)
36-41: 多个 PID 参数设置为零，请确认是否为占位符。
pitch_angle_kp、pitch_angle_ki、pitch_angle_kd均为 0.0，这意味着俯仰角 PID 控制器实际上不会产生任何控制输出。
- 如果这是待调参的占位符，建议添加注释说明
- 如果
is_auto_pitch_control_mode: 0表示手动模式而这些参数不会被使用，那么这是合理的🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_bringup/config/dart-launch.yaml` around lines 36 - 41, pitch_angle_kp/pitch_angle_ki/pitch_angle_kd are all zero which effectively disables the pitch PID controller; either document them as placeholders or provide sensible defaults and/or ensure they are not used when is_auto_pitch_control_mode is 0. Update the dart-launch.yaml by (a) adding a short comment next to pitch_angle_kp/ki/kd stating “placeholder — tune before enabling auto pitch” if they are intentionally zero, or (b) set reasonable initial gains (e.g., small nonzero kp/ki/kd) if auto control may be enabled, and verify the code path that reads is_auto_pitch_control_mode (symbol: is_auto_pitch_control_mode) actually bypasses PID usage when in manual mode. Ensure the chosen approach is clearly documented in the file.rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp (1)
1-337: 整个文件被注释掉，作为占位符/脚手架存在。如果短期内不打算启用此代码，建议：
- 在文件顶部添加注释说明此文件的用途和计划启用时间
- 确认
plugins.xml中对DartFillingTestHardware的引用已被注释或移除，否则插件加载时会失败之前的评审中指出的问题（
event_thread_初始化时序、uart_data_ptr未初始化）在代码取消注释后仍需修复。🤖 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/dart_filling_test_hardware.cpp` around lines 1 - 337, The file is completely commented out; add a short top-of-file comment stating this is a placeholder and when it will be enabled, and ensure the PLUGINLIB_EXPORT_CLASS reference to DartFillingTestHardware is removed or the plugins.xml entry is disabled so plugin loading won't fail. If you plan to re-enable the code later, fix the known runtime issues before uncommenting: initialize event_thread_ safely in the DartFillingTestHardware constructor (start the thread after all referenced members are constructed or guard join() with joinable()) and initialize/own the UART buffers (replace raw pointers trigger_servo_uart_data_ptr, limiting_servo_uart_data_ptr, lifting_left_uart_data_ptr, lifting_right_uart_data_ptr with nullptr-initialized pointers or std::unique_ptr and only call transmit_buffer_.add_uart2_transmission() with valid data) so no uninitialized pointer is used when trigger_servo_calibrate_subscription_callback or command_update runs.rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp (2)
305-305: 硬编码的陀螺仪漂移补偿常数
0.000512是一个未经解释的魔法数字。建议将其提取为命名常量或配置参数，并添加注释说明其来源和含义。建议重构
+ // Additional yaw drift offset determined empirically (rad/s) + static constexpr double kYawDriftOffset = 0.000512; + // In processImuData(): - t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; + t_yaw -= (yaw_drift_coefficient_ + kYawDriftOffset) * elapsed_seconds;🤖 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/catapult_dart_v4.cpp` at line 305, 在 catapult_dart_v4.cpp 中把硬编码的 0.000512 提取为有意义的命名常量或可配置参数（例如 constexpr double kGyroDriftCompensation = 0.000512 或成员变量 gyro_drift_compensation_），并在定义处添加注释说明该常数的来源/单位/测量方法；然后在使用处将 t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; 改为使用该命名常量或配置字段（例如 t_yaw -= (yaw_drift_coefficient_ + kGyroDriftCompensation) * elapsed_seconds;），确保同时更新相关构造函数/配置加载以支持可配置化。
235-236: 空的回调函数建议添加注释
uart2_receive_callback为空实现。如果这是有意为之（例如 UART2 暂不使用），建议添加简短注释说明原因。建议
void uart2_receive_callback(const librmcs::data::UartDataView& data) override { + // UART2 not used in CatapultDartV4 + (void)data; }🤖 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/catapult_dart_v4.cpp` around lines 235 - 236, uart2_receive_callback is an empty override with no explanation; either implement the intended UART2 handling or, if leaving it empty intentionally (e.g., UART2 unused), add a brief comment inside uart2_receive_callback explaining why it’s a no-op (mentioning it’s intentionally left empty or deferred) so future readers aren’t confused; locate the uart2_receive_callback(const librmcs::data::UartDataView& data) override in the CatapultDartV4 class and add the explanatory comment or implement the required handling.rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/executor.hpp (1)
185-185: 移除命名空间闭合括号后的多余分号命名空间闭合括号后的分号是多余的。
建议修复
-}; // namespace rmcs_executor +} // namespace rmcs_executor🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/executor.hpp` at line 185, 在 rmcs_executor 的命名空间结尾处删除多余的分号：定位到关闭命名空间的右大括号 '}'（namespace rmcs_executor 的闭合处），移除紧随其后的分号；确保只保留单个右大括号来关闭命名空间（即删除位于 executor.hpp 中 namespace rmcs_executor 闭合括号后的多余 ';'）。rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp (2)
255-260: 浮点数零值比较使用
== 0.0比较浮点数通常不推荐，但此处set_velocity直接赋值为0.0字面量，可精确比较。建议添加注释说明这是有意为之。🤖 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/dart/launch_controller_v2_full.cpp` around lines 255 - 260, The code in drive_belt_sync_control uses a direct floating-point equality check (set_velocity == 0.0); since that can be surprising, either replace it with a robust near-zero check (e.g., fabs(set_velocity) < epsilon) or, if the value is guaranteed to be a literal 0.0, add a short clarifying comment immediately above the if stating this exact comparison is intentional because set_velocity is set from a literal/controlled source; reference drive_belt_sync_control and the variables set_velocity, left_drive_belt_control_torque_, and right_drive_belt_control_torque_ when making the change.
276-284: 阻塞检测使用了硬编码的魔法数字速度阈值
0.5、扭矩阈值0.5和计数阈值1000应提取为命名常量或配置参数，以提高可读性和可维护性。建议重构
+ // Blocking detection thresholds + static constexpr double kBlockingVelocityThreshold = 0.5; + static constexpr double kBlockingTorqueThreshold = 0.5; + static constexpr int kBlockingCountThreshold = 1000; + bool blocking_detection() { - if ((std::abs(*left_drive_belt_velocity_) < 0.5 - && std::abs(*left_drive_belt_control_torque_) > 0.5) - || (std::abs(*right_drive_belt_velocity_) < 0.5 - && std::abs(*right_drive_belt_control_torque_) > 0.5)) { + if ((std::abs(*left_drive_belt_velocity_) < kBlockingVelocityThreshold + && std::abs(*left_drive_belt_control_torque_) > kBlockingTorqueThreshold) + || (std::abs(*right_drive_belt_velocity_) < kBlockingVelocityThreshold + && std::abs(*right_drive_belt_control_torque_) > kBlockingTorqueThreshold)) { drive_belt_block_count_++; } - return drive_belt_block_count_ > 1000; + return drive_belt_block_count_ > kBlockingCountThreshold; }🤖 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/dart/launch_controller_v2_full.cpp` around lines 276 - 284, The blocking_detection() function uses hard-coded magic numbers (0.5 and 1000); replace them with named constants or configurable member variables (e.g., kVelocityThreshold, kTorqueThreshold, kBlockCountThreshold or velocity_threshold_, torque_threshold_, block_count_threshold_) and use those in the comparisons against left_drive_belt_velocity_, left_drive_belt_control_torque_, right_drive_belt_velocity_, right_drive_belt_control_torque_, and drive_belt_block_count_; declare these constants as class-level static constexpr or as configurable members initialized from parameters/config so callers can tune them, then update the function to reference the new names instead of literal numbers.rmcs_ws/src/rmcs_core/plugins.xml (1)
101-130: 建议为新插件添加更具描述性的说明较早添加的插件条目（如
ForceSensorTest、DartLaunchController等）使用了通用的 "Test plugin." 描述，而后面的条目（Lines 134-154）提供了更有意义的描述。建议统一风格，为所有新插件提供清晰的功能描述，便于后续维护和理解。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_core/plugins.xml` around lines 101 - 130, 将文件中多个插件条目的通用描述 "Test plugin." 替换为更具描述性的说明以统一风格并便于维护；定位并更新类名 rmcs_core::hardware::ForceSensorTest、rmcs_core::hardware::ForceSensorReceive、rmcs_core::controller::dart::DartLaunchController、rmcs_core::hardware::CatapultDart、rmcs_core::controller::dart::DartSettings、rmcs_core::controller::dart::Test、rmcs_core::hardware::DartFillingTestHardware、rmcs_core::controller::dart::DartFillingTest、rmcs_core::hardware::ServoTest 和 rmcs_core::controller::test::PWMTest 的 <description> 元素，分别填入简短且具体的功能描述（例如传感器读取、飞镖发射控制、舵机测试、PWM 输出测试等），确保风格和详尽程度与文件中后面已有条目一致。
🤖 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_bringup/config/dart-launch-full.yaml`:
- Line 5: 配置引用了已被注释掉的组件类
rmcs_core::hardware::CatapultDartV3Lk，导致插件加载失败；修复方法二选一：1) 在实现文件
catapult_dart_v3_lk.cpp 中恢复（取消注释并确保编译通过）CatapultDartV3Lk 的实现并重新构建，以保留配置中的映射
rmcs_core::hardware::CatapultDartV3Lk -> dart_hardware；或 2) 修改配置
dart-launch-full.yaml 中将组件引用从 rmcs_core::hardware::CatapultDartV3Lk 替换为
rmcs_core::hardware::CatapultDartV3Full（保持箭头映射 -> dart_hardware
不变），然后重新构建并验证插件能正确加载。
In `@rmcs_ws/src/rmcs_bringup/config/dart.yaml`:
- Around line 84-91: The pitch_velocity_pid_controller currently has kp/ki/kd
all set to 0.0 which makes its control_torque output always zero; update the PID
parameters for pitch_velocity_pid_controller (kp, ki, kd) to sensible non-zero
gains (or load tuned params) so the /pitch/control/velocity setpoint produces a
non-zero /dart/pitch_motor/control_torque output—locate the
pitch_velocity_pid_controller block and replace the zero gains with appropriate
tuned values (or wire it to a properly tuned controller) to restore pitch axis
responsiveness.
In `@rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp`:
- Around line 66-67: The issue is that the output variable filling_stage_ is
registered via register_output("/dart/filling/stage", filling_stage_) but never
assigned, so the broadcaster always sees the default value; fix by writing to
*filling_stage_ at every place the stage can change: set concrete stage values
inside update() where state transitions occur (use the same enum/int values you
expect consumers to read), and reset or set a terminal/default stage in
disable_all() so the UI/broadcaster sees the correct stage when disabled; search
for filling_stage_, register_output, update(), and disable_all() in the diff
(lines ~74-135) and add the appropriate assignments.
- Around line 80-85: 当前逻辑只有当左右拨杆同时为 DOWN/UNKNOWN 才调用 disable_all()，导致单侧为
Switch::UNKNOWN 不会触发停机；请在判断中把任何一侧为 Switch::UNKNOWN 视为需停机的异常，修改包含 sw_left 和
sw_right 的条件（例如在调用 disable_all() 的分支中加入 sw_left == Switch::UNKNOWN || sw_right
== Switch::UNKNOWN 或重写为 if (sw_left == Switch::UNKNOWN || sw_right ==
Switch::UNKNOWN || (sw_left == Switch::DOWN && sw_right == Switch::DOWN))
disable_all()），以保证单侧 UNKNOWN 时也会执行 disable_all().
In `@rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp`:
- Around line 28-39: 当前代码把 /pitch/control/velocity 注册为一个 Eigen::Vector2d
(yaw_pitch_vel_setpoint_) 并在 update() 中用 yaw_pitch_velocity[0]/[1]，但实际生产者在
/yaw/control/velocity 和 /pitch/control/velocity 分别发布 double；把这部分改为分别注册两个标量输入（例如
register_input("/yaw/control/velocity", yaw_setpoint_) 和
register_input("/pitch/control/velocity", pitch_setpoint_)），然后在 update() 用
*yaw_setpoint_ 和 *pitch_setpoint_ 计算误差并传入 yaw_pid_.update(...) 与
pitch_pid_.update(...)，保持其余变量名（yaw_velocity_, pitch_velocity_, yaw_torque_,
pitch_torque_）不变以确保契约一致。
In `@rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp`:
- Around line 88-92: The update() path for is_auto_pitch_control_mode_ == 1
currently only calls pitch_control() which only updates PID params and setpoint
but does not produce actual control output because the assignment to
*pitch_control_velocity_ was commented out; restore and compute the velocity
assignment inside the auto-pitch branch (and similarly in the block around lines
127-134) by retrieving the PID output/setpoint from pitch_control() (or the
relevant member variables) and setting *pitch_control_velocity_ = computed_angle
* max_transform_rate_ (or the PID output scaled by max_transform_rate_),
ensuring the double-loop PID semantics are preserved and the break logic remains
unchanged so automatic pitch produces a real control command.
In `@rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp`:
- Around line 64-67: The parameter keys passed to get_parameter currently
include trailing underscores which can mismatch YAML names; update the
get_parameter calls so they use "limiting_free_angle" and "limiting_lock_angle"
(without the trailing underscore) while keeping the member variables
limiting_free_angle_ and limiting_lock_angle_ as-is; locate the two lines that
call get_parameter for limiting_free_angle_ and limiting_lock_angle_ and change
only the string keys to remove the trailing underscore so runtime parameter
lookup matches the YAML.
- Around line 303-309: delay_and_execute currently silently drops new requests
when is_delaying_ is true; change its behavior so callers won't lose actions by
either queuing or replacing the pending action—implement one of the suggested
fixes: (A) add a small FIFO queue member (e.g.,
std::deque<std::function<void()>>) and push the new action when is_delaying_ is
true so delayed_action_ remains the currently executing one and subsequent ticks
dequeue into delayed_action_; or (B) cancel/replace the current delay by setting
delay_remaining_ms_ = delay_ms and delayed_action_ = std::move(action) even if
is_delaying_ is true; or (C) at minimum call the logger to warn when an action
is dropped (e.g., log inside delay_and_execute when is_delaying_ is true before
returning). Update uses of is_delaying_, delay_remaining_ms_, and
delayed_action_ accordingly and ensure thread-safety if this method can be
called concurrently.
In `@rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_lk.cpp`:
- Around line 1-538: The entire CatapultDartV3Lk implementation (class
CatapultDartV3Lk, its methods, and the PLUGINLIB_EXPORT_CLASS macro) is
commented out, preventing pluginlib from loading the
rmcs_core::hardware::CatapultDartV3Lk component referenced by
dart-launch-full.yaml; either uncomment the full contents of
catapult_dart_v3_lk.cpp (restoring the class definition, methods like update(),
command_update(), processImuData(), can*/uart* callbacks, and the
PLUGINLIB_EXPORT_CLASS(...) line) and rebuild, or remove the
rmcs_core::hardware::CatapultDartV3Lk entry from dart-launch-full.yaml (or
delete the YAML) so the launch no longer references the missing plugin.
In `@rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp`:
- Around line 186-195: The log message in can1_receive_callback incorrectly
labels the source as "[ForceSensor CAN2:0x302]" which mismatches the callback
context; update the log string used where force_sensor_frame_count_ is logged to
reflect CAN1 (e.g., "[ForceSensor CAN1:0x302]") or otherwise match the actual
bus/handler, keeping the rest of the log call using logger_,
data.can_data.size(), and bytes_to_hex_string(...) unchanged; locate this in
can1_receive_callback around the force_sensor_frame_count_ increment and
force_sensor_.store_status(data.can_data) and change only the textual label to
the correct CAN bus.
- Around line 91-92: The subscription created for
force_sensor_calibrate_subscription_ uses rclcpp::QoS{0} which sets queue depth
to 0 and can drop messages; update the subscription creation (where
force_sensor_calibrate_subscription_ is constructed) to use a non‑zero QoS depth
(e.g. rclcpp::QoS(1) or larger) and consider setting reliability to Reliable
(rclcpp::ReliabilityPolicy::Reliable) for the calibration topic (or both) so
calibration commands are not lost.
In `@rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp`:
- Around line 21-25: The store_status function currently accepts and zero-pads
short CAN frames which treats incomplete/ corrupted input as valid; change
store_status to verify can_data.size() == sizeof(ForceSensorStatus) before
copying and updating force_sensor_status_ (i.e., only memcpy into a local
ForceSensorStatus and call force_sensor_status_.store(...) when the size
matches); if the size is different, skip the update (or optionally log an error)
so incomplete frames are not published.
- Around line 17-18: 当前代码使用 std::numeric_limits<int>::quiet_NaN()（在定义 nan_
的位置）并将 nan_ 作为 OutputInterface<int>（在 status_component.register_output 调用中用于
weight_ch1_/weight_ch2_ 的初始值），但对整型该调用返回 0，导致 0 被误当作无效哨兵值。将 nan_ 替换为明确的整型哨兵（例如
std::numeric_limits<int>::min() 或一个项目定义的
INVALID_WEIGHT_SENTINEL），并同时重命名变量为更明确的名称（例如 invalid_weight_ 或
sentinel_value_）；在定义（原使用 std::numeric_limits<int>::quiet_NaN() 的位置）和在
status_component.register_output("/force_sensor/channel_1/weight", weight_ch1_,
nan_) 与 "/channel_2/weight" 的注册处一并替换，确保 OutputInterface<int> 初始值为该新的哨兵值。
In `@rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/main.cpp`:
- Around line 64-66: The code sets
rmcs_executor::Component::initializing_component_name to component_name.c_str(),
creating a dangling pointer because component_name is a loop-local std::string;
change the implementation so the Component stores a std::string copy instead of
a const char* (make initializing_component_name a static std::string in class
Component and assign it with component_name), or ensure the Component
constructor copies the string passed to it; update locations that set
initializing_component_name (the lines using component_name.c_str(),
createSharedInstance and add_component) to assign the std::string rather than a
raw pointer.
In
`@rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/predefined_msg_provider.hpp`:
- Around line 10-17: 当前把 update_count_ 注册为 size_t(-1) 会导致首次调用 update() 时从
SIZE_MAX 回绕到 0；请将注册初值改为 0（例如在 register_output 调用里把初始值从 static_cast<size_t>(-1)
改为 static_cast<size_t>(0) 或直接 0），以确保 update_count_ 在 update()（*update_count_ +=
1）开始时从 0 向上递增而不是回绕；检查 register_output("/predefined/update_count", update_count_,
...) 和成员变量 update_count_ 的类型一致性即可完成修复。
---
Duplicate comments:
In `@rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp`:
- Around line 77-105: 在 update() 函数帧开头先把三路输出统一写成安全值（例如把 *yaw_control_velocity_,
*pitch_control_velocity_, *force_control_ 设为 0 或 reset_all_controls()
的等效安全值），然后再进入原来的分支（涉及
switch_left/switch_right、is_auto_pitch_control_mode_、is_auto_force_control_mode_、pitch_control()、force_control()
等）由命中分支覆盖对应通道的值，确保未命中的通道不会沿用上一帧残留命令。
---
Nitpick comments:
In `@rmcs_ws/src/rmcs_bringup/config/dart-launch.yaml`:
- Around line 36-41: pitch_angle_kp/pitch_angle_ki/pitch_angle_kd are all zero
which effectively disables the pitch PID controller; either document them as
placeholders or provide sensible defaults and/or ensure they are not used when
is_auto_pitch_control_mode is 0. Update the dart-launch.yaml by (a) adding a
short comment next to pitch_angle_kp/ki/kd stating “placeholder — tune before
enabling auto pitch” if they are intentionally zero, or (b) set reasonable
initial gains (e.g., small nonzero kp/ki/kd) if auto control may be enabled, and
verify the code path that reads is_auto_pitch_control_mode (symbol:
is_auto_pitch_control_mode) actually bypasses PID usage when in manual mode.
Ensure the chosen approach is clearly documented in the file.
In `@rmcs_ws/src/rmcs_core/plugins.xml`:
- Around line 101-130: 将文件中多个插件条目的通用描述 "Test plugin."
替换为更具描述性的说明以统一风格并便于维护；定位并更新类名
rmcs_core::hardware::ForceSensorTest、rmcs_core::hardware::ForceSensorReceive、rmcs_core::controller::dart::DartLaunchController、rmcs_core::hardware::CatapultDart、rmcs_core::controller::dart::DartSettings、rmcs_core::controller::dart::Test、rmcs_core::hardware::DartFillingTestHardware、rmcs_core::controller::dart::DartFillingTest、rmcs_core::hardware::ServoTest
和 rmcs_core::controller::test::PWMTest 的 <description>
元素，分别填入简短且具体的功能描述（例如传感器读取、飞镖发射控制、舵机测试、PWM 输出测试等），确保风格和详尽程度与文件中后面已有条目一致。
In `@rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp`:
- Around line 255-260: The code in drive_belt_sync_control uses a direct
floating-point equality check (set_velocity == 0.0); since that can be
surprising, either replace it with a robust near-zero check (e.g.,
fabs(set_velocity) < epsilon) or, if the value is guaranteed to be a literal
0.0, add a short clarifying comment immediately above the if stating this exact
comparison is intentional because set_velocity is set from a literal/controlled
source; reference drive_belt_sync_control and the variables set_velocity,
left_drive_belt_control_torque_, and right_drive_belt_control_torque_ when
making the change.
- Around line 276-284: The blocking_detection() function uses hard-coded magic
numbers (0.5 and 1000); replace them with named constants or configurable member
variables (e.g., kVelocityThreshold, kTorqueThreshold, kBlockCountThreshold or
velocity_threshold_, torque_threshold_, block_count_threshold_) and use those in
the comparisons against left_drive_belt_velocity_,
left_drive_belt_control_torque_, right_drive_belt_velocity_,
right_drive_belt_control_torque_, and drive_belt_block_count_; declare these
constants as class-level static constexpr or as configurable members initialized
from parameters/config so callers can tune them, then update the function to
reference the new names instead of literal numbers.
In `@rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp`:
- Around line 288-303: The sampling vectors yaw_samples_ and time_samples_ grow
during the calibration window causing reallocations; preallocate their capacity
in the class constructor (where the object is initialized) by estimating max
samples from (final_sample_spot_ - first_sample_spot_) * update_rate (or 1000.0
if update_rate is implicit) and calling reserve(...) on both yaw_samples_ and
time_samples_ to avoid repeated reallocations during the sampling loop in the
block that updates yaw_drift_coefficient_.
- Line 305: The magic constant 0.000512 used in the yaw drift correction (t_yaw
-= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds) lacks explanation or
configurability; replace it by either adding an inline comment describing its
origin/units/physical meaning and why it's added to yaw_drift_coefficient_, or
refactor it into a named configurable member (e.g., yaw_drift_offset_ or
default_yaw_drift_adjustment_) and use that symbol in the expression alongside
yaw_drift_coefficient_ and elapsed_seconds so the value is documented and
adjustable at runtime/config.
- Around line 235-236: The empty uart2_receive_callback(const
librmcs::data::UartDataView& data) override is ambiguous—either implement the
required UART2 receive handling in uart2_receive_callback or, if no handling is
intended, add an inline explanatory comment and silence the unused-parameter
warning (e.g. explicitly cast or reference data) so maintainers know this is
deliberate; update the body of uart2_receive_callback to contain that comment
and the minimal unused-parameter suppression.
In `@rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp`:
- Line 305: 在 catapult_dart_v4.cpp 中把硬编码的 0.000512 提取为有意义的命名常量或可配置参数（例如
constexpr double kGyroDriftCompensation = 0.000512 或成员变量
gyro_drift_compensation_），并在定义处添加注释说明该常数的来源/单位/测量方法；然后在使用处将 t_yaw -=
(yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; 改为使用该命名常量或配置字段（例如 t_yaw
-= (yaw_drift_coefficient_ + kGyroDriftCompensation) *
elapsed_seconds;），确保同时更新相关构造函数/配置加载以支持可配置化。
- Around line 235-236: uart2_receive_callback is an empty override with no
explanation; either implement the intended UART2 handling or, if leaving it
empty intentionally (e.g., UART2 unused), add a brief comment inside
uart2_receive_callback explaining why it’s a no-op (mentioning it’s
intentionally left empty or deferred) so future readers aren’t confused; locate
the uart2_receive_callback(const librmcs::data::UartDataView& data) override in
the CatapultDartV4 class and add the explanatory comment or implement the
required handling.
In `@rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp`:
- Around line 1-337: The file is completely commented out; add a short
top-of-file comment stating this is a placeholder and when it will be enabled,
and ensure the PLUGINLIB_EXPORT_CLASS reference to DartFillingTestHardware is
removed or the plugins.xml entry is disabled so plugin loading won't fail. If
you plan to re-enable the code later, fix the known runtime issues before
uncommenting: initialize event_thread_ safely in the DartFillingTestHardware
constructor (start the thread after all referenced members are constructed or
guard join() with joinable()) and initialize/own the UART buffers (replace raw
pointers trigger_servo_uart_data_ptr, limiting_servo_uart_data_ptr,
lifting_left_uart_data_ptr, lifting_right_uart_data_ptr with nullptr-initialized
pointers or std::unique_ptr and only call
transmit_buffer_.add_uart2_transmission() with valid data) so no uninitialized
pointer is used when trigger_servo_calibrate_subscription_callback or
command_update runs.
In `@rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp`:
- Around line 260-263: The rotary_knob field in the Dr16DataPart3 initializer
(used to set data_part3_) is currently 0 while other joystick channels use 1024
(midpoint); change the initializer for Dr16DataPart3::rotary_knob to 1024 so it
matches the DR16 protocol and the expectations of channel_to_double when
converting channel values.
- Around line 175-183: Add compile-time checks to ensure the types used with
std::bit_cast are compatible: for each conversion function (switch_right,
switch_left, mouse, keyboard) add static_asserts that the source and target
types are trivially copyable and have equal sizeof() (e.g.,
std::is_trivially_copyable_v<LocalType> &&
std::is_trivially_copyable_v<rmcs_msgs::Type> and
sizeof(LocalType)==sizeof(rmcs_msgs::Type)). Place these static_asserts in the
header (near the functions or in an anonymous namespace at top) so any change to
the rmcs_msgs types fails to compile instead of producing silent runtime
miscasts.
- Around line 136-170: The current use of [[gnu::packed]] with bool bit-fields
(structs Mouse and Keyboard and their zero() helpers) is implementation-defined
and non-portable; replace the bool bit-fields with fixed-width unsigned integer
bit-fields (e.g., use uint8_t for Mouse fields and uint16_t for Keyboard fields,
declared as uint8_t left:1; uint8_t right:1; and uint16_t w:1; ... b:1;) and
update the zero() helpers to bit_cast from the corresponding zero-width integer
(keep constexpr static Mouse zero() { constexpr uint8_t zero = 0; return
std::bit_cast<Mouse>(zero); } but with matching underlying type) and remove or
document the [[gnu::packed]] usage if portability is required—refer to the Mouse
and Keyboard struct definitions and their zero() methods to make these changes.
In `@rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/executor.hpp`:
- Line 185: 在 rmcs_executor 的命名空间结尾处删除多余的分号：定位到关闭命名空间的右大括号 '}'（namespace
rmcs_executor 的闭合处），移除紧随其后的分号；确保只保留单个右大括号来关闭命名空间（即删除位于 executor.hpp 中 namespace
rmcs_executor 闭合括号后的多余 ';'）。
In `@rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/main.cpp`:
- Around line 15-31: The segmentation_fault_handler currently calls
non-signal-safe functions (fprintf, exit); update segmentation_fault_handler to
use async-signal-safe APIs instead: replace fprintf calls with write(...) to
STDERR_FILENO (compose fixed C-strings) and replace exit(1) with _exit(1); keep
backtrace/backtrace_symbols_fd usage (they are commonly used in signal handlers)
and avoid any new allocations or non-reentrant APIs in that function.
- Around line 46-47: The thrown runtime_error on missing parameter is too
generic; improve diagnostics by including context and the parameter name/value
when the call to executor->get_parameter("components", component_descriptions)
fails. Update the error handling around executor->get_parameter to construct a
clearer message (e.g., indicate the node/executable name or parameter key
"components", and whether component_descriptions is empty or malformed) and
throw std::runtime_error with that enriched message so callers/getting logs can
identify which parameter/config failed.
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: 203caf99-fece-4099-b13a-c4add6319e2d
📒 Files selected for processing (27)
rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yamlrmcs_ws/src/rmcs_bringup/config/dart-guidance.yamlrmcs_ws/src/rmcs_bringup/config/dart-launch-full.yamlrmcs_ws/src/rmcs_bringup/config/dart-launch.yamlrmcs_ws/src/rmcs_bringup/config/dart.yamlrmcs_ws/src/rmcs_core/plugins.xmlrmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpprmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpprmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpprmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpprmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2.cpprmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpprmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpprmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpprmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpprmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpprmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_lk.cpprmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpprmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpprmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpprmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpprmcs_ws/src/rmcs_dart_guidancermcs_ws/src/rmcs_executor/include/rmcs_executor/src/component.cpprmcs_ws/src/rmcs_executor/include/rmcs_executor/src/executor.hpprmcs_ws/src/rmcs_executor/include/rmcs_executor/src/main.cpprmcs_ws/src/rmcs_executor/include/rmcs_executor/src/predefined_msg_provider.hpprmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_slider_status.hpp
✅ Files skipped from review due to trivial changes (2)
- rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2.cpp
- rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp
🚧 Files skipped from review as they are similar to previous changes (2)
- rmcs_ws/src/rmcs_dart_guidance
- rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml
Comment on lines
+84
to
+91
| pitch_velocity_pid_controller: | ||
| ros__parameters: | ||
| measurement: /dart/pitch_motor/velocity | ||
| setpoint: /pitch/control/velocity | ||
| control: /dart/pitch_motor/control_torque | ||
| kp: 0.0 | ||
| ki: 0.0 | ||
| kd: 0.0 |
There was a problem hiding this comment.
pitch_velocity_pid_controller 现在恒等于停机。
kp/ki/kd 全是 0.0 时，/pitch/control/velocity 无论给什么 setpoint，输出的 control_torque 都只会是 0。按当前组件图，pitch 轴根本不会响应控制命令。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@rmcs_ws/src/rmcs_bringup/config/dart.yaml` around lines 84 - 91, The
pitch_velocity_pid_controller currently has kp/ki/kd all set to 0.0 which makes
its control_torque output always zero; update the PID parameters for
pitch_velocity_pid_controller (kp, ki, kd) to sensible non-zero gains (or load
tuned params) so the /pitch/control/velocity setpoint produces a non-zero
/dart/pitch_motor/control_torque output—locate the pitch_velocity_pid_controller
block and replace the zero gains with appropriate tuned values (or wire it to a
properly tuned controller) to restore pitch axis responsiveness.
Comment on lines
+66
to
+67
| register_output("/dart/filling/stage", filling_stage_); | ||
| register_output("/force/sensor/average", average_force_); |
There was a problem hiding this comment.
filling_stage_ 只注册不赋值。
构造函数注册了 /dart/filling/stage，但 update() 和 disable_all() 从头到尾都没有写 *filling_stage_。这样 broadcaster/上位机最多只能看到默认值，根本无法判断测试流程当前处于哪个阶段。
Also applies to: 74-135
🤖 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/dart/dart_filling_test.cpp` around lines
66 - 67, The issue is that the output variable filling_stage_ is registered via
register_output("/dart/filling/stage", filling_stage_) but never assigned, so
the broadcaster always sees the default value; fix by writing to *filling_stage_
at every place the stage can change: set concrete stage values inside update()
where state transitions occur (use the same enum/int values you expect consumers
to read), and reset or set a terminal/default stage in disable_all() so the
UI/broadcaster sees the correct stage when disabled; search for filling_stage_,
register_output, update(), and disable_all() in the diff (lines ~74-135) and add
the appropriate assignments.
Comment on lines
+80
to
+85
| const bool all_down = (sw_left == Switch::DOWN || sw_left == Switch::UNKNOWN) | ||
| && (sw_right == Switch::DOWN || sw_right == Switch::UNKNOWN); | ||
|
|
||
| if (all_down) { | ||
| disable_all(); | ||
| } else if (sw_left == Switch::MIDDLE) { |
There was a problem hiding this comment.
单侧 UNKNOWN 不会触发停机。
这里只有“两侧都为 DOWN/UNKNOWN” 才会进入 disable_all()。一旦任一拨杆单独掉成 UNKNOWN，代码会跳过所有控制分支并把上一帧 torque 锁存下来；对遥控链路异常来说这是危险的。
建议修正
- const bool all_down = (sw_left == Switch::DOWN || sw_left == Switch::UNKNOWN)
- && (sw_right == Switch::DOWN || sw_right == Switch::UNKNOWN);
+ const bool emergency_stop =
+ sw_left == Switch::UNKNOWN || sw_right == Switch::UNKNOWN
+ || (sw_left == Switch::DOWN && sw_right == Switch::DOWN);
- if (all_down) {
+ if (emergency_stop) {
disable_all();🤖 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/dart/dart_filling_test.cpp` around lines
80 - 85, 当前逻辑只有当左右拨杆同时为 DOWN/UNKNOWN 才调用 disable_all()，导致单侧为 Switch::UNKNOWN
不会触发停机；请在判断中把任何一侧为 Switch::UNKNOWN 视为需停机的异常，修改包含 sw_left 和 sw_right 的条件（例如在调用
disable_all() 的分支中加入 sw_left == Switch::UNKNOWN || sw_right == Switch::UNKNOWN
或重写为 if (sw_left == Switch::UNKNOWN || sw_right == Switch::UNKNOWN || (sw_left
== Switch::DOWN && sw_right == Switch::DOWN)) disable_all()），以保证单侧 UNKNOWN 时也会执行
disable_all().
Comment on lines
+28
to
+39
| register_input("/pitch/control/velocity", yaw_pitch_vel_setpoint_); | ||
| register_input("/dart/yaw_motor/velocity", yaw_velocity_); | ||
| register_input("/dart/pitch_motor/velocity", pitch_velocity_); | ||
|
|
||
| register_output("/dart/yaw_motor/control_torque", yaw_torque_, 0.0); | ||
| register_output("/dart/pitch_motor/control_torque", pitch_torque_, 0.0); | ||
| } | ||
|
|
||
| void update() override { | ||
| const Eigen::Vector2d& yaw_pitch_velocity = *yaw_pitch_vel_setpoint_; | ||
| *yaw_torque_ = yaw_pid_.update(yaw_pitch_velocity[0] - *yaw_velocity_); | ||
| *pitch_torque_ = pitch_pid_.update(yaw_pitch_velocity[1] - *pitch_velocity_); |
There was a problem hiding this comment.
输入话题契约和 DartSettings 对不上。
这里把 /pitch/control/velocity 当成 Eigen::Vector2d 读取，并且把 [0] 当 yaw、[1] 当 pitch；但同一 PR 里的 rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp 在 /yaw/control/velocity 和 /pitch/control/velocity 上分别发布的是 double。按当前改动，这个控制器接不上任何现有生产者。
更直接的接法
- register_input("/pitch/control/velocity", yaw_pitch_vel_setpoint_);
+ register_input("/yaw/control/velocity", yaw_velocity_setpoint_);
+ register_input("/pitch/control/velocity", pitch_velocity_setpoint_);
register_input("/dart/yaw_motor/velocity", yaw_velocity_);
register_input("/dart/pitch_motor/velocity", pitch_velocity_);
@@
- const Eigen::Vector2d& yaw_pitch_velocity = *yaw_pitch_vel_setpoint_;
- *yaw_torque_ = yaw_pid_.update(yaw_pitch_velocity[0] - *yaw_velocity_);
- *pitch_torque_ = pitch_pid_.update(yaw_pitch_velocity[1] - *pitch_velocity_);
+ *yaw_torque_ = yaw_pid_.update(*yaw_velocity_setpoint_ - *yaw_velocity_);
+ *pitch_torque_ = pitch_pid_.update(*pitch_velocity_setpoint_ - *pitch_velocity_);
@@
- InputInterface<Eigen::Vector2d> yaw_pitch_vel_setpoint_;
- InputInterface<double> yaw_velocity_;
- InputInterface<double> pitch_velocity_;
+ InputInterface<double> yaw_velocity_setpoint_;
+ InputInterface<double> pitch_velocity_setpoint_;
+ InputInterface<double> yaw_velocity_;
+ InputInterface<double> pitch_velocity_;Also applies to: 46-48
🤖 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/dart/dart_setting.cpp` around lines 28 -
39, 当前代码把 /pitch/control/velocity 注册为一个 Eigen::Vector2d
(yaw_pitch_vel_setpoint_) 并在 update() 中用 yaw_pitch_velocity[0]/[1]，但实际生产者在
/yaw/control/velocity 和 /pitch/control/velocity 分别发布 double；把这部分改为分别注册两个标量输入（例如
register_input("/yaw/control/velocity", yaw_setpoint_) 和
register_input("/pitch/control/velocity", pitch_setpoint_)），然后在 update() 用
*yaw_setpoint_ 和 *pitch_setpoint_ 计算误差并传入 yaw_pid_.update(...) 与
pitch_pid_.update(...)，保持其余变量名（yaw_velocity_, pitch_velocity_, yaw_torque_,
pitch_torque_）不变以确保契约一致。
Comment on lines
+186
to
+195
| if (can_id == 0x302) { | ||
| force_sensor_frame_count_++; | ||
| if (force_sensor_frame_count_ % 200 == 1) { | ||
| RCLCPP_INFO( | ||
| logger_, "[ForceSensor CAN2:0x302] raw(%zu): %s", | ||
| data.can_data.size(), | ||
| bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); | ||
| } | ||
| force_sensor_.store_status(data.can_data); | ||
| } |
There was a problem hiding this comment.
日志注释与代码位置不匹配
日志消息中写的是 [ForceSensor CAN2:0x302]，但此代码位于 can1_receive_callback 中。这会造成调试时的困惑。
建议修复
if (can_id == 0x302) {
force_sensor_frame_count_++;
if (force_sensor_frame_count_ % 200 == 1) {
RCLCPP_INFO(
- logger_, "[ForceSensor CAN2:0x302] raw(%zu): %s",
+ logger_, "[ForceSensor CAN1:0x302] raw(%zu): %s",
data.can_data.size(),
bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str());
}🤖 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/catapult_dart_v4.cpp` around lines 186 -
195, The log message in can1_receive_callback incorrectly labels the source as
"[ForceSensor CAN2:0x302]" which mismatches the callback context; update the log
string used where force_sensor_frame_count_ is logged to reflect CAN1 (e.g.,
"[ForceSensor CAN1:0x302]") or otherwise match the actual bus/handler, keeping
the rest of the log call using logger_, data.can_data.size(), and
bytes_to_hex_string(...) unchanged; locate this in can1_receive_callback around
the force_sensor_frame_count_ increment and
force_sensor_.store_status(data.can_data) and change only the textual label to
the correct CAN bus.
Comment on lines
+17
to
+18
| status_component.register_output("/force_sensor/channel_1/weight", weight_ch1_, nan_); | ||
| status_component.register_output("/force_sensor/channel_2/weight", weight_ch2_, nan_); |
There was a problem hiding this comment.
🧩 Analysis chain
🌐 Web query:
In C++23, what does std::numeric_limits<int>::quiet_NaN() return for integral types, and can it be used as a true NaN sentinel for int outputs?
💡 Result:
For integral types like int, std::numeric_limits<int>::has_quiet_NaN is false, and std::numeric_limits<int>::quiet_NaN() returns 0 (value-initialized int). [1][2]
So it cannot be used as a “true NaN” sentinel for int outputs—there is no NaN representation for integers, and the returned value (0) is just a normal integer that compares equal to itself and may be a perfectly valid result. [1][2]
Sources: [1] [2]
🏁 Script executed:
find rmcs_ws -name "force_sensor.hpp" -type fRepository: Alliance-Algorithm/RMCS
Length of output: 125
🏁 Script executed:
cat -n rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hppRepository: Alliance-Algorithm/RMCS
Length of output: 2774
🏁 Script executed:
find rmcs_ws -name "can_packet.hpp" -type fRepository: Alliance-Algorithm/RMCS
Length of output: 123
🏁 Script executed:
find rmcs_ws -path "*/rmcs_executor/*" -name "*.hpp" -type f | head -20Repository: Alliance-Algorithm/RMCS
Length of output: 374
🏁 Script executed:
cat -n rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp | head -150Repository: Alliance-Algorithm/RMCS
Length of output: 6143
nan_ 对当前整型输出无效，初始值为0会被当作有效数据发布
Line 49 使用 std::numeric_limits<int>::quiet_NaN()，但对整型而言此函数返回 0（整数没有 NaN 表示），Line 58-59 的 OutputInterface<int> 会将此转换后的 int(0) 作为初始值。由于 0 可能是有效的权重值，无法可靠地用作无效状态的哨兵。变量名 nan_ 也具有误导性。建议改为明确的整型哨兵值。
💡 修复方案
- status_component.register_output("/force_sensor/channel_1/weight", weight_ch1_, nan_);
- status_component.register_output("/force_sensor/channel_2/weight", weight_ch2_, nan_);
+ status_component.register_output("/force_sensor/channel_1/weight", weight_ch1_, invalid_weight_);
+ status_component.register_output("/force_sensor/channel_2/weight", weight_ch2_, invalid_weight_);
@@
- static constexpr double nan_ = std::numeric_limits<int>::quiet_NaN();
+ static constexpr int invalid_weight_ = std::numeric_limits<int>::min();🤖 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/device/force_sensor.hpp` around lines 17 -
18, 当前代码使用 std::numeric_limits<int>::quiet_NaN()（在定义 nan_ 的位置）并将 nan_ 作为
OutputInterface<int>（在 status_component.register_output 调用中用于
weight_ch1_/weight_ch2_ 的初始值），但对整型该调用返回 0，导致 0 被误当作无效哨兵值。将 nan_ 替换为明确的整型哨兵（例如
std::numeric_limits<int>::min() 或一个项目定义的
INVALID_WEIGHT_SENTINEL），并同时重命名变量为更明确的名称（例如 invalid_weight_ 或
sentinel_value_）；在定义（原使用 std::numeric_limits<int>::quiet_NaN() 的位置）和在
status_component.register_output("/force_sensor/channel_1/weight", weight_ch1_,
nan_) 与 "/channel_2/weight" 的注册处一并替换，确保 OutputInterface<int> 初始值为该新的哨兵值。
Comment on lines
+21
to
+25
| void store_status(std::span<const std::byte> can_data) { | ||
| ForceSensorStatus status{}; | ||
| std::memcpy(&status, can_data.data(), | ||
| std::min(can_data.size(), sizeof(ForceSensorStatus))); | ||
| force_sensor_status_.store(status, std::memory_order_relaxed); |
There was a problem hiding this comment.
store_status 不应静默接受不完整 CAN 帧
Line 21-25 当前会把短帧零填充后写入状态，这会把损坏/不完整输入当成有效数据发布。建议仅在帧长等于 sizeof(ForceSensorStatus) 时更新状态。
💡 建议修复
void store_status(std::span<const std::byte> can_data) {
+ if (can_data.size() != sizeof(ForceSensorStatus)) {
+ return;
+ }
ForceSensorStatus status{};
- std::memcpy(&status, can_data.data(),
- std::min(can_data.size(), sizeof(ForceSensorStatus)));
+ std::memcpy(&status, can_data.data(), sizeof(ForceSensorStatus));
force_sensor_status_.store(status, std::memory_order_relaxed);
}🤖 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/device/force_sensor.hpp` around lines 21 -
25, The store_status function currently accepts and zero-pads short CAN frames
which treats incomplete/ corrupted input as valid; change store_status to verify
can_data.size() == sizeof(ForceSensorStatus) before copying and updating
force_sensor_status_ (i.e., only memcpy into a local ForceSensorStatus and call
force_sensor_status_.store(...) when the size matches); if the size is
different, skip the update (or optionally log an error) so incomplete frames are
not published.
Comment on lines
+64
to
+66
| rmcs_executor::Component::initializing_component_name = component_name.c_str(); | ||
| auto component = component_loader.createSharedInstance(plugin_name); | ||
| executor->add_component(component); |
There was a problem hiding this comment.
悬垂指针：initializing_component_name 指向局部变量的内部缓冲区。
component_name 是循环内的局部 std::string，c_str() 返回的指针在循环迭代结束或 component_name 被修改后失效。虽然 createSharedInstance 可能立即使用该指针，但如果 Component 构造函数异步或延迟访问该指针，将导致未定义行为。
建议修复
for (const auto& component_description : component_descriptions) {
std::smatch matches;
- std::string plugin_name, component_name;
+ std::string plugin_name;
+ static thread_local std::string component_name; // 延长生命周期
if (std::regex_search(component_description, matches, regex)) {或者更安全的方式是让 Component 在构造时复制字符串：
// 在 Component 基类中
static std::string initializing_component_name; // 改为 std::string 而非 const char*🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/main.cpp` around lines 64
- 66, The code sets rmcs_executor::Component::initializing_component_name to
component_name.c_str(), creating a dangling pointer because component_name is a
loop-local std::string; change the implementation so the Component stores a
std::string copy instead of a const char* (make initializing_component_name a
static std::string in class Component and assign it with component_name), or
ensure the Component constructor copies the string passed to it; update
locations that set initializing_component_name (the lines using
component_name.c_str(), createSharedInstance and add_component) to assign the
std::string rather than a raw pointer.
Comment on lines
+10
to
+17
| register_output("/predefined/update_count", update_count_, static_cast<size_t>(-1)); | ||
| register_output("/predefined/timestamp", timestamp_); | ||
| } | ||
|
|
||
| void set_update_rate(double frame_rate) { *update_rate_ = frame_rate; } | ||
| void set_timestamp(std::chrono::steady_clock::time_point timestamp) { *timestamp_ = timestamp; } | ||
|
|
||
| void update() override { *update_count_ += 1; } |
There was a problem hiding this comment.
update_count_ 的初值会让首帧计数回绕。
这里把计数注册成 size_t(-1)，第一次 update() 就会从 SIZE_MAX 回到 0。如果下游把它当作真实更新次数或单调计数器，启动阶段会直接误判。
可直接应用的修正
- register_output("/predefined/update_count", update_count_, static_cast<size_t>(-1));
+ register_output("/predefined/update_count", update_count_, static_cast<size_t>(0));📝 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
| register_output("/predefined/update_count", update_count_, static_cast<size_t>(-1)); | |
| register_output("/predefined/timestamp", timestamp_); | |
| } | |
| void set_update_rate(double frame_rate) { *update_rate_ = frame_rate; } | |
| void set_timestamp(std::chrono::steady_clock::time_point timestamp) { *timestamp_ = timestamp; } | |
| void update() override { *update_count_ += 1; } | |
| register_output("/predefined/update_count", update_count_, static_cast<size_t>(0)); | |
| register_output("/predefined/timestamp", timestamp_); | |
| } | |
| void set_update_rate(double frame_rate) { *update_rate_ = frame_rate; } | |
| void set_timestamp(std::chrono::steady_clock::time_point timestamp) { *timestamp_ = timestamp; } | |
| void update() override { *update_count_ += 1; } |
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In
`@rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/predefined_msg_provider.hpp`
around lines 10 - 17, 当前把 update_count_ 注册为 size_t(-1) 会导致首次调用 update() 时从
SIZE_MAX 回绕到 0；请将注册初值改为 0（例如在 register_output 调用里把初始值从 static_cast<size_t>(-1)
改为 static_cast<size_t>(0) 或直接 0），以确保 update_count_ 在 update()（*update_count_ +=
1）开始时从 0 向上递增而不是回绕；检查 register_output("/predefined/update_count", update_count_,
...) 和成员变量 update_count_ 的类型一致性即可完成修复。
There was a problem hiding this comment.
Actionable comments posted: 7
♻️ Duplicate comments (1)
rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp (1)
28-40:⚠️ Potential issue | 🟠 Major这里的 yaw/pitch 输入契约还没接顺。
当前只注册了一个
/pitch/control/velocity输入，却在update()里把它当成[yaw, pitch]两路 setpoint 使用。这样控制器没有独立的 yaw 输入，和常见的/yaw/control/velocity、/pitch/control/velocity标量接法对不上。🛠️ 建议改成两路标量输入
- register_input("/pitch/control/velocity", yaw_pitch_vel_setpoint_); + register_input("/yaw/control/velocity", yaw_velocity_setpoint_); + register_input("/pitch/control/velocity", pitch_velocity_setpoint_); register_input("/dart/yaw_motor/velocity", yaw_velocity_); register_input("/dart/pitch_motor/velocity", pitch_velocity_); @@ - const Eigen::Vector2d& yaw_pitch_velocity = *yaw_pitch_vel_setpoint_; - *yaw_torque_ = yaw_pid_.update(yaw_pitch_velocity[0] - *yaw_velocity_); - *pitch_torque_ = pitch_pid_.update(yaw_pitch_velocity[1] - *pitch_velocity_); + *yaw_torque_ = yaw_pid_.update(*yaw_velocity_setpoint_ - *yaw_velocity_); + *pitch_torque_ = pitch_pid_.update(*pitch_velocity_setpoint_ - *pitch_velocity_); @@ - InputInterface<Eigen::Vector2d> yaw_pitch_vel_setpoint_; - InputInterface<double> yaw_velocity_; - InputInterface<double> pitch_velocity_; + InputInterface<double> yaw_velocity_setpoint_; + InputInterface<double> pitch_velocity_setpoint_; + InputInterface<double> yaw_velocity_; + InputInterface<double> pitch_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/dart/dart_setting.cpp` around lines 28 - 40, The code registers only a vector input yaw_pitch_vel_setpoint_ under "/pitch/control/velocity" but update() expects separate yaw and pitch setpoints; fix by registering two scalar inputs (e.g. register_input("/yaw/control/velocity", yaw_vel_setpoint_) and register_input("/pitch/control/velocity", pitch_vel_setpoint_)) and change update() to use *yaw_vel_setpoint_ and *pitch_vel_setpoint_ when computing yaw_pid_.update(...) and pitch_pid_.update(...), or alternatively adjust the registration to the correct two-element vector topic name if you intend to keep yaw_pitch_vel_setpoint_; ensure the symbols yaw_pitch_vel_setpoint_, yaw_vel_setpoint_, pitch_vel_setpoint_, register_input and update() are updated consistently.
🤖 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/plugins.xml`:
- Around line 137-156: 插件描述和实现不一致：更新对应 class 的 description 字段以匹配实现，具体修改为：将
rmcs_core::hardware::CatapultDartV3Lk 的 CAN ID 描述改为 0x141/0x145（与
catapult_dart_v2.cpp 一致）；将 rmcs_core::controller::dart::DartSettingController
的描述改为“输出 yaw/pitch torque，不包含 force 通道”；将
rmcs_core::controller::dart::DartLaunchSettingV2 的描述改为“注册 lifting_*
/control_velocity（控制速度），不是 angle
control”。确保使用这些类名（CatapultDartV3Lk、DartSettingController、DartLaunchSettingV2）定位并更新
plugins.xml 中对应 <class> 的 <description> 文本。
In `@rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp`:
- Around line 53-64: The lifting inputs are read but never written to the
outputs: in update() read the registered input lifting_command_ (and any
relevant field like velocity/value) and assign it to lifting_left_vel_ and
lifting_right_vel_ so the outputs stop staying at 0.0; implement a minimal
mapping in update() that fetches lifting_command_.get()/value and writes that
value (optionally clamped/scaled by belt_down_scale_ or shot_count logic if
needed) into lifting_left_vel_.set(...) and lifting_right_vel_.set(...),
ensuring the outputs are updated each cycle.
- Around line 89-99: The WAIT branch currently writes belt_hold_torque_ directly
to outputs and can exceed max_control_torque_; modify the branch in the method
containing belt_pid_ and prev_belt_cmd_ so that before assigning to
left_belt_torque_ and right_belt_torque_ you clamp the hold torque to the
configured max_control_torque_ (i.e., limit the magnitude to max_control_torque_
while preserving sign), then assign the clamped value instead of
belt_hold_torque_; keep the existing belt_pid_.reset() and the prev_belt_cmd_
check logic intact.
In `@rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v2.cpp`:
- Around line 379-384: The pitch computation using pitch = std::asin(-rot(2, 0))
can produce NaN if rot(2,0) is slightly outside [-1,1]; clamp the argument to
asin to [-1.0, 1.0] before calling it (e.g., compute a double v =
std::clamp(-rot(2,0), -1.0, 1.0) or equivalent and then pitch = std::asin(v)).
Update the code around imu_quat/toRotationMatrix and the pitch calculation
(referencing imu_quat, rot, and pitch) to perform this clamping to avoid NaNs
from floating-point error.
- Around line 119-133: referee_ring_buffer_receive_ can grow without bound
because uart1_receive_callback() unconditionally pushes bytes; add a
fixed-capacity policy: define a MAX_REFEREE_BUFFER (e.g., 4KB or configurable),
and in uart1_receive_callback() check referee_ring_buffer_receive_.size() before
pushing—if full either drop the new byte or pop_front() to make room (choose
overwrite vs drop policy) and optionally increment a dropped-counter or log
once; keep the existing referee_serial_->read lambda unchanged. Apply the same
bounded-buffer change to the other similar deque used in the second receive path
noted around the other receive callback (the block referenced in the review) so
both ring buffers are capacity-limited.
In `@rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp`:
- Around line 297-303: The pitch calculation uses std::asin(-rot(2, 0)) which
can receive values slightly outside [-1,1] due to floating-point error and
produce NaN; clamp the value passed to asin (e.g., using std::clamp or a
min/max) before calling asin to ensure the argument is in [-1.0, 1.0]; update
the code around imu_quat, rot, and the pitch computation (where roll, pitch, yaw
are computed) to compute a safe value like double v = std::clamp(-rot(2,0),
-1.0, 1.0) and then call std::asin(v).
- Around line 117-131: referee_ring_buffer_receive_ is unbounded and will grow
without any consumer; add capacity protection where bytes are enqueued (the code
that writes into referee_ring_buffer_receive_, also referenced around lines
handled by the read lambda and the other enqueue site near 248-252) by
introducing a fixed max size constant (e.g., kRefereeRingBufferMax) and, under
referee_mutex_, check before push_back: if size() >= kRefereeRingBufferMax then
either pop_front() to make room (keep newest) or drop the incoming byte (keep
oldest) — choose one policy and document it — then push_back the byte;
optionally increment a dropped-counter and emit a one-per-interval log via
processLogger or equivalent to aid debugging.
---
Duplicate comments:
In `@rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp`:
- Around line 28-40: The code registers only a vector input
yaw_pitch_vel_setpoint_ under "/pitch/control/velocity" but update() expects
separate yaw and pitch setpoints; fix by registering two scalar inputs (e.g.
register_input("/yaw/control/velocity", yaw_vel_setpoint_) and
register_input("/pitch/control/velocity", pitch_vel_setpoint_)) and change
update() to use *yaw_vel_setpoint_ and *pitch_vel_setpoint_ when computing
yaw_pid_.update(...) and pitch_pid_.update(...), or alternatively adjust the
registration to the correct two-element vector topic name if you intend to keep
yaw_pitch_vel_setpoint_; ensure the symbols yaw_pitch_vel_setpoint_,
yaw_vel_setpoint_, pitch_vel_setpoint_, register_input and update() are updated
consistently.
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: 41f751d2-7249-4b06-aa68-38d0063ba8b3
📒 Files selected for processing (9)
rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yamlrmcs_ws/src/rmcs_bringup/config/dart-guidance.yamlrmcs_ws/src/rmcs_core/plugins.xmlrmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpprmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpprmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpprmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpprmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v2.cpprmcs_ws/src/rmcs_dart_guidance
🚧 Files skipped from review as they are similar to previous changes (1)
- rmcs_ws/src/rmcs_dart_guidance
Comment on lines
+137
to
+156
| <class type="rmcs_core::hardware::CatapultDartV3Lk" base_class_type="rmcs_executor::Component"> | ||
| <description>Catapult dart hardware v3 with LK4005 lifting motors (CAN1 0x141/0x142).</description> | ||
| </class> | ||
| <class type="rmcs_core::controller::dart::DartLaunchControllerV2" base_class_type="rmcs_executor::Component"> | ||
| <description>Launch controller Phase 1: belt + trigger only, no lifting/limiting.</description> | ||
| </class> | ||
| <class type="rmcs_core::controller::dart::DartLaunchControllerV2Full" base_class_type="rmcs_executor::Component"> | ||
| <description>Launch controller Phase 2: full filling sequence with LK4005 lifting + limiting servo.</description> | ||
| </class> | ||
| <class type="rmcs_core::hardware::CatapultDartV4" base_class_type="rmcs_executor::Component"> | ||
| <description>Catapult dart hardware v4: force_screw path renamed to /dart/force_screw for DartManager compatibility.</description> | ||
| </class> | ||
| <class type="rmcs_core::controller::dart::DartLaunchSetting" base_class_type="rmcs_executor::Component"> | ||
| <description>Launch setting controller: subscribes to DartManager belt/trigger commands, outputs belt torques and trigger servo value.</description> | ||
| </class> | ||
| <class type="rmcs_core::controller::dart::DartSettingController" base_class_type="rmcs_executor::Component"> | ||
| <description>Dart setting controller: subscribes to RemoteCommandBridge delta commands, outputs yaw/pitch/force control velocities.</description> | ||
| </class> | ||
| <class type="rmcs_core::controller::dart::DartLaunchSettingV2" base_class_type="rmcs_executor::Component"> | ||
| <description>Launch setting V2: belt + trigger + LK lifting angle control.</description> |
There was a problem hiding this comment.
这几条插件描述已经和实现漂移了。
CatapultDartV3Lk 在 catapult_dart_v2.cpp 里用的是 0x141/0x145；DartSettingController 当前输出的是 yaw/pitch torque，不包含 force 通道；DartLaunchSettingV2 注册的是 lifting_* /control_velocity，不是 angle control。描述继续这样写会直接误导接线和调参。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@rmcs_ws/src/rmcs_core/plugins.xml` around lines 137 - 156, 插件描述和实现不一致：更新对应
class 的 description 字段以匹配实现，具体修改为：将 rmcs_core::hardware::CatapultDartV3Lk 的 CAN
ID 描述改为 0x141/0x145（与 catapult_dart_v2.cpp 一致）；将
rmcs_core::controller::dart::DartSettingController 的描述改为“输出 yaw/pitch torque，不包含
force 通道”；将 rmcs_core::controller::dart::DartLaunchSettingV2 的描述改为“注册 lifting_*
/control_velocity（控制速度），不是 angle
control”。确保使用这些类名（CatapultDartV3Lk、DartSettingController、DartLaunchSettingV2）定位并更新
plugins.xml 中对应 <class> 的 <description> 文本。
Comment on lines
+53
to
+64
| register_input("/dart/manager/lifting/command", lifting_command_); | ||
| // 第一发下降速度缩放因子（shot_count==1 时 0.5，其余 1.0） | ||
| register_input("/dart/manager/belt/down_scale", belt_down_scale_); | ||
|
|
||
| register_output( | ||
| "/dart/drive_belt/left/control_torque", left_belt_torque_, 0.0); | ||
| register_output( | ||
| "/dart/drive_belt/right/control_torque", right_belt_torque_, 0.0); | ||
| register_output("/dart/trigger_servo/value", trigger_value_, trigger_lock_value_); | ||
| register_output("/dart/lifting_left/control_velocity", lifting_left_vel_, 0.0); | ||
| register_output("/dart/lifting_right/control_velocity", lifting_right_vel_, 0.0); | ||
| } |
There was a problem hiding this comment.
升降命令现在完全没有落到输出上。
这里已经注册了 /dart/manager/lifting/command 和两路 lifting_*_vel_ 输出，但 update() 里没有任何分支去写它们，所以这两个输出会一直停在默认值 0.0，V2 的升降机构实际上不会动作。
🛠️ 建议补上最小映射
void update() override {
+ switch (*lifting_command_) {
+ case rmcs_msgs::DartSliderStatus::UP:
+ *lifting_left_vel_ = +lifting_velocity_;
+ *lifting_right_vel_ = -lifting_velocity_;
+ break;
+ case rmcs_msgs::DartSliderStatus::DOWN:
+ *lifting_left_vel_ = -lifting_velocity_;
+ *lifting_right_vel_ = +lifting_velocity_;
+ break;
+ case rmcs_msgs::DartSliderStatus::WAIT:
+ default:
+ *lifting_left_vel_ = 0.0;
+ *lifting_right_vel_ = 0.0;
+ break;
+ }
+
double control_velocity = 0.0;
switch (*belt_command_) {Also applies to: 66-85
🤖 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/dart/launch_setting_v2.cpp` around lines
53 - 64, The lifting inputs are read but never written to the outputs: in
update() read the registered input lifting_command_ (and any relevant field like
velocity/value) and assign it to lifting_left_vel_ and lifting_right_vel_ so the
outputs stop staying at 0.0; implement a minimal mapping in update() that
fetches lifting_command_.get()/value and writes that value (optionally
clamped/scaled by belt_down_scale_ or shot_count logic if needed) into
lifting_left_vel_.set(...) and lifting_right_vel_.set(...), ensuring the outputs
are updated each cycle.
Comment on lines
+89
to
+99
| // WAIT 状态：清除 PID 积分；若上一个方向是 DOWN，施加保持扭矩防止滑落 | ||
| if (set_velocity == 0.0) { | ||
| belt_pid_.reset(); | ||
| if (prev_belt_cmd_ == rmcs_msgs::DartSliderStatus::DOWN) { | ||
| *left_belt_torque_ = +belt_hold_torque_; | ||
| *right_belt_torque_ = +belt_hold_torque_; | ||
| } else { | ||
| *left_belt_torque_ = 0.0; | ||
| *right_belt_torque_ = 0.0; | ||
| } | ||
| return; |
There was a problem hiding this comment.
WAIT 分支绕过了 max_control_torque_ 限幅。
PID 路径会 clamp，但这里直接把 belt_hold_torque_ 写进输出。只要参数配得比 max_control_torque_ 大，就会把超限扭矩直接下发到电机。
🛠️ 建议在保持扭矩路径上也做限幅
if (set_velocity == 0.0) {
belt_pid_.reset();
if (prev_belt_cmd_ == rmcs_msgs::DartSliderStatus::DOWN) {
- *left_belt_torque_ = +belt_hold_torque_;
- *right_belt_torque_ = +belt_hold_torque_;
+ const double hold_torque =
+ std::clamp(belt_hold_torque_, -max_control_torque_, max_control_torque_);
+ *left_belt_torque_ = hold_torque;
+ *right_belt_torque_ = hold_torque;
} else {
*left_belt_torque_ = 0.0;
*right_belt_torque_ = 0.0;🤖 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/dart/launch_setting_v2.cpp` around lines
89 - 99, The WAIT branch currently writes belt_hold_torque_ directly to outputs
and can exceed max_control_torque_; modify the branch in the method containing
belt_pid_ and prev_belt_cmd_ so that before assigning to left_belt_torque_ and
right_belt_torque_ you clamp the hold torque to the configured
max_control_torque_ (i.e., limit the magnitude to max_control_torque_ while
preserving sign), then assign the clamped value instead of belt_hold_torque_;
keep the existing belt_pid_.reset() and the prev_belt_cmd_ check logic intact.
Comment on lines
+119
to
+133
| register_output("/referee/serial", referee_serial_); | ||
| referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { | ||
| std::lock_guard lock(referee_mutex_); | ||
| size_t count = 0; | ||
| while (count < size && !referee_ring_buffer_receive_.empty()) { | ||
| buffer[count++] = referee_ring_buffer_receive_.front(); | ||
| referee_ring_buffer_receive_.pop_front(); | ||
| } | ||
| return count; | ||
| }; | ||
| referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { | ||
| auto board = start_transmit(); | ||
| board.uart1_transmit({.uart_data = std::span{buffer, size}}); | ||
| return size; | ||
| }; |
There was a problem hiding this comment.
这里的 referee 缓存同样需要限长。
uart1_receive_callback() 只负责把字节压进 referee_ring_buffer_receive_，但当前 v2 guidance 配置也没有挂任何 /referee/serial 消费者。这个 deque 会一直涨，跑久了就是稳定的内存泄漏。
🛠️ 最小修复示例
void uart1_receive_callback(const librmcs::data::UartDataView& data) override {
std::lock_guard lock(referee_mutex_);
for (auto byte : data.uart_data) {
+ if (referee_ring_buffer_receive_.size() >= 4096) {
+ referee_ring_buffer_receive_.pop_front();
+ }
referee_ring_buffer_receive_.push_back(byte);
}
}Also applies to: 266-270
🤖 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/catapult_dart_v2.cpp` around lines 119 -
133, referee_ring_buffer_receive_ can grow without bound because
uart1_receive_callback() unconditionally pushes bytes; add a fixed-capacity
policy: define a MAX_REFEREE_BUFFER (e.g., 4KB or configurable), and in
uart1_receive_callback() check referee_ring_buffer_receive_.size() before
pushing—if full either drop the new byte or pop_front() to make room (choose
overwrite vs drop policy) and optionally increment a dropped-counter or log
once; keep the existing referee_serial_->read lambda unchanged. Apply the same
bounded-buffer change to the other similar deque used in the second receive path
noted around the other receive callback (the block referenced in the review) so
both ring buffers are capacity-limited.
Comment on lines
+379
to
+384
| Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); | ||
| Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); | ||
|
|
||
| double roll = std::atan2(rot(2, 1), rot(2, 2)); | ||
| double pitch = std::asin(-rot(2, 0)); | ||
| double yaw = std::atan2(rot(1, 0), rot(0, 0)); |
There was a problem hiding this comment.
这里也建议夹紧 asin 输入。
rot(2, 0) 只要因为浮点误差越界一点点，pitch 就会直接变成 NaN，TF 和角度输出也会一起坏掉。
🤖 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/catapult_dart_v2.cpp` around lines 379 -
384, The pitch computation using pitch = std::asin(-rot(2, 0)) can produce NaN
if rot(2,0) is slightly outside [-1,1]; clamp the argument to asin to [-1.0,
1.0] before calling it (e.g., compute a double v = std::clamp(-rot(2,0), -1.0,
1.0) or equivalent and then pitch = std::asin(v)). Update the code around
imu_quat/toRotationMatrix and the pitch calculation (referencing imu_quat, rot,
and pitch) to perform this clamping to avoid NaNs from floating-point error.
Comment on lines
+117
to
+131
| register_output("/referee/serial", referee_serial_); | ||
| referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { | ||
| std::lock_guard lock(referee_mutex_); | ||
| size_t count = 0; | ||
| while (count < size && !referee_ring_buffer_receive_.empty()) { | ||
| buffer[count++] = referee_ring_buffer_receive_.front(); | ||
| referee_ring_buffer_receive_.pop_front(); | ||
| } | ||
| return count; | ||
| }; | ||
| referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { | ||
| auto board = start_transmit(); | ||
| board.uart1_transmit({.uart_data = std::span{buffer, size}}); | ||
| return size; | ||
| }; |
There was a problem hiding this comment.
给 referee 接收队列加上容量保护。
这里的 UART1 数据只入队、不封顶，而当前这套 dart guidance 配置里又没有任何 /referee/serial 消费者，referee_ring_buffer_receive_ 会持续增长，长时间运行会把内存慢慢吃满。
🛠️ 最小修复示例
void uart1_receive_callback(const librmcs::data::UartDataView& data) override {
std::lock_guard lock(referee_mutex_);
for (auto byte : data.uart_data) {
+ if (referee_ring_buffer_receive_.size() >= 4096) {
+ referee_ring_buffer_receive_.pop_front();
+ }
referee_ring_buffer_receive_.push_back(byte);
}
}Also applies to: 248-252
🤖 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/catapult_dart.cpp` around lines 117 - 131,
referee_ring_buffer_receive_ is unbounded and will grow without any consumer;
add capacity protection where bytes are enqueued (the code that writes into
referee_ring_buffer_receive_, also referenced around lines handled by the read
lambda and the other enqueue site near 248-252) by introducing a fixed max size
constant (e.g., kRefereeRingBufferMax) and, under referee_mutex_, check before
push_back: if size() >= kRefereeRingBufferMax then either pop_front() to make
room (keep newest) or drop the incoming byte (keep oldest) — choose one policy
and document it — then push_back the byte; optionally increment a
dropped-counter and emit a one-per-interval log via processLogger or equivalent
to aid debugging.
Comment on lines
+297
to
+303
| Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); | ||
| Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); | ||
|
|
||
| double roll = std::atan2(rot(2, 1), rot(2, 2)); | ||
| double pitch = std::asin(-rot(2, 0)); | ||
| double yaw = std::atan2(rot(1, 0), rot(0, 0)); | ||
|
|
There was a problem hiding this comment.
先夹紧 asin 的输入，避免姿态偶发变成 NaN。
四元数转矩阵后会有浮点误差，-rot(2, 0) 偶尔会略微越出 [-1, 1]。这里一旦变成 NaN，后面的 TF 和角度输出都会被污染。
🤖 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/catapult_dart.cpp` around lines 297 - 303,
The pitch calculation uses std::asin(-rot(2, 0)) which can receive values
slightly outside [-1,1] due to floating-point error and produce NaN; clamp the
value passed to asin (e.g., using std::clamp or a min/max) before calling asin
to ensure the argument is in [-1.0, 1.0]; update the code around imu_quat, rot,
and the pitch computation (where roll, pitch, yaw are computed) to compute a
safe value like double v = std::clamp(-rot(2,0), -1.0, 1.0) and then call
std::asin(v).
There was a problem hiding this comment.
Actionable comments posted: 3
♻️ Duplicate comments (4)
rmcs_ws/src/rmcs_core/plugins.xml (2)
155-157:⚠️ Potential issue | 🟡 Minor
DartLaunchSettingV2描述写成角度控制，和实现不符。
rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp输出的是lifting_* /control_velocity，不是 angle control。请修正描述。🛠️ 建议修复
- <class type="rmcs_core::controller::dart::DartLaunchSettingV2" base_class_type="rmcs_executor::Component"> - <description>Launch setting V2: belt + trigger + LK lifting angle control.</description> + <class type="rmcs_core::controller::dart::DartLaunchSettingV2" base_class_type="rmcs_executor::Component"> + <description>Launch setting V2: belt + trigger + LK lifting velocity control.</description> </class>🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_core/plugins.xml` around lines 155 - 157, Update the plugin description for class DartLaunchSettingV2 to reflect that it performs lifting velocity control rather than angle control: change the <description> text in the plugins.xml entry for type="rmcs_core::controller::dart::DartLaunchSettingV2" to mention "lifting velocity/control_velocity" (or "lifting_* control_velocity") or similar wording consistent with the implementation in rmcs_core/src/controller/dart/launch_setting.cpp, which outputs lifting_* /control_velocity, not angle control.
137-139:⚠️ Potential issue | 🟡 Minor
CatapultDartV3Lk的 CAN ID 描述与实现不一致。这里写的是
0x141/0x142，但rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp（Line 312-313）定义为0x141/0x145。请统一。🛠️ 建议修复
- <class type="rmcs_core::hardware::CatapultDartV3Lk" base_class_type="rmcs_executor::Component"> - <description>Catapult dart hardware v3 with LK4005 lifting motors (CAN1 0x141/0x142).</description> + <class type="rmcs_core::hardware::CatapultDartV3Lk" base_class_type="rmcs_executor::Component"> + <description>Catapult dart hardware v3 with LK4005 lifting motors (CAN1 0x141/0x145).</description> </class>🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@rmcs_ws/src/rmcs_core/plugins.xml` around lines 137 - 139, The XML description for class CatapultDartV3Lk lists CAN IDs as 0x141/0x142 but the implementation in the CatapultDart (catapult_dart.cpp) defines the lifting-motor CAN IDs as 0x141/0x145; update the plugins.xml <class type="rmcs_core::hardware::CatapultDartV3Lk"> description to match the implementation (use 0x141/0x145) or change the implementation to use 0x142—ensure the class name CatapultDartV3Lk and the CAN ID constants in catapult_dart.cpp are consistent.rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp (2)
266-270:⚠️ Potential issue | 🟠 MajorReferee 接收队列无上限，长期运行有内存增长风险。
referee_ring_buffer_receive_只入队不封顶；在没有及时消费时会持续增长。这个问题与之前评论一致，仍建议尽快加容量保护。🛠️ 最小修复示例
void uart1_receive_callback(const librmcs::data::UartDataView& data) override { std::lock_guard lock(referee_mutex_); for (auto byte : data.uart_data) { + if (referee_ring_buffer_receive_.size() >= 4096) { + referee_ring_buffer_receive_.pop_front(); // 保留最新数据 + } referee_ring_buffer_receive_.push_back(byte); } }🤖 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/catapult_dart.cpp` around lines 266 - 270, uart1_receive_callback is unboundedly appending into referee_ring_buffer_receive_ under referee_mutex_, causing unbounded memory growth; introduce a fixed-capacity policy (e.g., const size_t MAX_REFEREE_BUFFER_SIZE) and enforce it inside uart1_receive_callback before pushing bytes: either drop oldest entries by popping from referee_ring_buffer_receive_ until size() < MAX_REFEREE_BUFFER_SIZE or skip appending new bytes when full. Update the code around uart1_receive_callback/referee_ring_buffer_receive_ to use that MAX, keep locking via referee_mutex_, and ensure the chosen policy (drop-oldest vs drop-new) is applied consistently wherever referee_ring_buffer_receive_ is written/read.
383-384:⚠️ Potential issue | 🟡 Minor
asin输入未夹紧，姿态角存在 NaN 风险。浮点误差可能让
-rot(2, 0)轻微超出[-1, 1]。该问题与之前评论一致，建议在调用asin前先做clamp。🛠️ 建议修复
- double pitch = std::asin(-rot(2, 0)); + const double pitch_arg = std::clamp(-rot(2, 0), -1.0, 1.0); + double pitch = std::asin(pitch_arg);🤖 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/catapult_dart.cpp` around lines 383 - 384, The asin call can receive values slightly outside [-1,1] due to FP error causing NaN for pitch; clamp the input to std::asin by bounding -rot(2,0) into [-1.0, 1.0] (e.g., with std::clamp or a manual min/max) before computing pitch, and leave the yaw computation unchanged; update the code around the pitch/yaw calculation in catapult_dart.cpp where pitch is computed to use the clamped value.
🧹 Nitpick comments (1)
rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp (1)
118-123: 高频控制回路中使用 INFO 日志会造成日志洪泛。这段在 WAIT 态会持续触发，建议改为节流日志或降级到 DEBUG，避免影响实时性与排障信噪比。
🤖 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/dart/launch_setting.cpp` around lines 118 - 123, 日志在高频控制回路（出现 "belt_combating" / "belt_init done" 分支）使用了 RCLCPP_INFO，会造成日志洪泛；请将这两条信息改为更低级别或节流输出，例如使用 RCLCPP_DEBUG(logger_, ...) 或添加节流机制（仅在状态切换时或每 N 次/每隔 T ms 打印），定位修改点为产生消息的分支（涉及 logger_、left_belt_torque_、right_belt_torque_ 所在函数/方法）并确保在 WAIT 态不会频繁打印。
🤖 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/plugins.xml`:
- Around line 158-163: The descriptions for
rmcs_core::controller::dart::DartFilling and
rmcs_core::hardware::CatapultDartFillingTest are incorrect copies of another
class; update the <description> text for each class in plugins.xml to reflect
their actual responsibilities (e.g., DartFilling: controller logic for
filling/feeding darts and managing timing/actuation; CatapultDartFillingTest:
hardware test harness for catapult dart filling including diagnostics and safety
checks), replacing the generic "Launch setting V2..." text so operators see
accurate intent for each component.
In `@rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp`:
- Around line 113-125: The WAIT branch currently assigns belt_hold_torque_
directly and returns, bypassing the torque limiting logic; modify the branch in
the set_velocity == 0.0 block (around belt_pid_.reset() / prev_belt_cmd_) so
that when setting *left_belt_torque_ and *right_belt_torque_ you first clamp
belt_hold_torque_ with std::clamp(..., -torque_limit, torque_limit) (or the
existing torque_limit variable/mechanism used elsewhere) before assignment, then
continue to return as before; ensure you reference belt_hold_torque_,
torque_limit, prev_belt_cmd_, left_belt_torque_ and right_belt_torque_ so the
homing 10% limit is enforced.
In `@rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp`:
- Around line 38-40: The header comment for CatapultDartV3Lk states the right
lifting motor CAN ID is 0x142 but the implementation constant
LK_LIFTING_RIGHT_ID is 0x145; make these consistent by updating the
header/comment occurrences (including the other comment instances around the
file mentioned) to match the actual constant LK_LIFTING_RIGHT_ID (0x145), or if
the intended ID is 0x142, change the constant LK_LIFTING_RIGHT_ID to
0x142—ensure the header comment text and any inline comments reference the same
hex value as the LK_LIFTING_RIGHT_ID symbol.
---
Duplicate comments:
In `@rmcs_ws/src/rmcs_core/plugins.xml`:
- Around line 155-157: Update the plugin description for class
DartLaunchSettingV2 to reflect that it performs lifting velocity control rather
than angle control: change the <description> text in the plugins.xml entry for
type="rmcs_core::controller::dart::DartLaunchSettingV2" to mention "lifting
velocity/control_velocity" (or "lifting_* control_velocity") or similar wording
consistent with the implementation in
rmcs_core/src/controller/dart/launch_setting.cpp, which outputs lifting_*
/control_velocity, not angle control.
- Around line 137-139: The XML description for class CatapultDartV3Lk lists CAN
IDs as 0x141/0x142 but the implementation in the CatapultDart
(catapult_dart.cpp) defines the lifting-motor CAN IDs as 0x141/0x145; update the
plugins.xml <class type="rmcs_core::hardware::CatapultDartV3Lk"> description to
match the implementation (use 0x141/0x145) or change the implementation to use
0x142—ensure the class name CatapultDartV3Lk and the CAN ID constants in
catapult_dart.cpp are consistent.
In `@rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp`:
- Around line 266-270: uart1_receive_callback is unboundedly appending into
referee_ring_buffer_receive_ under referee_mutex_, causing unbounded memory
growth; introduce a fixed-capacity policy (e.g., const size_t
MAX_REFEREE_BUFFER_SIZE) and enforce it inside uart1_receive_callback before
pushing bytes: either drop oldest entries by popping from
referee_ring_buffer_receive_ until size() < MAX_REFEREE_BUFFER_SIZE or skip
appending new bytes when full. Update the code around
uart1_receive_callback/referee_ring_buffer_receive_ to use that MAX, keep
locking via referee_mutex_, and ensure the chosen policy (drop-oldest vs
drop-new) is applied consistently wherever referee_ring_buffer_receive_ is
written/read.
- Around line 383-384: The asin call can receive values slightly outside [-1,1]
due to FP error causing NaN for pitch; clamp the input to std::asin by bounding
-rot(2,0) into [-1.0, 1.0] (e.g., with std::clamp or a manual min/max) before
computing pitch, and leave the yaw computation unchanged; update the code around
the pitch/yaw calculation in catapult_dart.cpp where pitch is computed to use
the clamped value.
---
Nitpick comments:
In `@rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp`:
- Around line 118-123: 日志在高频控制回路（出现 "belt_combating" / "belt_init done" 分支）使用了
RCLCPP_INFO，会造成日志洪泛；请将这两条信息改为更低级别或节流输出，例如使用 RCLCPP_DEBUG(logger_, ...)
或添加节流机制（仅在状态切换时或每 N 次/每隔 T ms 打印），定位修改点为产生消息的分支（涉及
logger_、left_belt_torque_、right_belt_torque_ 所在函数/方法）并确保在 WAIT 态不会频繁打印。
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: 4017b7ef-a70f-42ae-9d1b-ad8657a65533
📒 Files selected for processing (6)
rmcs_ws/src/rmcs_bringup/config/dart-guidance.yamlrmcs_ws/src/rmcs_core/librmcsrmcs_ws/src/rmcs_core/plugins.xmlrmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpprmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpprmcs_ws/src/rmcs_dart_guidance
💤 Files with no reviewable changes (1)
- rmcs_ws/src/rmcs_core/librmcs
🚧 Files skipped from review as they are similar to previous changes (2)
- rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml
- rmcs_ws/src/rmcs_dart_guidance
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
3 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
deleted package "rmcs_dart_guidance" and add it as submodule