Full with position control#8
Open
hyperle wants to merge 19 commits into
Open
Conversation
- Added belt angle input interfaces for position feedback - Added belt_zero_calibration output for zero point calibration - Added belt_down_distance and belt_pulley_radius parameters - Added BeltConstantVelocityMoveAction for position-based control - Added ZeroCalibrationAction for zero point recording - Updated LaunchPreparationTask to use position control instead of stall detection - Updated CancelLaunchTask with position control - Updated SliderInitTask to trigger zero calibration after homing Key features preserved: - Downward movement uses position feedback to determine arrival - Distance (m) to angle (rad) conversion using pulley radius - Zero point calibration on each upward stall (mechanical limit)
- Added fire_count increment after fire task completion - Added fire_count reset after cancel_launch task completion - Restored belt_velocity_ramp_action.hpp (BeltModerateAction) - Added logging for fire_count changes Task flow now works correctly: 1. slider_init: calibrates zero point 2. launch_prepare: uses fire_count to select velocity (first vs subsequent) 3. fire: increments fire_count 4. cancel_launch: resets fire_count to 0 This ensures proper position control behavior across multiple launch cycles.
BeltMoveAction changes: - Added timeout_returns_success parameter (default: false) - Updated update() to use this parameter for timeout handling - Added member variable declaration Task updates: - silder_init_task: Already correct (timeout_returns_success=true) - launch_preparation_task: Added timeout_returns_success=false to belt_reset_up - cancel_launch_task: Added timeout_returns_success=false to belt_reset This ensures all BeltMoveAction calls have the correct number of parameters and proper timeout behavior.
Walkthrough本次变更对飞镖引导系统进行了全面重构，引入自动瞄准动作、新型传送带常速控制动作、力传感器标定动作等多个新动作类，重构了任务框架和管理器核心逻辑，添加了视频录制功能，并扩展了测试覆盖。插件注册表更新，配置依赖项调整。 Changes
Sequence Diagram(s)sequenceDiagram
participant Manager as DartManager
participant VisionTask as VisionAssistedLaunchPreparationTask
participant AutoAim as AutoAimAction
participant LaunchPrep as LaunchPreparationTask
participant BeltControl as BeltConstantVelocityMoveAction
participant ForceCal as ForceScrewCalibrationAction
participant Hardware as 硬件控制
Manager->>VisionTask: 执行视觉辅助发射准备
VisionTask->>AutoAim: tick() - 自动瞄准
activate AutoAim
AutoAim->>Hardware: 读取目标位置/跟踪状态
Hardware-->>AutoAim: 返回像素误差
alt 误差在可接受范围
AutoAim-->>VisionTask: SUCCESS
else 误差超范围
AutoAim-->>VisionTask: RUNNING<br/>设置控制速度
end
deactivate AutoAim
VisionTask->>LaunchPrep: then() - 发射准备
activate LaunchPrep
LaunchPrep->>BeltControl: 执行传送带下行
activate BeltControl
BeltControl->>Hardware: 恒速控制传送带
Hardware-->>BeltControl: 返回角度/速度反馈
alt 位置误差<阈值
BeltControl-->>LaunchPrep: SUCCESS
end
deactivate BeltControl
LaunchPrep->>ForceCal: 执行力标定
activate ForceCal
ForceCal->>Hardware: 读取力传感器
Hardware-->>ForceCal: 返回力值
alt 力误差<容差持续确认
ForceCal-->>LaunchPrep: SUCCESS
end
deactivate ForceCal
LaunchPrep-->>VisionTask: SUCCESS
deactivate LaunchPrep
VisionTask-->>Manager: 任务完成
sequenceDiagram
participant User as 用户/手动控制
participant ManualForce as DartManualForceControlAction
participant Feedback as 力螺杆反馈
participant Motor as 力螺杆电机
User->>ManualForce: 摇杆输入
ManualForce->>ManualForce: 计算力控速度<br/>= 摇杆×scale
ManualForce->>Motor: 设置目标速度
alt 摇杆命令≈0
ManualForce->>ManualForce: 清空停滞计数器
else 持续非零命令>100刻度
ManualForce->>Feedback: 读取速度/扭矩反馈
Feedback-->>ManualForce: 返回反馈值
alt 速度/扭矩检测到停滞
ManualForce->>ManualForce: 递增停滞计数
else 无停滞
ManualForce->>ManualForce: 重置停滞计数
end
alt 停滞计数≥100
ManualForce-->>User: SUCCESS<br/>任务完成
else 计数<100
ManualForce-->>User: RUNNING
end
end
Estimated code review effort🎯 4 (Complex) | ⏱️ ~60 minutes Possibly related PRs
Poem
🚥 Pre-merge checks | ✅ 3 | ❌ 2❌ Failed checks (1 warning, 1 inconclusive)
✅ Passed checks (3 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
⚔️ Resolve merge conflicts
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 |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 10
Note
Due to the large number of review comments, Critical, Major severity comments were prioritized as inline comments.
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (4)
src/manager/action/filling_limit_servo_action.hpp (1)
9-9:⚠️ Potential issue | 🟡 Minor注释与新签名不一致。
Line 9 的中文注释仍写“等待
fill_ticks，再下发LOCK”，但fill_ticks参数已被移除。建议同步更新为“等待固定 200 ticks”或说明实际时序，避免误导后续读者。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/action/filling_limit_servo_action.hpp` at line 9, The file-level comment for the FillingLimitServoAction is out of sync with the new API: it still mentions waiting for `fill_ticks` which no longer exists; update the comment above the FillingLimitServoAction (or the function/constructor with the new signature) to state that after sending the FREE semantic command the action waits a fixed 200 ticks (or the precise timing used) before sending LOCK, and remove any mention of the removed `fill_ticks` parameter; ensure the comment text around the class/constructor/method name FillingLimitServoAction reflects the actual timing and is consistent with the current signature so future readers are not misled.src/manager/task/fire_and_preload_task.hpp (1)
15-71:⚠️ Potential issue | 🟠 Major类注释与实现严重不一致，声称的三个步骤中仅实现其一，且传入的参数被完全忽略。
- 类注释声称执行 "1. 解锁扳机；2. 填装升降上行；3. 限位舵机执行预装填动作"，但实现中仅执行第 3 步，前两步整体被注释掉
- 构造参数
trigger_lock_enable、lifting_command、lifting_left_vel_fb、lifting_right_vel_fb在当前实现中完全未被使用，仅在注释代码中出现is_first_shot参数已传递（在dart_manager.cpp中调用时明确设置is_first_shot = (fire_count_ == 0)），但对应的条件分支完全被注释，参数形同虚设- 调用方在
dart_manager.cpp传递这些参数时实际上其参数会被静默忽略，导致行为与预期不符需明确化处理：
- 若确实要简化为仅限位舵机操作：删除未使用的参数，修改类注释改为 "仅执行限位舵机预装填动作"，清理注释掉的死代码
- 若计划保留三步逻辑：将注释掉的
if (is_first_shot)分支完整解注，确保传入的参数被正确使用否则 PR 合入后会导致行为变更不可见，调用方在传递参数时无法意识到其参数被忽略。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/task/fire_and_preload_task.hpp` around lines 15 - 71, The FireAndPreloadTask implementation currently only enqueues a FillingLimitServoAction while the constructor parameters (trigger_lock_enable, lifting_command, lifting_left_vel_fb, lifting_right_vel_fb, is_first_shot) are ignored and the intended if (is_first_shot) / else branch is commented out; either (A) remove the unused parameters from FireAndPreloadTask, delete the commented dead code, and update the class comment to state it only performs the limiting-servo preload (adjust Task name/description if needed), or (B) restore and uncomment the original logic in the FireAndPreloadTask constructor: re-enable the TriggerControlAction and FillingLiftAction sequences (including the is_first_shot conditional that only unlocks the trigger for the first shot), ensure lifting_command and velocity feedbacks (lifting_left_vel_fb, lifting_right_vel_fb) are passed into FillingLiftAction, and keep the final FillingLimitServoAction; pick one approach and make the constructor, comment, and callers consistent with that choice.src/vision/dart_vision_core.cpp (2)
255-268:⚠️ Potential issue | 🔴 Critical识别成功分支中
*target_position_未更新即用于计算diff，导致输出陈旧/无效像素差在
if (!identifier_.result_status_())的 else 分支里，initial_position已通过identifier_.get_result()拿到，但并未赋值给*target_position_；随后第 265 行cv::Point2i diff = *target_position_ - image_center_;使用的仍是上一次状态（很可能是(-1, -1)），导致*yaw_pitch_distance_输出与真实首帧目标位置不符。下游auto_aim等动作消费的是该yaw_pitch_target_distance，会基于错误的像素误差驱动云台。🐛 建议的修正
is_tracker_stage_ = true; tracker_.Init(initial_position); // Output relative position (target - center) - cv::Point2i diff = *target_position_ - image_center_; + *target_position_ = initial_position; + cv::Point2i diff = initial_position - image_center_; *yaw_pitch_distance_ = Eigen::Vector2d(diff.x, diff.y); *tracking_ = true;🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/vision/dart_vision_core.cpp` around lines 255 - 268, The else branch after identifier_.result_status_() uses *target_position_ to compute diff but never updates it; assign *target_position_ = initial_position (where initial_position = identifier_.get_result()) before computing diff so the immediate pixel offset is correct, then proceed with tracker_.Init(initial_position), compute cv::Point2i diff = *target_position_ - image_center_, set *yaw_pitch_distance_ and *tracking_ as before; this ensures yaw_pitch_distance_ reflects the newly detected initial position rather than a stale value.
269-296:⚠️ Potential issue | 🔴 Critical跟踪分支未同步更新
yaw_pitch_distance_，下游消费者将持续读取过期值与 261-267 行识别→跟踪切换时计算
diff的逻辑相比，跟踪稳定后的成功分支（289-294 行）只更新了*target_position_与*tracking_，没有刷新*yaw_pitch_distance_。后果是auto_aim_action等消费/dart_guidance/tracker/yaw_pitch_target_distance的下游始终拿到首帧的差值，无法跟随目标移动。跟踪丢失分支（278-288 行）建议同时把
*yaw_pitch_distance_复位（如Eigen::Vector2d::Zero()），以避免目标丢失后输出仍残留旧误差。🐛 建议的修正
if (!is_tracking) { tracker_loss_count_++; *target_position_ = PointT(-1, -1); *tracking_ = false; + *yaw_pitch_distance_ = Eigen::Vector2d::Zero(); if (tracker_loss_count_ > 100) { ... } } else { tracker_loss_count_ = 0; *target_position_ = current_position; *tracking_ = true; + cv::Point2i diff = current_position - image_center_; + *yaw_pitch_distance_ = Eigen::Vector2d(diff.x, diff.y); cv::circle(display_image, current_position, 20, cv::Scalar(255, 0, 255), 2); }🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/vision/dart_vision_core.cpp` around lines 269 - 296, When tracking is successful or lost you must update the shared yaw_pitch_distance_ the same way the identification->tracking switch does: in the tracking-success branch (where tracker_.get_current_position() is used and *target_position_/*tracking_ are set) recompute and store *yaw_pitch_distance_ using the same computation/utility used in the identification→tracking transition (reuse the same function/logic that produced the initial diff) so downstream consumers see up-to-date error; in the tracking-loss branch also reset *yaw_pitch_distance_ to Eigen::Vector2d::Zero() (in addition to setting *target_position_ and *tracking_ and incrementing tracker_loss_count_) to avoid stale error values.
🟡 Minor comments (13)
test/force_sensor_recorder.cpp-66-82 (1)
66-82:⚠️ Potential issue | 🟡 Minor
initialize_output_file每次启动都std::ios::trunc，会丢失历史记录。每次组件构造（节点重启 / 重配置）都会无条件清空
output_file_path_指向的文件，这在测试调试场景下很容易把上一轮采集的力传感器数据全部清零。建议至少满足下列之一：
- 用带时间戳/轮次的文件名（如
force_sensor_record_2025-04-26T10-02.md）；- 仅在文件不存在时才写入表头，否则
std::ios::app续写；- 或在覆盖前把旧文件改名备份。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@test/force_sensor_recorder.cpp` around lines 66 - 82, The initialize_output_file function currently opens output_file_path_ with std::ios::trunc which always erases prior data; change it so existing data isn't lost by checking for the file existence first: if the file exists, open with std::ios::app and only write the header when creating a new file, otherwise create a new timestamped/round-specific filename or rename the old file as a backup before truncating; update references to output_file_path_, the local std::ofstream creation logic in initialize_output_file, and the header-write block to run only on new-file creation.test/force_sensor_recorder.cpp-95-100 (1)
95-100:⚠️ Potential issue | 🟡 Minor记录上升沿但未做去抖：单条
fire_trigger异常脉冲会导致总发射次数错位。当前实现仅以
current_trigger && !last_trigger_state_作为上升沿判定，没有最小持续帧数 / 时间戳间隔限制。如果fire_trigger_因任何原因出现“一帧 true 后又落下”的毛刺脉冲（重置 / 节点重启 / 上游 publisher 重连等），都会让total_shot_count_多走一格，进而把后续轮次（(total_shot_count_ - 1) % 4 + 1）整体偏移，给后期数据分析带来对齐困难。建议加入最小高电平确认帧数（例如 ≥ 2 ticks）或与 DartManager 一侧的发射事件对齐（带 sequence id 的消息）。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@test/force_sensor_recorder.cpp` around lines 95 - 100, The code currently treats any current_trigger && !last_trigger_state_ as an edge and increments total_shot_count_, which allows single-frame glitches to miscount; modify the edge-detection in the function handling fire_trigger_ so an up-edge is only accepted after confirming a minimum high duration (e.g. require current_trigger to be true for >=2 consecutive ticks or check timestamp delta >= configured debounce_ms) before incrementing total_shot_count_ and computing round_number = (total_shot_count_ - 1) % 4 + 1; update/maintain a small counter or last_high_timestamp variable alongside last_trigger_state_ to implement the debounce, and ensure async_write_to_file(total_shot_count_, round_number, ...) is only called after the debounced edge is confirmed.src/manager/action/action_sequence.hpp-49-77 (1)
49-77:⚠️ Potential issue | 🟡 Minor确认
update()内确实可连续推进多个动作，建议明确文档或调整实现。代码确实允许在单次
update()调用中连续执行多个动作：当一个动作返回SUCCESS时（第61-65行），代码调用continue直接进入下一次循环迭代，可能立即对下一个非空动作调用tick_first()/tick()。这与现有注释（第16行："当前动作返回 SUCCESS 后自动推进到下一个动作"）的措辞模糊——未明确指出是否在同一帧内推进。带来的风险包括：
- 新进入的动作在
tick_first()可能读到陈旧的输入状态（上游传感器/控制量尚未在本帧刷新）- 可观察的状态机推进速度高于预期
- 单帧执行耗时可能上升
建议：
- 若设计意图是"一次 update 至多推进一帧"：将第65行的
continue改为return ActionStatus::RUNNING（或仅在跳过nullptr时continue）- 若意图就是同帧推进多个动作：在类注释中显式文档化这一语义，例如"可在单次
update()内连续推进，直到某动作返回RUNNING或FAILURE"，避免后续维护误解🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/action/action_sequence.hpp` around lines 49 - 77, The update() loop currently advances immediately to the next action when the current action returns SUCCESS (using continue), allowing multiple actions to be progressed in a single update; to limit progress to at most one actionable step per update, change the SUCCESS branch in update() (the block handling ActionStatus::SUCCESS around current.on_exit(), ++cursor_, first_tick_of_current_ = true) to return ActionStatus::RUNNING instead of continue (keep the continue for the nullptr skip), so the method advances the cursor but yields RUNNING to prevent ticking the next action in the same call; update the class comment to state this "one-action-per-update" semantic and reference cursor_, actions_, first_tick_of_current_, and update().src/manager/task/silder_init_task.hpp-27-65 (1)
27-65:⚠️ Potential issue | 🟡 Minor视觉校准参数当前为死代码，建议明确处理。
yaw_pitch_angle、yaw_pitch_distance、yaw_angle_mapping_rate已纳入构造函数签名，但唯一的使用点（第 61-65 行）整段被注释。这会带来三个问题：
- 未使用形参可能触发
-Wunused-parameter告警；- 调用方（如
dart_manager.cpp）会以为视觉校准已生效，实际并未启用；vision_yaw_calibration.hpp当前仅因为这段注释而 include，等价于隐式依赖。如果暂未准备启用，建议先把视觉相关参数从构造函数移除，待
VisionYawCalibration调用真正落地时再恢复；或者保留参数但取消注释、用if (yaw_pitch_angle && yaw_pitch_distance)守卫调用。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/task/silder_init_task.hpp` around lines 27 - 65, The constructor currently accepts yaw_pitch_angle, yaw_pitch_distance, and yaw_angle_mapping_rate but the VisionYawCalibration call is commented out causing unused-parameter warnings and misleading API behavior; either remove these three parameters from the constructor signature (and related includes) if vision calibration is not supported, or re-enable the guarded calibration call by uncommenting and adding the runtime guard if (yaw_pitch_angle && yaw_pitch_distance) then(std::make_shared<VisionYawCalibration>(*yaw_pitch_angle, *yaw_pitch_distance, yaw_angle_mapping_rate)); so that VisionYawCalibration is invoked only when both pointers are provided; update any callers (e.g., where this Task is constructed) accordingly to match the chosen approach.src/manager/dart_launch_sequence.hpp-57-67 (1)
57-67:⚠️ Potential issue | 🟡 Minor
configure与current_desired_target_px对空 offsets 的处理不一致（小问题）。
configure()强制aim_dart_offsets_px.size() == dart_count，因此正常路径上 offsets 永远不可能为空；但current_desired_target_px()第 87-89 行又对空 offsets 做了回退（直接返回参考点）。要么这条回退分支永远死代码，要么应放宽configure的校验（例如允许empty()表示「所有飞镖共用参考点」）。建议二选一以消除歧义，并据此调整configure_from_parameter_values的校验条件。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/dart_launch_sequence.hpp` around lines 57 - 67, 当前不一致：configure() 强制 aim_dart_offsets_px.size() == dart_count 导致 offsets 永不为空，但 current_desired_target_px() 又对空 offsets 做回退，造成歧义；请统一逻辑：修改 configure()（及 configure_from_parameter_values 的校验）以允许 aim_dart_offsets_px.size() == 0 或 == dart_count（空表示所有飞镖共用参考点），保留 current_desired_target_px() 的空回退分支；在 configure() 中更新错误消息和条件以接受 size()==0，并在 configure_from_parameter_values 中相应放宽验证以匹配此行为。src/manager/task/silder_init_task.hpp-31-54 (1)
31-54:⚠️ Potential issue | 🟡 Minor确认
BeltMoveAction参数顺序与新签名一致。第 43 行注释为「右同步带力矩（输出）」，但
BeltMoveAction中该参数实际为const double& right_belt_torque（输入），注释与实际相反，请修正。同时第 54 行timeout_returns_success=true（"测试环境无接驳件"）会让超时被当作成功上报：在生产环境中，如果接驳件未到位也会静默走到零点校准步骤，可能造成误标定。建议把该开关改为通过构造函数参数透出，由调用方按部署环境显式配置，而不是在任务内硬编码true。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/task/silder_init_task.hpp` around lines 31 - 54, The BeltMoveAction parameter comment for the right belt torque is incorrect—update the comment near the BeltMoveAction construction in this task to reflect that right_belt_torque is an input (const double& right_belt_torque) rather than an output; also stop hardcoding timeout_returns_success=true in the task: add a constructor or task-level parameter (e.g., bool timeout_returns_success) and thread it through to the BeltMoveAction invocation so callers can explicitly configure timeout behavior for production versus test (update the call site symbol BeltMoveAction(...) and the task/class constructor that creates this action to accept and forward the new flag).tests/launch_preparation_task_test.cpp-1-80 (1)
1-80:⚠️ Potential issue | 🟡 Minor与
vision_assisted_launch_preparation_task_test.cpp同样的问题：整文件被注释。
LaunchPreparationTask是本 PR 的核心重构对象之一，却没有任何可执行测试。建议要么移除该文件，要么至少打开一个最小TEST_F验证tick_first()能拿到RUNNING与初始指令状态，否则即便 CMake 注册了tests/*.cpp，覆盖率仍为 0。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@tests/launch_preparation_task_test.cpp` around lines 1 - 80, The entire test file is commented out so LaunchPreparationTask has no executable tests; either delete the file or restore a minimal test: uncomment the includes and the LaunchPreparationTaskTest/Harness definitions and add at least one TEST_F (e.g., using LaunchPreparationTaskTest and Harness with LaunchPreparationTask::Mode::NORMAL) that calls harness.task.tick_first() and asserts ActionStatus::RUNNING and the expected initial commands (check harness.belt_command, harness.belt_target_velocity, harness.lifting_command, and harness.trigger_lock_enable); ensure tick() and tick_first() references and the LaunchPreparationTask::Mode symbol names match the refactored API so the test compiles and runs.src/manager/action/belt_constant_velocity_move_action.hpp-32-32 (1)
32-32:⚠️ Potential issue | 🟡 Minor
min_running_ticks参数注释与含义不符。注释写作「下行最大距离限制（m，正值，防止过度下行）」，但实际参数是用于位置到达判定前的最短运行帧数（ticks）。建议修正注释，避免后续维护者按错误含义传参。
- uint64_t min_running_ticks = 50) // 下行最大距离限制（m，正值，防止过度下行） + uint64_t min_running_ticks = 50) // 最短运行帧数（避免起步阶段误判到位）🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/action/belt_constant_velocity_move_action.hpp` at line 32, min_running_ticks 的注释目前写成“下行最大距离限制（m，正值，防止过度下行）”，但实际含义是到达位置判定前的最短运行帧数（ticks）；请在 BeltConstantVelocityMoveAction 构造函数或参数声明处（标识符 min_running_ticks）将注释改为表明它是“到达判定前的最短运行帧数/最小运行 ticks（uint64_t，默认 50）”，并补充一句说明其用途（防止过早判定到达），以避免调用者误传距离值。src/manager/action/force_screw_calibration_action.hpp-109-109 (1)
109-109:⚠️ Potential issue | 🟡 Minor显式添加
#include <algorithm>头文件。
std::clamp在 C++17 中定义于<algorithm>头，但当前文件未显式包含。虽然代码可能通过间接传递编译，但这种做法跨编译器、跨标准库版本不可靠，应显式在文件顶部增加#include <algorithm>。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/action/force_screw_calibration_action.hpp` at line 109, The file uses std::clamp (seen in the line with "output = std::clamp(output, -max_velocity_, max_velocity_);") but does not explicitly include the header that defines it; add an explicit `#include` <algorithm> at the top of the header (force_screw_calibration_action.hpp) so std::clamp is reliably available across compilers and standard library versions.test/image_displayer.cpp-46-46 (1)
46-46:⚠️ Potential issue | 🟡 Minor日志中残留旧类名
DebugDisplayComponent类已重命名为
ImageDisplayer，但第 46、61 行的日志仍输出旧名，会让排障时混淆是否加载到了正确组件。📝 建议的修正
- RCLCPP_INFO(logger_, "DebugDisplayComponent constructed"); + RCLCPP_INFO(logger_, "ImageDisplayer constructed"); ... - RCLCPP_INFO(logger_, "Initializing DebugDisplayComponent"); + RCLCPP_INFO(logger_, "Initializing ImageDisplayer");Also applies to: 61-61
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@test/image_displayer.cpp` at line 46, 日志中仍使用旧类名 "DebugDisplayComponent"（出现在 ImageDisplayer 的构造/初始化日志语句 logger_ 输出处），请将这两处日志字符串改为当前类名 "ImageDisplayer"：定位到 ImageDisplayer 构造函数/初始化代码中使用 RCLCPP_INFO(logger_, ...) 的日志调用（原文为 "DebugDisplayComponent constructed" 和另一个同类条目）并替换为描述性字符串包含 "ImageDisplayer"（例如 "ImageDisplayer constructed" 或相似一致的文本）。test/image_displayer.cpp-268-280 (1)
268-280:⚠️ Potential issue | 🟡 Minor
std::localtime非线程安全；建议改用localtime_r
save_current_frame由 OpenCV 显示线程调用，而std::localtime返回的是共享静态缓冲区，与其他线程（或未来扩展）并发调用会发生数据竞争。可参考src/vision/video_recorder.hpp:206已经使用的localtime_r模式。🔒 建议的修正
- auto time_t = std::chrono::system_clock::to_time_t(now); - char time_str[100]; - std::strftime(time_str, sizeof(time_str), "%Y%m%d_%H%M%S", std::localtime(&time_t)); + auto time_t = std::chrono::system_clock::to_time_t(now); + std::tm tm_buf; + localtime_r(&time_t, &tm_buf); + char time_str[100]; + std::strftime(time_str, sizeof(time_str), "%Y%m%d_%H%M%S", &tm_buf);🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@test/image_displayer.cpp` around lines 268 - 280, In save_current_frame(), replace the non-thread-safe std::localtime usage with the reentrant localtime_r: create a std::tm (or struct tm) instance, call localtime_r(&time_t, &tm), then use std::strftime with that tm to populate time_str before building filename; update references in save_current_frame (processed_image_, save_directory_, logger_) accordingly so the timestamp formatting is thread-safe (matching the localtime_r pattern used in video_recorder.hpp).src/vision/video_recorder.hpp-30-46 (1)
30-46:⚠️ Potential issue | 🟡 Minor对
fps/rotation_minutes/codec的输入未做合法性校验
rotation_minutes_为 0 或负数时，rotate_video_if_needed中elapsed.count() >= rotation_minutes_会对每一帧都成立，导致每一帧都新开一个cv::VideoWriter文件，瞬间产生大量很小的视频文件并显著抖动 IO。fps为 0 或负数时会被传给cv::VideoWriter::open(filename, fourcc, fps, size)，多数后端会直接打开失败或写出不可播放的文件。- 大小写敏感的字符串比较意味着
"h264"、"mjpeg"等会走到默认分支并落回 H264，但只有RCLCPP_ERROR提示，建议宽松匹配或显式 reject。建议在
Init入口加一层校验并 clamp/reject 非法值。🛡️ 建议的校验示例
bool Init(const std::string& save_directory, const std::string& codec, int fps, int rotation_minutes) { + if (fps <= 0) { + RCLCPP_ERROR(logger_, "Invalid fps=%d, must be > 0", fps); + return false; + } + if (rotation_minutes <= 0) { + RCLCPP_ERROR(logger_, "Invalid rotation_minutes=%d, must be > 0", rotation_minutes); + return false; + } save_directory_ = save_directory; fps_ = fps; rotation_minutes_ = rotation_minutes;🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/vision/video_recorder.hpp` around lines 30 - 46, The Init method currently doesn't validate inputs causing rotate_video_if_needed to reopen files every frame when rotation_minutes_ <= 0 and cv::VideoWriter to fail when fps_ <= 0, and codec matching is case-sensitive; update the Init(const std::string& save_directory, const std::string& codec, int fps, int rotation_minutes) implementation to validate and clamp/reject parameters: ensure fps > 0 (reject or clamp to a sane default like 30 and log via RCLCPP_ERROR/RCLCPP_WARN), ensure rotation_minutes_ > 0 (reject or clamp to a minimum like 1 to prevent per-frame rotation), and perform case-insensitive codec matching for codec strings before setting fourcc_code_ and file_extension_ (or explicitly reject unsupported values), logging clear errors; adjust any callers or docs accordingly so rotate_video_if_needed, fourcc_code_, file_extension_, fps_, and rotation_minutes_ get only validated values.src/manager/auto_aim_feedback.hpp-9-66 (1)
9-66:⚠️ Potential issue | 🟡 Minor
AutoAimFeedback是未使用的死代码，应移除仓库内搜索确认：
- 没有任何文件引用或 include
auto_aim_feedback.hppAutoAimFeedback类未在任何地方被实例化或使用- 相关功能已由
AutoAimAction直接处理（如src/manager/task/vision_assisted_launch_preparation_task.hpp中的用法）建议删除此类及其所在文件，或如有后续规划需要该中间层接口，应在该计划实现并连接调用点时再引入，避免维护未使用的代码路径。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/auto_aim_feedback.hpp` around lines 9 - 66, The AutoAimFeedback class is dead code and should be removed: delete the AutoAimFeedback declaration/implementation (auto_aim_feedback.hpp) and remove any references or build/include entries that mention AutoAimFeedback or auto_aim_feedback.hpp (e.g., in headers, source files, and CMake/BUILD lists); if you prefer to keep the interface for future use, instead add a clear TODO and unit test plan, but do not leave the unused class orphaned—either remove the file or wire it into the code paths that use AutoAimAction so it becomes referenced.
🧹 Nitpick comments (20)
src/manager/action/filling_limit_servo_action.hpp (1)
23-29: 硬编码200帧切换阈值，建议保留可配置参数（带默认值）。将原本可配置的
fill_ticks改为硬编码200，会把舵机释放→上锁的时序耦合到当前控制频率。两个调用点（launch_preparation_task.hpp、fire_and_preload_task.hpp）当前都用同一时序虽然没问题，但如果将来需要为某个流程加快/拉长释放时间，就要再改这个头文件。建议保留构造参数并给出默认值 200：建议的修改
FillingLimitServoAction( rmcs_msgs::DartLimitingServoStatus& limiting_command, rmcs_msgs::DartLimitingServoStatus trigger_command, - rmcs_msgs::DartLimitingServoStatus lock_command) + rmcs_msgs::DartLimitingServoStatus lock_command, uint64_t fill_ticks = 200) : IAction("limiting_fill") , limiting_command_(limiting_command) , trigger_command_(trigger_command) - , lock_command_(lock_command) {} + , lock_command_(lock_command) + , fill_ticks_(fill_ticks) {} @@ ActionStatus update() override { - if (elapsed_ticks() >= 200) { + if (elapsed_ticks() >= fill_ticks_) { limiting_command_ = lock_command_; return ActionStatus::SUCCESS; } return ActionStatus::RUNNING; } @@ rmcs_msgs::DartLimitingServoStatus lock_command_; + uint64_t fill_ticks_;🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/action/filling_limit_servo_action.hpp` around lines 23 - 29, The update() method in filling_limit_servo_action.hpp uses a hard-coded threshold (200) for elapsed_ticks(); change the class to accept a configurable fill_ticks parameter with a default value of 200 (exposed in the constructor) and replace the literal 200 in update() with that member (e.g., fill_ticks_); ensure existing callers (launch_preparation_task.hpp and fire_and_preload_task.hpp) either keep using the default or are updated to pass a custom fill_ticks when needed, and keep behavior of assigning limiting_command_ = lock_command_ and returning ActionStatus::SUCCESS unchanged.src/manager/action/belt_zero_calibration.hpp (1)
1-12: 建议显式包含<cstdint>。
hold_ticks_和tick_count_使用uint64_t，但本头文件没有显式#include <cstdint>，目前可能依赖action.hpp间接引入，未来重构action.hpp时可能编译失败。建议的修改
`#include` "action.hpp" +#include <cstdint> + namespace rmcs_dart_guidance::manager {🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/action/belt_zero_calibration.hpp` around lines 1 - 12, This header uses uint64_t for members like hold_ticks_ (and likely tick_count_) in class ZeroCalibrationAction but doesn't include <cstdint>; add an explicit `#include` <cstdint> at the top of src/manager/action/belt_zero_calibration.hpp so the use of uint64_t (in the ZeroCalibrationAction constructor and any tick_count_ declarations) does not rely on transitive includes from action.hpp.src/manager/action/vision_yaw_calibration.hpp (2)
22-25:5000帧硬编码，建议参数化为构造入参。
elapsed_ticks() > 5000是一个隐式的“校准持续时长”，与控制频率耦合。建议像ZeroCalibrationAction(hold_ticks)一样把它做成构造参数（默认值 5000），方便不同 tick 频率下复用，也便于单元测试。建议的修改
VisionYawCalibration( Eigen::Vector2d& yaw_pitch_angle, const Eigen::Vector2d& yaw_pitch_distance, - double yaw_angle_mapping_rate) + double yaw_angle_mapping_rate, uint64_t calibration_ticks = 5000) : IAction("vision_yaw_calibration") , yaw_control_angle_(yaw_pitch_angle.x()) , yaw_distance_(yaw_pitch_distance.x()) - , yaw_rate_(yaw_angle_mapping_rate) {} + , yaw_rate_(yaw_angle_mapping_rate) + , calibration_ticks_(calibration_ticks) {} @@ ActionStatus update() override { - if (elapsed_ticks() > 5000) { + if (elapsed_ticks() > calibration_ticks_) { return ActionStatus::SUCCESS; } @@ double yaw_distance_; double yaw_rate_; + uint64_t calibration_ticks_;🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/action/vision_yaw_calibration.hpp` around lines 22 - 25, The hardcoded 5000 tick duration in update() should be parameterized: add a constructor parameter (e.g., hold_ticks with default 5000) and store it as a member (e.g., hold_ticks_) in this calibration action class, then change the condition in ActionStatus update() from elapsed_ticks() > 5000 to elapsed_ticks() > hold_ticks_; update any callers/constructors to accept the new parameter or rely on the default so tests and different tick rates can pass a custom hold duration (see ZeroCalibrationAction(hold_ticks) for the intended pattern).
5-6:<opencv2/core/types.hpp>未被使用，可移除。本文件中只用到
Eigen::Vector2d，opencv2/core/types.hpp是无关引入，建议删除以减少编译依赖。建议的修改
`#include` <eigen3/Eigen/Dense> -#include <opencv2/core/types.hpp>🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/action/vision_yaw_calibration.hpp` around lines 5 - 6, 在 src/manager/action/vision_yaw_calibration.hpp 中移除未使用的 include <opencv2/core/types.hpp>，因為此檔案僅使用 Eigen::Vector2d（由 `#include` <eigen3/Eigen/Dense> 提供）；確保刪除該 include 後重新編譯以驗證沒有其他依賴於 OpenCV types 的引用（檢查本檔案內與相關函式/類別引用，例如任何使用 Eigen::Vector2d 的函式或類別定義）。test/force_sensor_recorder.cpp (1)
103-127: “异步写入”实际上仍可能阻塞控制循环：先join()再启动新线程。
async_write_to_file在启动新线程前会先对上一个线程join()。若上一次写入因为磁盘/IO 卡顿尚未结束，控制线程的update()就会同步等待——这违背了注释里 “异步写入，不阻塞控制循环” 的承诺，并且在两次触发间隔很短的场景下尤其明显。同时，由于这种 “先 join 再 spawn” 的串行化已经保证了同一时间只有一个 writer 线程，
file_mutex_实际上没有保护任何东西，可以拿掉，或者改为真正的“后台单线程 + 任务队列”：建议方向（任选其一）
- 简单兜底：去掉 mutex，并把对“控制线程不应阻塞”的承诺写得更弱（例如“尽量异步，但极端情况下会等待上一次写入”）。
- 推荐：用一个常驻 worker 线程 +
std::queue+std::condition_variable把记录入队即返回，析构时设置停止标志再 join。- // 异步写入 - std::mutex file_mutex_; - std::thread write_thread_; + // 异步写入（单生产者单消费者队列） + std::mutex queue_mutex_; + std::condition_variable queue_cv_; + std::queue<RecordEntry> pending_; + std::thread writer_thread_; + std::atomic<bool> stop_{false};🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@test/force_sensor_recorder.cpp` around lines 103 - 127, The current async_write_to_file() calls write_thread_.join() before spawning a new thread, which can block update(); change this by replacing the spawn-on-each-call pattern with a persistent background worker: create a worker thread (e.g., worker_thread_) that runs a loop consuming a thread-safe std::queue of write tasks guarded by a std::mutex and std::condition_variable, modify async_write_to_file() to push {shot_number, round_number, ch1_value, ch2_value} into the queue and notify the condition variable (do not join there), remove the unnecessary file_mutex_ from per-write code, have the worker open/append to output_file_path_, and in the class destructor set a stop flag, notify the worker and join worker_thread_ to cleanly shut down; alternatively, if you prefer the minimal change, remove file_mutex_ and the join in async_write_to_file() and update the function comment to state it may block in rare cases.src/manager/dart_launch_sequence.hpp (1)
71-78:advance_after_fire语义建议在文档中明确。返回
false时不递增 index——这意味着调用方在最后一发后再次调用仍会停留在最后一个 index。这通常是期望行为，但建议在头部加注释说明「sequence exhausted; caller should callreset()before reusing」，避免误用。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/dart_launch_sequence.hpp` around lines 71 - 78, 在 advance_after_fire() 上方添加一条注释，明确说明其语义：当 current_dart_index_ + 1 >= config_.dart_count 时函数返回 false 且不会递增 current_dart_index_，表示发射序列已耗尽；调用方若要再次使用序列应先调用 reset()（或其他复位方法），并说明返回 true 时会将 current_dart_index_ 递增一位。标注涉及的标识符：advance_after_fire, current_dart_index_, config_.dart_count, reset().tests/manual_force_control_test.cpp (1)
41-61: stall counter 重置测试逻辑依赖具体 tick 数，建议加注释。第 58-60 行连续 3 次
tick()才返回SUCCESS，这一长度与DartManualForceControlAction的 stall 确认窗口耦合。一旦后续调整stall_confirm_ticks默认值，本测试会成为盲改点。建议在测试上方注释中标注当前依赖的 stall 确认窗口大小，或改为通过构造参数显式传入，避免「神奇数字」。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@tests/manual_force_control_test.cpp` around lines 41 - 61, The test DartManualForceControlActionTest::ResetsStallCounterWhenCommandDropsToZero relies on a magic number of consecutive tick() calls (3) to reach SUCCESS which is coupled to DartManualForceControlAction's internal stall_confirm_ticks; either add a short comment above the test documenting the expected stall_confirm_ticks value being relied on, or better, modify the test to pass an explicit stall_confirm_ticks (or equivalent constructor parameter) into DartManualForceControlAction so the confirmation window is deterministic and not tied to defaults—locate the DartManualForceControlAction constructor invocation in this test and change it to supply the explicit stall_confirm_ticks value (or add a clarifying comment if you prefer the simpler opt).src/manager/action/belt_constant_velocity_move_action.hpp (1)
109-109:on_exit未重置belt_target_velocity_/belt_torque_limit_/belt_command_。当前仅清零
belt_torque_offset_。虽然现有调用链下一步是BeltHoldTorqueAction会重新设置这些字段，但一旦后续插入其他动作或异常退出，残留的速度/力矩限幅可能造成意外的传送带运动。建议同时把这三个字段恢复到安全默认（例如belt_command_=WAIT; belt_target_velocity_=0;）。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/action/belt_constant_velocity_move_action.hpp` at line 109, on_exit currently only clears belt_torque_offset_ but must also reset belt_target_velocity_, belt_torque_limit_ and belt_command_ to safe defaults to avoid leftover motion; update the on_exit() override to set belt_command_ = WAIT, belt_target_velocity_ = 0.0, belt_torque_limit_ = 0.0 (and keep belt_torque_offset_ = 0.0) so that if future actions or failures occur these fields cannot cause unexpected belt movement (references: on_exit, belt_target_velocity_, belt_torque_limit_, belt_command_, belt_torque_offset_, BeltHoldTorqueAction).tests/vision_assisted_launch_preparation_task_test.cpp (1)
1-131: 整文件均为注释，建议明确处理。该测试文件 100% 是注释代码，CMake 中虽然
tests/*.cpp会被收集，但本文件实际不贡献任何可执行用例。两种处理方式更佳：
- 暂不合入：先从本 PR 中移除该文件，等
VisionAssistedLaunchPreparationTask的接口稳定后再补充；- 保留占位：保留文件但用
GTEST_SKIP() << "TODO: 启用后再实现"写一个最小骨架，附 issue 链接，避免被当作已覆盖的测试。当前形式下，PR 描述中宣称的「扩展了测试覆盖」对该任务并未生效。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@tests/vision_assisted_launch_preparation_task_test.cpp` around lines 1 - 131, The whole test file is commented out and provides no executable tests; either remove it from the PR or replace it with a minimal skipped test so CI shows an intentional placeholder. To fix, either delete this file from the change set, or re-enable a tiny skeleton using the existing VisionAssistedLaunchPreparationTaskTest/ Harness symbols: add a single TEST_F(VisionAssistedLaunchPreparationTaskTest, Placeholder) that calls GTEST_SKIP() << "TODO: enable after VisionAssistedLaunchPreparationTask API stabilizes; see <issue-link>"; keep any needed includes/SetUpTestSuite/TearDownTestSuite so the test compiles and reference the issue URL in the skip message.src/manager/task/silder_init_task.hpp (1)
1-12: 缺少 Eigen 头文件包含。构造函数签名引用了
Eigen::Vector2d*（第 27-28 行），但本文件并未直接包含 Eigen 头文件，目前只是通过其他 include 间接传递。建议显式引入，避免后续重排 include 时产生编译失败。建议修改
`#include` "manager/action/belt_move_action.hpp" `#include` "manager/action/belt_zero_calibration.hpp" `#include` "manager/action/delay_action.hpp" `#include` "manager/action/vision_yaw_calibration.hpp" `#include` "manager/task/task.hpp" `#include` <memory> +#include <eigen3/Eigen/Dense> `#include` <rmcs_msgs/dart_limiting_servo_status.hpp> - `#include` <rmcs_msgs/dart_slider_status.hpp>🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/task/silder_init_task.hpp` around lines 1 - 12, The header is missing an explicit Eigen include used by the constructor signature that references Eigen::Vector2d* (the SliderInitTask constructor declaration around lines referencing Eigen::Vector2d*). Add the appropriate Eigen header (e.g., `#include` <Eigen/Dense> or `#include` <Eigen/Core>) near the other includes in silder_init_task.hpp so the Eigen::Vector2d type is defined directly in this header and will not rely on transitive includes.src/manager/action/force_screw_calibration_action.hpp (1)
96-106: 积分项缺少抗饱和处理（建议）。
integral_ += error;没有任何 anti-windup，长时间未达稳态时 integral 会无限增长，配合kp/ki/kd会让输出长时间打到±max_velocity_极限并在到达后产生大幅过冲。建议至少：
- 在 clamp 输出后做 back-calculation：
integral_ -= (raw_output - clamped_output) / ki_;（避免 ki=0 除零）；- 或对
integral_也做硬限幅（如±max_velocity_/ki_）。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/action/force_screw_calibration_action.hpp` around lines 96 - 106, The integral term integral_ is not protected against windup in ForceScrewCalibrationAction (integral_ updated with integral_ += error and used with kp_/ki_/kd_), so clamp-induced saturation can cause runaway integral; fix by implementing anti-windup: after computing raw output = kp_*error + ki_*integral_ + kd_*derivative (and applying any rate feedforward when use_filtered_input_ && use_rate_feedforward_ && force_rate_ != nullptr), clamp the final output to ±max_velocity_ and then perform back-calculation to roll back the integral: integral_ -= (raw_output - clamped_output) / ki_ while guarding against ki_ == 0, or alternatively enforce a hard clamp on integral_ itself (e.g., limit integral_ to ±(max_velocity_/max(ki_, epsilon))) so integral_ cannot grow unbounded; apply this near the block handling output and clamping.src/vision/video_recorder.hpp (2)
30-63:Init不可重入：重复调用会泄漏前一个 worker 线程若上层逻辑（例如重新加载参数）二次调用
Init，当前实现会在is_running_已为true时再次启动一个worker_thread_，覆盖原std::thread句柄并丢失对前一个线程的 join 入口。建议在入口先stop()，或显式拒绝重复 Init。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/vision/video_recorder.hpp` around lines 30 - 63, Init currently starts a new worker_thread_ even if is_running_ is true, leaking the previous thread; update Init to check is_running_ at the top and either call stop() to cleanly join/stop the existing worker_thread_ before proceeding or return false to reject re-entry, ensuring is_running_ and worker_thread_ are correctly synchronized (use the existing stop() method to join the old thread and reset state) before launching a new std::thread using &VideoRecorder::worker_loop.
65-77:frame.clone()在临界区内进行，可在锁外完成以减小持锁时间
push_frame当前在持有queue_mutex_的情况下做cv::Mat::clone()，对于 720p/1080p 的彩色图大约要拷贝几 MB，会让生产者（视觉主循环）和消费者（写盘线程）在同一锁上无谓争用。可以先在锁外 clone，再入队。♻️ 建议的微调
void push_frame(const cv::Mat& frame, const std::string& type) { if (!is_running_) return; - std::lock_guard<std::mutex> lock(queue_mutex_); - if (job_queue_.size() < max_queue_size_) { - job_queue_.push({frame.clone(), type, std::chrono::system_clock::now()}); - queue_cv_.notify_one(); - } else { + RecordJob job{frame.clone(), type, std::chrono::system_clock::now()}; + std::lock_guard<std::mutex> lock(queue_mutex_); + if (job_queue_.size() < max_queue_size_) { + job_queue_.push(std::move(job)); + queue_cv_.notify_one(); + } else { if (dropped_frames_++ % 30 == 0) { RCLCPP_WARN(logger_, "Video frame queue is full, dropping frame (%s)", type.c_str()); } } }🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/vision/video_recorder.hpp` around lines 65 - 77, push_frame currently calls frame.clone() while holding queue_mutex_, causing long lock durations; move the heavy clone call outside the critical section: create a local cv::Mat cloned = frame.clone() (and capture the timestamp) before acquiring queue_mutex_, then lock queue_mutex_, check job_queue_.size() < max_queue_size_, push the cloned frame with its type and timestamp into job_queue_, notify queue_cv_, and otherwise increment dropped_frames_ and log as before; keep symbols: push_frame, queue_mutex_, job_queue_, max_queue_size_, dropped_frames_, queue_cv_ and preserve existing behavior when queue is full.src/vision/dart_vision_core.cpp (1)
337-337:image_center_硬编码为{701, 565}，建议参数化图像中心目前是写死的常量，无法适配不同分辨率/不同标定的相机；与上方通过 ROS 参数化的相机配置（曝光、增益、HSV 阈值等）风格不一致。建议改为从 ROS 参数读入，或在收到首帧后由实际
cols/rows计算（与 192-193 行的cx/cy复用同一来源）。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/vision/dart_vision_core.cpp` at line 337, image_center_ is hard-coded as cv::Point2i{701,565}; change it to be configurable by reading a ROS parameter or computed from the first received frame (using image.cols/rows) and reuse the same cx/cy source used at lines where cx/cy are derived; specifically replace the literal cv::Point2i image_center_ initialization so it is assigned either from a ROS param (e.g., "image_center_x"/"image_center_y") or set to cv::Point2i(static_cast<int>(cx), static_cast<int>(cy)) after computing cx/cy from the camera info/first frame, ensuring image_center_ and the existing cx/cy logic stay consistent.CMakeLists.txt (1)
53-62: Debug/test 组件被无条件编入生产共享库
DEBUG_COMPONENT_SOURCE把test/*.cpp（包括image_displayer.cpp、force_sensor_recorder.cpp等带PLUGINLIB_EXPORT_CLASS的调试组件）始终编入${PROJECT_NAME}库并随plugins.xml一并发布。这意味着即便 release 构建也会带上调试组件，会增大体积、引入运行时依赖（如 HighGUI），并让真实部署可加载到测试节点。建议至少在if(BUILD_TESTING)或自定义开关下才合并这部分源。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@CMakeLists.txt` around lines 53 - 62, DEBUG_COMPONENT_SOURCE (which glob-matches test/*.cpp/.cc) is being unconditionally added into the production shared library via add_library(${PROJECT_NAME} SHARED ... ${DEBUG_COMPONENT_SOURCE}), causing test/debug plugins (e.g., image_displayer.cpp, force_sensor_recorder.cpp) to be built into release artifacts; change the CMake flow to only append DEBUG_COMPONENT_SOURCE when a test/build toggle is enabled (e.g., wrap the glob + the addition in an if(BUILD_TESTING) or a custom option like if(ENABLE_DEBUG_COMPONENTS)), so modify where DEBUG_COMPONENT_SOURCE is defined and where add_library(${PROJECT_NAME} ...) is called to conditionally include DEBUG_COMPONENT_SOURCE based on BUILD_TESTING or a new option, keeping the rest of ${PROJECT_SOURCE} always included.test/image_displayer.cpp (2)
268-280: 保存目录未创建即写入，失败时仅依赖 cv::imwrite 内部错误
save_directory_仅作为字符串参数读入，未在初始化时调用std::filesystem::create_directories，目录不存在或无写权限时cv::imwrite会静默失败（仍打印 "Saved frame: ..."），用户难以察觉。建议在initialize()里预先创建并校验写权限（参考video_recorder.hpp:48-58的处理）。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@test/image_displayer.cpp` around lines 268 - 280, The save_current_frame() path assumes save_directory_ exists and always logs success even if cv::imwrite silently fails; update initialization (e.g., initialize()) to call std::filesystem::create_directories(save_directory_) and verify the directory is writable, storing a boolean or validated path flag on the class; then in save_current_frame() (which uses processed_image_, processed_mutex_, logger_) check that the directory was validated before calling cv::imwrite and inspect its return value, logging an error via logger_ on failure and only logging "Saved frame" when imwrite returns true.
304-304:save_on_error_与last_target_time_已读取/赋值但未被使用
save_on_error_（第 34 行读入，第 322 行成员）在整个文件中没有被消费过，建议要么实现“出错时自动保存”的逻辑，要么从参数列表与成员中移除以避免误用。last_target_time_（第 138、304 行）只写不读，可考虑在显示循环中用它判定 target 信息是否过期（与 STALE 标注一致）。Also applies to: 322-322
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@test/image_displayer.cpp` at line 304, The member save_on_error_ is read from params and stored but never used—either implement the "save on error" behavior (catch errors where images/frames are processed/decoded and, when save_on_error_ is true, write the current frame to disk/emit a dump) or remove the parameter and the member to avoid dead code; the timestamp last_target_time_ is written but never consulted—update the code paths that receive/parse target info to set last_target_time_ and modify the display loop (the frame/render loop, e.g., the function that processes/display frames) to check std::chrono::steady_clock::now() - last_target_time_ against the STALE threshold and treat target data as expired (skip rendering target overlays or mark as stale) when exceeded.src/manager/dart_manager.cpp (2)
553-553:target_position_与target_position_input_重复且未注册。第 553 行的
target_position_input_与第 632 行的target_position_都是InputInterface<Eigen::Vector2d>成员，但只有target_position_input_在make_slider_init_task中被引用，且其对应的register_input在第 67 行已被注释；而target_position_既未注册也未被使用。结果是target_position_input_.ready()在运行期始终为false，分支一直走到default_target_position。请删除冗余成员并明确视觉接入策略：要么恢复register_input的注册，要么直接保留default_target_position并删除整段条件判断。Also applies to: 632-632
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/dart_manager.cpp` at line 553, There are two redundant InputInterface members (target_position_input_ and target_position_) causing ready() to never be true; either delete the unused target_position_ and re-enable the input registration for target_position_input_ (restore the register_input call referenced in register_input) so make_slider_init_task can use target_position_input_.ready(), or simplify by removing the conditional logic in make_slider_init_task that checks target_position_input_.ready() and always use default_target_position, then delete target_position_input_ entirely; update/remove references to target_position_ and target_position_input_ accordingly.
397-401:prepare_outputs_for_task是空实现的死代码。该函数体仅由一个判断 + 空
{}组成，没有任何副作用，被dispatch_next_task调用却不会做任何事情。建议删除该方法及调用点，或补上原计划的输出预清零逻辑（例如根据任务类型重置belt_*_/lifting_command_/force_control_velocity_），避免误导后来阅读者。🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/dart_manager.cpp` around lines 397 - 401, The function prepare_outputs_for_task(const Task& task) is a no-op (empty body) and should be fixed: either remove prepare_outputs_for_task and its callers (e.g., dispatch_next_task) to eliminate dead code, or implement the intended output reset logic inside prepare_outputs_for_task by switching on task.name() for the listed task types ("launch_preparation", "fire", "cancel_launch", "slider_init", "manual_angle", "manual_force") and clearing/resetting the relevant output state variables (e.g., belt_* variables, lifting_command_ fields, force_control_velocity_) to their safe default values before dispatching the task so downstream code receives deterministic outputs.src/manager/action/auto_aim_action.hpp (1)
184-201:std::abs(x) <= 0.0等价于x == 0.0，写法有歧义。
std::abs(error_px) <= 0.0与std::abs(gain) <= 0.0在数学上仅当数值严格为 0 才成立，使用abs() <= 0.0形式既冗余又会让阅读者误以为存在容差。建议改为显式的== 0.0比较，或引入一个明确的小阈值（如1e-9）让"忽略极小误差"的意图变清晰。♻️ 建议改写
- double compute_axis_velocity(double error_px, double gain) const { - if (std::abs(error_px) <= 0.0 || std::abs(gain) <= 0.0) { - return 0.0; - } + double compute_axis_velocity(double error_px, double gain) const { + constexpr double kEpsilon = 1e-9; + if (std::abs(error_px) <= kEpsilon || std::abs(gain) <= kEpsilon) { + return 0.0; + }🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/manager/action/auto_aim_action.hpp` around lines 184 - 201, In compute_axis_velocity, the checks using std::abs(... ) <= 0.0 are misleading (equivalent to == 0.0) — change them to explicit zero comparisons or use a small epsilon to express "near zero"; specifically replace the initial guard (std::abs(error_px) <= 0.0 || std::abs(gain) <= 0.0) with either (error_px == 0.0 || gain == 0.0) or a tolerance check like (std::abs(error_px) <= kEpsilon || std::abs(gain) <= kEpsilon), and likewise replace the bounded_magnitude check (bounded_magnitude <= 0.0) with (bounded_magnitude == 0.0) or (bounded_magnitude <= kEpsilon); introduce a named constant (e.g. constexpr double kEpsilon = 1e-9) if using a tolerance so intent is clear and use these changes inside compute_axis_velocity to preserve existing behavior while improving readability.
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: f8f3a6d6-3e11-4460-8c09-020a38ba639f
📒 Files selected for processing (33)
CMakeLists.txtpackage.xmlplugins.xmlsrc/manager/action/action_sequence.hppsrc/manager/action/auto_aim_action.hppsrc/manager/action/belt_constant_velocity_move_action.hppsrc/manager/action/belt_hold_torque_action.hppsrc/manager/action/belt_move_action.hppsrc/manager/action/belt_zero_calibration.hppsrc/manager/action/filling_lift_action.hppsrc/manager/action/filling_limit_servo_action.hppsrc/manager/action/force_screw_calibration_action.hppsrc/manager/action/manual_angle_control.hppsrc/manager/action/manual_force_control.hppsrc/manager/action/vision_yaw_calibration.hppsrc/manager/auto_aim_feedback.hppsrc/manager/command.cppsrc/manager/dart_launch_sequence.hppsrc/manager/dart_manager.cppsrc/manager/task/cancel_launch_task.hppsrc/manager/task/fire_and_preload_task.hppsrc/manager/task/launch_preparation_task.hppsrc/manager/task/silder_init_task.hppsrc/manager/task/vision_assisted_launch_preparation_task.hppsrc/vision/dart_vision_core.cppsrc/vision/video_recorder.hpptest/force_sensor_recorder.cpptest/image_displayer.cpptests/auto_aim_action_test.cpptests/dart_launch_sequence_test.cpptests/launch_preparation_task_test.cpptests/manual_force_control_test.cpptests/vision_assisted_launch_preparation_task_test.cpp
💤 Files with no reviewable changes (1)
- src/manager/command.cpp
Comment on lines
+49
to
+67
| void on_enter() override { | ||
| stall_counter_ = 0; | ||
| initial_angle_ = left_belt_angle_; | ||
| double target_angle_offset = | ||
| target_distance_ / 0.0195; // 将目标距离换算为角度偏移（rad），滑轮半径为0.0195m | ||
| target_position_ = initial_angle_ + target_angle_offset; | ||
|
|
||
| double distance_to_target = target_position_ - initial_angle_; | ||
| if (distance_to_target > 0) { | ||
| belt_command_ = rmcs_msgs::DartSliderStatus::DOWN; | ||
| belt_target_velocity_ = std::abs(velocity_); | ||
| } else { | ||
| belt_command_ = rmcs_msgs::DartSliderStatus::UP; | ||
| belt_target_velocity_ = std::abs(velocity_); | ||
| } | ||
|
|
||
| belt_torque_offset_ = torque_offset_value_; | ||
| belt_torque_limit_ = torque_limit_; | ||
| } |
There was a problem hiding this comment.
initial_angle_ 与终止判断使用的角度口径不一致。
on_enter 中 initial_angle_ = left_belt_angle_;（仅左电机），而 update() 中却用 avg_angle = (left + right) / 2.0 来评估位置误差。一旦左右电机存在初始相位差或运行中产生差异（双电机通常会），target_position_ 与 avg_angle 就处于不同口径，会引入恒定的位置偏置，可能导致提前/滞后到位甚至误判 overshoot。
建议修改
- initial_angle_ = left_belt_angle_;
+ initial_angle_ = (left_belt_angle_ + right_belt_angle_) / 2.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
| void on_enter() override { | |
| stall_counter_ = 0; | |
| initial_angle_ = left_belt_angle_; | |
| double target_angle_offset = | |
| target_distance_ / 0.0195; // 将目标距离换算为角度偏移（rad），滑轮半径为0.0195m | |
| target_position_ = initial_angle_ + target_angle_offset; | |
| double distance_to_target = target_position_ - initial_angle_; | |
| if (distance_to_target > 0) { | |
| belt_command_ = rmcs_msgs::DartSliderStatus::DOWN; | |
| belt_target_velocity_ = std::abs(velocity_); | |
| } else { | |
| belt_command_ = rmcs_msgs::DartSliderStatus::UP; | |
| belt_target_velocity_ = std::abs(velocity_); | |
| } | |
| belt_torque_offset_ = torque_offset_value_; | |
| belt_torque_limit_ = torque_limit_; | |
| } | |
| void on_enter() override { | |
| stall_counter_ = 0; | |
| initial_angle_ = (left_belt_angle_ + right_belt_angle_) / 2.0; | |
| double target_angle_offset = | |
| target_distance_ / 0.0195; // 将目标距离换算为角度偏移（rad），滑轮半径为0.0195m | |
| target_position_ = initial_angle_ + target_angle_offset; | |
| double distance_to_target = target_position_ - initial_angle_; | |
| if (distance_to_target > 0) { | |
| belt_command_ = rmcs_msgs::DartSliderStatus::DOWN; | |
| belt_target_velocity_ = std::abs(velocity_); | |
| } else { | |
| belt_command_ = rmcs_msgs::DartSliderStatus::UP; | |
| belt_target_velocity_ = std::abs(velocity_); | |
| } | |
| belt_torque_offset_ = torque_offset_value_; | |
| belt_torque_limit_ = torque_limit_; | |
| } |
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/manager/action/belt_constant_velocity_move_action.hpp` around lines 49 -
67, The initial angle used to compute target_position_ is taken from only
left_belt_angle_ in on_enter(), while update() evaluates progress using
avg_angle = (left_belt_angle_ + right_belt_angle_) / 2.0, causing a consistent
bias when left/right differ; change on_enter() to set initial_angle_ using the
same averaging as update() (use both left_belt_angle_ and right_belt_angle_ to
compute initial_angle_), then compute target_position_ from that averaged
initial_angle_ so initial_angle_, target_position_, and the avg_angle used in
update() share the same reference frame (adjust references to initial_angle_,
left_belt_angle_, right_belt_angle_, target_position_, and on_enter()).
Comment on lines
+51
to
+52
| , current_force_ch1_(*(int*)nullptr) // dummy, won't be used | ||
| , current_force_ch2_(*(int*)nullptr) |
There was a problem hiding this comment.
严重未定义行为：将引用绑定到空指针解引用结果。
, current_force_ch1_(*(int*)nullptr) // dummy, won't be used
, current_force_ch2_(*(int*)nullptr)C++ 标准规定引用必须绑定到合法对象，对空指针解引用属于 UB（即使后续不读取）。GCC/Clang 在开启优化或 UBSan 时可能直接生成 trap 指令或基于「引用永远非空」做激进优化，进而导致随机崩溃或被裁剪。
建议将 current_force_ch1_/ch2_ 改为指针成员，或采用静态哨兵对象绑定：
建议修改：改用指针并在 update 中按 mode 分支
private:
double& force_screw_velocity_;
- const int& current_force_ch1_;
- const int& current_force_ch2_;
+ const int* current_force_ch1_;
+ const int* current_force_ch2_;构造函数相应改为：
- , current_force_ch1_(current_force_ch1)
- , current_force_ch2_(current_force_ch2)
+ , current_force_ch1_(¤t_force_ch1)
+ , current_force_ch2_(¤t_force_ch2)
...
- , current_force_ch1_(*(int*)nullptr) // dummy, won't be used
- , current_force_ch2_(*(int*)nullptr)
+ , current_force_ch1_(nullptr)
+ , current_force_ch2_(nullptr)update() 中的取值改为：
- current_force =
- static_cast<double>(force_channel_ == 2 ? current_force_ch2_ : current_force_ch1_) *
- 0.00981;
+ const int raw =
+ force_channel_ == 2 ? *current_force_ch2_ : *current_force_ch1_;
+ current_force = static_cast<double>(raw) * 0.00981;🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/manager/action/force_screw_calibration_action.hpp` around lines 51 - 52,
The constructor currently binds references current_force_ch1_ and
current_force_ch2_ to a null-dereference which is undefined behavior; change
these members to either pointers (e.g., int* current_force_ch1_,
current_force_ch2_) and initialize them to nullptr in the constructor and
dereference only after checking in update(), or replace the references with
references bound to a static sentinel int (e.g., a file-static kDummy) to
guarantee a valid object; update the constructor, the class member declarations,
and the update() logic in ForceScrewCalibrationAction (and any uses of
current_force_ch1_/current_force_ch2_) so they safely read via pointer checks or
from the sentinel depending on chosen approach.
Comment on lines
+75
to
+77
| if (elapsed_ticks() >= timeout_ticks_) { | ||
| return ActionStatus::SUCCESS; | ||
| } |
There was a problem hiding this comment.
超时返回 SUCCESS 容易掩盖标定失败。
elapsed_ticks() >= timeout_ticks_ 直接返回 SUCCESS，会让上层任务以为已经完成稳定校准，但实际可能力值从未进入容差。建议改为 FAILURE，或将「超时是否视为成功」做成构造参数（与 BeltMoveAction::timeout_returns_success 保持一致风格），由调用方按场景决定。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/manager/action/force_screw_calibration_action.hpp` around lines 75 - 77,
The code currently treats a timeout in ForceScrewCalibrationAction as success
(the elapsed_ticks() >= timeout_ticks_ branch returns ActionStatus::SUCCESS),
which can hide failed calibrations; modify the logic to mirror BeltMoveAction by
adding a constructor parameter (e.g. bool timeout_returns_success_ defaulting to
false) stored as a member and change the timeout branch to return
timeout_returns_success_ ? ActionStatus::SUCCESS : ActionStatus::FAILURE;
alternatively, if you prefer a minimal change, change the hardcoded return to
ActionStatus::FAILURE; update the constructor and member declarations in
ForceScrewCalibrationAction and use the elapsed_ticks()/timeout_ticks_ check to
consult timeout_returns_success_ when deciding the return value.
Comment on lines
+12
to
+33
| VisionYawCalibration( | ||
| Eigen::Vector2d& yaw_pitch_angle, const Eigen::Vector2d& yaw_pitch_distance, | ||
| double yaw_angle_mapping_rate) | ||
| : IAction("vision_yaw_calibration") | ||
| , yaw_control_angle_(yaw_pitch_angle.x()) | ||
| , yaw_distance_(yaw_pitch_distance.x()) | ||
| , yaw_rate_(yaw_angle_mapping_rate) {} | ||
|
|
||
| void on_enter() override {} | ||
|
|
||
| ActionStatus update() override { | ||
| if (elapsed_ticks() > 5000) { | ||
| return ActionStatus::SUCCESS; | ||
| } | ||
| yaw_control_angle_ = yaw_distance_ * yaw_rate_; | ||
| return ActionStatus::RUNNING; | ||
| } | ||
|
|
||
| private: | ||
| double& yaw_control_angle_; | ||
| double yaw_distance_; | ||
| double yaw_rate_; |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# 找到 VisionYawCalibration 的所有构造点，确认传入的 Vector2d 来源与生命周期
rg -nP -C5 '\bVisionYawCalibration\s*\('
# 找到 IAction 默认 elapsed_ticks() 的实现，确认每次 on_enter 后是否会重置
rg -nP -C3 '\belapsed_ticks\b'Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 9915
🏁 Script executed:
# Find the full IAction class definition to see how elapsed_ticks_ is managed
fd -t f action.hpp | head -5 | xargs cat -n | head -100Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 3418
🏁 Script executed:
# Search for VisionYawCalibration instantiation/usage
rg -n 'VisionYawCalibration' --type cpp --type hppRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 109
🏁 Script executed:
# Find and examine slider_init_task.hpp mentioned in the review
fd -t f 'slider_init_task.hpp' | xargs cat -nRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 63
🏁 Script executed:
# Correct search for VisionYawCalibration instantiation
rg -n 'VisionYawCalibration' -A 5 -B 5Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 1984
🏁 Script executed:
# Find slider_init_task file
fd 'slider_init_task' -type fRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 252
🏁 Script executed:
# Read silder_init_task.hpp to understand the lifetime of yaw_pitch_angle and yaw_pitch_distance
cat -n src/manager/task/silder_init_task.hppRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 3756
yaw_distance_ 按值捕获导致后续每帧使用的是构造期的初值，无法反映视觉系统的动态更新。
构造参数 const Eigen::Vector2d& yaw_pitch_distance 被复制为成员 double yaw_distance_;，仅在初始化时拷贝一次值。而 yaw_control_angle_ 采用 double& 引用绑定，每帧都能获取最新值。这种不对称在每帧调用 yaw_distance_ * yaw_rate_ 时会导致使用过时的距离值——即使视觉系统在 5000 帧内更新了 yaw_pitch_distance。
建议将 yaw_distance_ 改为 const double& 以保持与 yaw_control_angle_ 一致的引用语义：
建议的修改
private:
double& yaw_control_angle_;
- double yaw_distance_;
+ const double& yaw_distance_;
double yaw_rate_;参考 silder_init_task.hpp 第 27–28 行，传入的 yaw_pitch_angle 和 yaw_pitch_distance 为指针参数，指向的对象生命周期贯穿整个任务执行，不存在悬垂引用的风险。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/manager/action/vision_yaw_calibration.hpp` around lines 12 - 33, The
constructor copies yaw_pitch_distance into yaw_distance_ causing stale values
each frame; change the member from double yaw_distance_ to const double&
yaw_distance_ and bind it in VisionYawCalibration's initializer so that update()
reads the live yaw_pitch_distance each tick (mirror how yaw_control_angle_ is a
reference); adjust any initializer lists and class declaration accordingly to
avoid dangling references only if the passed-in yaw_pitch_distance lifetime is
guaranteed (per silder_init_task usage).
Comment on lines
+433
to
+484
| if (left_belt_angle_.ready() && right_belt_angle_.ready()) { | ||
| RCLCPP_INFO( | ||
| logger_, "[DartManager] Belt angles: left=%.4f, right=%.4f", *left_belt_angle_, | ||
| *right_belt_angle_); | ||
| } else { | ||
| RCLCPP_WARN(logger_, "[DartManager] Belt angle feedback NOT READY!"); | ||
| } | ||
| RCLCPP_INFO( | ||
| logger_, "[DartManager] Belt params: down_distance=%.4f m, pulley_radius=%.4f m", | ||
| belt_down_distance_, down_velocity); | ||
|
|
||
| // 读取力值用于力矩闭环（不在此处记录） | ||
| bool ch1_ready = current_force_ch1_.ready(); | ||
| bool ch2_ready = current_force_ch2_.ready(); | ||
|
|
||
| int force_ch1_value = ch1_ready ? *current_force_ch1_ : 0; | ||
| int force_ch2_value = ch2_ready ? *current_force_ch2_ : 0; | ||
|
|
||
| // 根据 force_channel_ 选择主控通道 | ||
| if (force_channel_ == 2 && ch2_ready) { | ||
| last_fire_force_ = static_cast<double>(force_ch2_value); | ||
| } else if (force_channel_ == 1 && ch1_ready) { | ||
| last_fire_force_ = static_cast<double>(force_ch1_value); | ||
| } | ||
|
|
||
| // Prepare Kalman filter pointers (if enabled and ready) | ||
| const double* kalman_force_ptr = nullptr; | ||
| const double* kalman_rate_ptr = nullptr; | ||
| if (use_kalman_force_ && kalman_filtered_force_.ready() && kalman_force_rate_.ready()) { | ||
| kalman_force_ptr = &(*kalman_filtered_force_); | ||
| kalman_rate_ptr = &(*kalman_force_rate_); | ||
| RCLCPP_INFO( | ||
| logger_, "[DartManager] Using Kalman force: F_filt=%.1fN, dF/dt=%.1fN/s", | ||
| *kalman_force_ptr, *kalman_rate_ptr); | ||
| } else if (use_kalman_force_) { | ||
| RCLCPP_WARN( | ||
| logger_, | ||
| "[DartManager] use_kalman_force=true but Kalman inputs not ready, falling back " | ||
| "to raw sensors"); | ||
| } | ||
|
|
||
| return std::make_shared<LaunchPreparationTask>( | ||
| *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, | ||
| *belt_wait_zero_velocity_, | ||
| *left_belt_velocity_, *right_belt_velocity_, | ||
| *left_belt_torque_, *right_belt_torque_, | ||
| *trigger_lock_enable_, | ||
| *lifting_command_, | ||
| *lifting_left_vel_fb_, *lifting_right_vel_fb_, | ||
| lifting_stall_threshold_, lifting_stall_confirm_ticks_, | ||
| lifting_stall_min_run_ticks_, lifting_stall_timeout_ticks_); | ||
| *belt_wait_zero_velocity_, *belt_torque_offset_, *left_belt_angle_, | ||
| *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, | ||
| *trigger_lock_enable_, belt_down_distance_, down_velocity, require_lifting_down, | ||
| *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, | ||
| *belt_zero_calibration_, *force_control_velocity_, *current_force_ch1_, | ||
| *current_force_ch2_, force_channel_, last_fire_force_, enable_force_calibration_, | ||
| force_tolerance_, force_settle_ticks_, force_timeout_ticks_, force_kp_, force_ki_, | ||
| force_kd_, is_first_shot, use_kalman_force_, kalman_force_ptr, kalman_rate_ptr, | ||
| kalman_rate_feedforward_, kalman_rate_gain_); |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# Verify InputInterface dereference behavior when not ready
ast-grep --pattern 'class InputInterface { $$$ }'
rg -nP --type=cpp -C2 '\bInputInterface<.*>::operator\*'
rg -nP --type=cpp -C3 '\.ready\(\)' src/manager/dart_manager.cppRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 2569
🏁 Script executed:
sed -n '430,520p' src/manager/dart_manager.cpp | cat -nRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 5575
🏁 Script executed:
fd -t f -name "*.hpp" -o -name "*.h" | xargs rg -l "class InputInterface" | head -5Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 312
🏁 Script executed:
rg -A 10 "operator\*" --type=cpp | grep -A 10 "InputInterface"Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 63
🏁 Script executed:
find . -name "*.hpp" -o -name "*.h" | xargs rg -l "class InputInterface" | head -3Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 63
🏁 Script executed:
rg -B5 -A15 "class InputInterface" --type=cpp --type=hpp | head -60Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 109
🏁 Script executed:
rg "class InputInterface" --type-list | head -20Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 471
🏁 Script executed:
rg "class InputInterface" -g "*.h" -g "*.hpp" -g "*.cpp"Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 63
🏁 Script executed:
find . -name "InputInterface*" -o -name "*input*interface*"Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 63
🏁 Script executed:
rg "InputInterface" --type=c --type=cppRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 2419
🏁 Script executed:
rg -B2 -A8 "operator\s*\*" --type=c --type=cpp | grep -A10 -B2 "InputInterface"Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 63
🏁 Script executed:
git ls-files | head -30Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 1202
🏁 Script executed:
rg "#include.*InputInterface" --type=cppRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 63
🏁 Script executed:
cat package.xmlRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 1142
🏁 Script executed:
rg -B3 "\.ready\(\)" src/manager/dart_manager.cpp | head -40Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 1643
未就绪输入被无条件解引用，构造调用前缺少返回防护。
代码在第 433–439 行显式检查 left_belt_angle_ 和 right_belt_angle_ 的 ready() 状态并在 else 分支打印警告，但随后在第 476–481 行的 LaunchPreparationTask 构造调用中无条件解引用 *left_belt_angle_, *right_belt_angle_ 以及 *current_force_ch1_, *current_force_ch2_（这两个虽在第 445–446 行检查过，但 ch1_ready/ch2_ready 标志并未被用来防护构造参数）。同样问题出现在第 489–495 行的 CancelLaunchTask 调用中（*left_belt_angle_, *right_belt_angle_, *lifting_left_vel_fb_, *lifting_right_vel_fb_），后两个接口甚至没有任何 ready() 检查。
相比之下，同文件其他位置采用了安全的条件解引用模式（如第 410 行的三元表达式、第 520–522 行的空指针检查），说明 InputInterface 的非就绪状态需要特殊处理。第 433 行的 ready() 检查和警告日志明确承认了这个风险，但代码继续执行而非提前返回，违反了防御性编程原则。建议在 ready() 检查失败时添加 return nullptr; 并打 ERROR 日志，或将所有 InputInterface 参数改为安全的三元表达式/条件指针形式。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/manager/dart_manager.cpp` around lines 433 - 484, The code
unconditionally dereferences InputInterface values (e.g., *left_belt_angle_,
*right_belt_angle_, *current_force_ch1_, *current_force_ch2_,
*lifting_left_vel_fb_, *lifting_right_vel_fb_) when constructing
LaunchPreparationTask and CancelLaunchTask despite earlier ready() checks, so
either (A) perform strict readiness guarding: if any required input for the
upcoming task is not ready, log an RCLCPP_ERROR with context and return nullptr
immediately from the function, or (B) make the constructor calls safe by passing
guarded/nullable values (use ternary-style safe dereference or pass
nullptr/optional pointers) for each possibly-unready InputInterface (references:
LaunchPreparationTask, CancelLaunchTask, variables left_belt_angle_,
right_belt_angle_, current_force_ch1_, current_force_ch2_, lifting_left_vel_fb_,
lifting_right_vel_fb_), ensuring no raw dereference occurs unless ready() was
true.
Comment on lines
+38
to
40
| // 立即停止力丝杆电机（直接设置速度为0） | ||
| force_screw_control_velocity = 0.0; | ||
|
|
There was a problem hiding this comment.
构造期写入输出可能引发竞态。
force_screw_control_velocity = 0.0; 在构造函数体中立即执行，而 CancelLaunchTask 的实际入队/调度发生在 dart_manager.cpp 的 submit_task 之后。如果队列中尚有未完成的任务（例如 LaunchPreparationTask 仍在使用力丝杆），构造该取消任务时就会被立刻清零，可能导致前置任务行为异常。建议把"立即停止力丝杆"封装为一个动作（如轻量的 then(std::make_shared<SetValueAction>(force_screw_control_velocity, 0.0))），放在动作链最前面，使其在调度到本任务时才生效。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/manager/task/cancel_launch_task.hpp` around lines 38 - 40, The
constructor for CancelLaunchTask currently writes force_screw_control_velocity =
0.0 immediately, which can race with already-queued tasks like
LaunchPreparationTask; instead, remove the direct write from the
CancelLaunchTask constructor and wrap the stop into a scheduled action so it
executes when the cancel task runs (e.g., create a lightweight action such as
std::make_shared<SetValueAction>(force_screw_control_velocity, 0.0) and prepend
it to the CancelLaunchTask action chain via then(...)); update CancelLaunchTask
to build and enqueue that action in its action list rather than mutating
force_screw_control_velocity at construction time, ensuring submit_task in
dart_manager.cpp schedules the stop only when the cancel task is dispatched.
Comment on lines
43
to
+48
| then( | ||
| std::make_shared<BeltMoveAction>( | ||
| "belt_move_down", // 动作名称 | ||
| belt_command, // 同步带目标状态（输出） | ||
| belt_target_velocity, // 同步带目标速度（输出） | ||
| belt_torque_limit, // 同步带力矩限制（输出） | ||
| belt_hold_torque, // 同步带保持力矩（输出） | ||
| belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 | ||
| left_belt_velocity, // 左同步带反馈（输入） | ||
| right_belt_velocity, // 右同步带反馈（输入） | ||
| left_belt_torque, // 左同步带力矩（输入） | ||
| right_belt_torque, // 右同步带力矩（输入） | ||
| rmcs_msgs::DartSliderStatus::DOWN, // 指令状态 | ||
| 10.0, // 设定速度 | ||
| torque_limit, // 设定力矩限制 | ||
| hold_torque, // 设定保持力矩 | ||
| 10000, // 超时帧数 | ||
| 1.0, // 堵转速度阈值 | ||
| 0.5, // 堵转力矩阈值 | ||
| 100, // 堵转确认帧数 | ||
| 50, // 最短运行帧数 | ||
| BeltMoveAction::ExitMode::WAIT_HOLD_TORQUE)); | ||
|
|
||
| std::make_shared<FillingLimitServoAction>( | ||
| limiting_command, // 限位舵机状态（输出） | ||
| rmcs_msgs::DartLimitingServoStatus::FREE, // 先释放 | ||
| rmcs_msgs::DartLimitingServoStatus::LOCK // 再锁回 | ||
| )); |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# Confirm limiting_command is undeclared in launch_preparation_task.hpp
rg -nP --type=cpp '\blimiting_command\b' src/manager/task/launch_preparation_task.hpp
rg -nP --type=cpp 'LaunchPreparationTask\s*\(' src/manager/dart_manager.cpp -A 12Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 156
🏁 Script executed:
#!/bin/bash
# Read the launch_preparation_task.hpp file to see full constructor
cat -n src/manager/task/launch_preparation_task.hpp | head -70Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 4272
🏁 Script executed:
#!/bin/bash
# Search for LaunchPreparationTask class definition and constructor more broadly
rg -nP --type=cpp 'class LaunchPreparationTask' -A 50 src/manager/task/launch_preparation_task.hppRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 3389
🏁 Script executed:
#!/bin/bash
# Search for LaunchPreparationTask constructor calls in dart_manager.cpp
rg -nP --type=cpp 'LaunchPreparationTask' src/manager/dart_manager.cpp -B 2 -A 15Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 1292
🏁 Script executed:
#!/bin/bash
# Check if limiting_command_ is a member variable in LaunchPreparationTask
rg -nP --type=cpp 'limiting_command' src/manager/task/launch_preparation_task.hppRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 156
🏁 Script executed:
#!/bin/bash
# Check the class definition for member variables
cat -n src/manager/task/launch_preparation_task.hpp | tail -50Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 2892
🏁 Script executed:
#!/bin/bash
# Check dart_manager.cpp for limiting_command_
rg -nP --type=cpp 'limiting_command' src/manager/dart_manager.cppRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 534
limiting_command 在构造函数中未声明，无法编译。
第 45 行使用 limiting_command，但该变量未在构造函数参数列表（26–40 行）中声明，也不是类成员变量。需要添加为构造函数参数：
修改位置
- bool& trigger_lock_enable, double belt_down_distance, double down_velocity,
- bool require_lifting_down, rmcs_msgs::DartSliderStatus& lifting_command,
+ bool& trigger_lock_enable, rmcs_msgs::DartLimitingServoStatus& limiting_command,
+ double belt_down_distance, double down_velocity,
+ bool require_lifting_down, rmcs_msgs::DartSliderStatus& lifting_command,并在 src/manager/dart_manager.cpp 第 474–484 行的 LaunchPreparationTask 实例化调用中，在 *trigger_lock_enable_ 后传入 *limiting_command_。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/manager/task/launch_preparation_task.hpp` around lines 43 - 48, The
constructor for LaunchPreparationTask is using limiting_command in the then(...
FillingLimitServoAction(...)) call but limiting_command is not declared; add a
parameter (e.g., const rmcs_msgs::DartLimitingServoCommand& limiting_command or
a pointer/reference type matching usages) to the LaunchPreparationTask
constructor signature and initializer list so the member is available in the
constructor body, and update the LaunchPreparationTask instantiation in
DartManager (the call that currently passes *trigger_lock_enable_ ) to also pass
*limiting_command_ so the new constructor parameter is provided; ensure the
symbol names LaunchPreparationTask, limiting_command, and
FillingLimitServoAction are consistent.
Comment on lines
+190
to
+212
| if (enable_force_calibration) { | ||
| double target_force_value = is_first_shot ? 21800.0 : target_force; | ||
| double tolerance_value = is_first_shot ? 0.5 : force_tolerance; | ||
|
|
||
| if (use_kalman_force && kalman_filtered_force != nullptr | ||
| && kalman_force_rate != nullptr) { | ||
| // Use Kalman-filtered force (already in Newtons) | ||
| then( | ||
| std::make_shared<ForceScrewCalibrationAction>( | ||
| "force_calibration_kalman", force_screw_velocity, *kalman_filtered_force, | ||
| *kalman_force_rate, target_force_value, tolerance_value, force_settle_ticks, | ||
| force_timeout_ticks, force_kp, force_ki, force_kd, 5.0, | ||
| kalman_rate_feedforward, kalman_rate_gain)); | ||
| } else { | ||
| // Use raw force sensor readings (in grams) | ||
| then( | ||
| std::make_shared<ForceScrewCalibrationAction>( | ||
| "force_calibration_raw", force_screw_velocity, current_force_ch1, | ||
| current_force_ch2, force_channel, target_force_value, tolerance_value, | ||
| force_settle_ticks, force_timeout_ticks, force_kp, force_ki, force_kd, | ||
| 5.0)); | ||
| } | ||
| } |
There was a problem hiding this comment.
首发力标定的目标值/容差与单位语义不一致。
第 191–192 行无论后续走 Kalman 分支还是原始传感器分支，都对 is_first_shot 强制覆盖为 target_force_value = 21800.0、tolerance_value = 0.5：
- 在 197–202 的 Kalman 分支中，注释明确指出"already in Newtons"，因此
21800.0 N显然不是首发期望值（按字面值，约 ~2 吨力）。 - 在 205–211 的原始通道分支中，传感器读数是 grams（参见调用点
force_ch1_value/force_ch2_value为int克数），21800 g才符合预期，但此时tolerance_value = 0.5又看起来像 Newtons 量级（0.5 g 容差几乎不可能稳态）。
无论是哪种解释，这两个量纲在两个分支间无法同时成立。建议为 Kalman 与原始模式分别提供首发覆盖值，或在调用方就把单位归一化后再传入；否则首发标定要么永远超时要么过早完成。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/manager/task/launch_preparation_task.hpp` around lines 190 - 212, The
is_first_shot override uses a single hardcoded pair (target_force_value =
21800.0, tolerance_value = 0.5) but your branches use different units: the
Kalman branch (use_kalman_force, kalman_filtered_force, kalman_force_rate)
expects Newtons while the raw branch (current_force_ch1, current_force_ch2,
force_channel) expects grams; update the logic so that when is_first_shot is
true you set unit-appropriate defaults (e.g., separate
kalman_target_force_newtons / kalman_tolerance_newtons for the Kalman path and
raw_target_force_grams / raw_tolerance_grams for the raw path) or normalize
units before constructing ForceScrewCalibrationAction; adjust the assignment of
target_force_value and tolerance_value (or create two variables) and use the
correct ones in each then(...) call to ForceScrewCalibrationAction
("force_calibration_kalman" vs "force_calibration_raw").
Comment on lines
+22
to
+62
| VisionAssistedLaunchPreparationTask( | ||
| rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, | ||
| double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, | ||
| double& belt_torque_offset, const double& left_belt_angle, const double& right_belt_angle, | ||
| const double& left_belt_velocity, const double& right_belt_velocity, | ||
| const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, | ||
| double belt_down_distance, double belt_pulley_radius, double down_velocity, | ||
| double torque_limit, double up_torque_limit, uint64_t down_ramp_ticks, | ||
| double down_torque_offset, double down_hold_torque, double down_zero_velocity_threshold, | ||
| uint64_t down_zero_confirm_ticks, uint64_t down_ramp_timeout_ticks, | ||
| bool require_lifting_down, rmcs_msgs::DartSliderStatus& lifting_command, | ||
| const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, | ||
| bool& belt_zero_calibration, double belt_up_distance, double up_velocity, | ||
| double up_decel_target_velocity, double up_decel_torque_offset, | ||
| double up_stall_velocity_threshold, uint64_t up_stall_confirm_ticks, | ||
| uint64_t up_stall_min_run_ticks, uint64_t up_decel_timeout_ticks, | ||
| Eigen::Vector2d& yaw_pitch_control_velocity, bool& aim_ready, Eigen::Vector2d& aim_error_px, | ||
| Eigen::Vector2d& desired_target_px_output, const cv::Point2i& target_position, | ||
| const bool& target_tracking, const rclcpp::Logger& logger, AutoAimParams auto_aim_params, | ||
| bool is_first_shot = false) | ||
| : Task("launch_preparation", "视觉辅助滑块发射准备") { | ||
|
|
||
| then( | ||
| std::make_shared<AutoAimAction>( | ||
| yaw_pitch_control_velocity, aim_ready, aim_error_px, desired_target_px_output, | ||
| target_position, target_tracking, logger, std::move(auto_aim_params))); | ||
|
|
||
| then( | ||
| std::make_shared<LaunchPreparationTask>( | ||
| belt_command, belt_target_velocity, belt_torque_limit, belt_hold_torque, | ||
| belt_wait_zero_velocity, belt_torque_offset, left_belt_angle, right_belt_angle, | ||
| left_belt_velocity, right_belt_velocity, left_belt_torque, right_belt_torque, | ||
| trigger_lock_enable, belt_down_distance, belt_pulley_radius, down_velocity, | ||
| torque_limit, up_torque_limit, down_ramp_ticks, down_torque_offset, | ||
| down_hold_torque, down_zero_velocity_threshold, down_zero_confirm_ticks, | ||
| down_ramp_timeout_ticks, require_lifting_down, lifting_command, lifting_left_vel_fb, | ||
| lifting_right_vel_fb, belt_zero_calibration, belt_up_distance, up_velocity, | ||
| up_decel_target_velocity, up_decel_torque_offset, up_stall_velocity_threshold, | ||
| up_stall_confirm_ticks, up_stall_min_run_ticks, up_decel_timeout_ticks, | ||
| is_first_shot)); | ||
| } |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# Confirm constructor signature mismatch and check for any usage of VisionAssistedLaunchPreparationTask
rg -nP --type=cpp -C2 'VisionAssistedLaunchPreparationTask'
rg -nP --type=cpp -C0 'class\s+LaunchPreparationTask\b' src/manager/task/launch_preparation_task.hpp
ast-grep --pattern 'LaunchPreparationTask($$$)'Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 3381
🏁 Script executed:
# Read the VisionAssistedLaunchPreparationTask constructor to see what's being passed
cat -n src/manager/task/vision_assisted_launch_preparation_task.hpp | head -70Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 4037
🏁 Script executed:
# Read the LaunchPreparationTask constructor signature
cat -n src/manager/task/launch_preparation_task.hpp | head -50Repository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 2923
🏁 Script executed:
# Check if LaunchPreparationTask header exists and get its full signature
fd launch_preparation_task.hppRepository: Alliance-Algorithm/rmcs_dart_guidance
Length of output: 187
LaunchPreparationTask 构造调用参数签名不匹配，将无法编译。
第 50–61 行对 LaunchPreparationTask 的调用传入了 38 个参数，但其实际构造函数签名（launch_preparation_task.hpp 第 26–40 行）仅期望 35 个参数。
传入但不期望的参数（13 个）：
left_belt_torque, right_belt_torque, belt_pulley_radius, torque_limit, up_torque_limit, down_ramp_ticks, down_torque_offset, down_hold_torque, down_zero_velocity_threshold, down_zero_confirm_ticks, down_ramp_timeout_ticks, belt_up_distance, up_velocity, up_decel_target_velocity, up_decel_torque_offset, up_stall_velocity_threshold, up_stall_confirm_ticks, up_stall_min_run_ticks, up_decel_timeout_ticks
期望但未传入的参数（12 个）：
force_screw_velocity, current_force_ch1, current_force_ch2, force_channel, target_force, enable_force_calibration, force_tolerance, force_settle_ticks, force_timeout_ticks, force_kp, force_ki, force_kd- 可选参数：
use_kalman_force, kalman_filtered_force, kalman_force_rate, kalman_rate_feedforward, kalman_rate_gain
需要修正 VisionAssistedLaunchPreparationTask 的构造参数和转发逻辑，使其与新版 LaunchPreparationTask 接口对齐。
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/manager/task/vision_assisted_launch_preparation_task.hpp` around lines 22
- 62, The LaunchPreparationTask call in VisionAssistedLaunchPreparationTask is
mismatched: remove the unexpected belt/torque/pulley parameters
(left_belt_torque, right_belt_torque, belt_pulley_radius, torque_limit,
up_torque_limit, down_ramp_ticks, down_torque_offset, down_hold_torque,
down_zero_velocity_threshold, down_zero_confirm_ticks, down_ramp_timeout_ticks,
belt_up_distance, up_velocity, up_decel_target_velocity, up_decel_torque_offset,
up_stall_velocity_threshold, up_stall_confirm_ticks, up_stall_min_run_ticks,
up_decel_timeout_ticks) and instead add and forward the force-related parameters
that LaunchPreparationTask now expects (force_screw_velocity, current_force_ch1,
current_force_ch2, force_channel, target_force, enable_force_calibration,
force_tolerance, force_settle_ticks, force_timeout_ticks, force_kp, force_ki,
force_kd and the optional use_kalman_force, kalman_filtered_force,
kalman_force_rate, kalman_rate_feedforward, kalman_rate_gain) by updating
VisionAssistedLaunchPreparationTask's parameter list and the
std::make_shared<LaunchPreparationTask>(...) call to match the exact order and
names in launch_preparation_task.hpp (use the same symbols to locate where to
add/remove arguments).
| : false; | ||
|
|
||
| // Video recording parameters | ||
| enable_video_recording_ = has_parameter("enable_video_recording"); |
There was a problem hiding this comment.
enable_video_recording_ 仅判断参数是否声明，未读取其值
与上方 enable_image_saving_（第 48-50 行）等参数读取方式不一致：此处只调用了 has_parameter("enable_video_recording")，没有进一步通过 get_parameter(...).as_bool() 读取实际值。后果是只要参数被声明（即使被设为 false），录制就会被启用；反之则永远禁用。
🐛 建议的修正
- // Video recording parameters
- enable_video_recording_ = has_parameter("enable_video_recording");
+ // Video recording parameters
+ enable_video_recording_ = has_parameter("enable_video_recording")
+ ? get_parameter("enable_video_recording").as_bool()
+ : false;📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
Suggested change
| enable_video_recording_ = has_parameter("enable_video_recording"); | |
| // Video recording parameters | |
| enable_video_recording_ = has_parameter("enable_video_recording") | |
| ? get_parameter("enable_video_recording").as_bool() | |
| : false; |
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/vision/dart_vision_core.cpp` at line 64, The assignment to
enable_video_recording_ incorrectly uses has_parameter("enable_video_recording")
instead of reading the parameter value; change it to read the declared parameter
value like enable_image_saving_ does (use
get_parameter("enable_video_recording").as_bool() or equivalent) so that
enable_video_recording_ reflects the actual boolean value passed rather than
mere presence; ensure to check has_parameter first if needed and fall back to a
default consistent with enable_image_saving_.
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
2 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.
概述
本PR实现了完整的位置控制功能，通过引入新的动作(Action)和任务(Task)系统、视觉辅助发射准备流程、视频录制和力传感器数据记录等功能，为飞镖发射机制提供精细的位置与力度控制能力。
核心改动
构建系统与插件
BUILD_TESTING)和测试依赖ament_cmake_gtest测试依赖DartManagerV2组件，新增ImageDisplayer和ForceSensorRecorder测试组件动作系统(Action)扩展
新增动作类：
修改现有动作：
timeout_returns_success参数，使超时可选返回成功或失败require_motion_before_stall参数，灵活控制失速检测逻辑fill_ticks参数，采用固定常数(200)阈值update()周期内连续执行多个动作反馈与配置管理
任务系统(Task)重构
DartManager: 从
DartManagerV2更名，大幅扩展输入/输出接口，增加传送带角度、力控制反馈等新传感器，集成零点标定和力传感器标定流程LaunchPreparationTask: 完全重构，采用新的
BeltConstantVelocityMoveAction和BeltHoldTorqueAction，支持条件化提升和力螺杆标定(兼容原始传感器和卡尔曼滤波输入)FireAndPreloadTask: 简化控制逻辑，移除旧的填充时序参数，改用
FillingLimitServoAction直接进行舵机控制CancelLaunchTask: 完善多阶段控制序列，使用恒速移动和扭矩保持动作，支持条件化提升操作
SliderInitTask: 扩展为包含零点标定和视觉偏航标定(可选)的完整初始化序列
VisionAssistedLaunchPreparationTask: 新增任务，组合自动瞄准和发射准备的两阶段流程
视觉与录制模块
测试与调试组件
新增组件：
新增单元测试：
auto_aim_action_test.cpp: 验证自动瞄准的目标跟踪、死区切换和超时行为dart_launch_sequence_test.cpp: 验证发射序列的推进和重置逻辑manual_force_control_test.cpp: 验证力控制动作的速度设置和失速检测launch_preparation_task_test.cpp/vision_assisted_launch_preparation_task_test.cpp: 任务集成测试(代码暂为注释)影响范围