Add trapezoidal braking and group action interface to vel-to-pos cont…#29
Merged
Merged
Conversation
…roller Replace linear braking ramp with RT-safe trapezoidal velocity profiles and add two ROS actions for coordinated multi-joint moves on top of sync_pd. Actions (hector_ros_controllers_msgs): - DriveFlipperGroup: drive all joints in a sync group to a target position - SyncFlipperGroup: drive joints in one or more groups to their group average Both auto-cancel on a non-zero velocity command for any joint in the group. Controller changes: - Trapezoidal braking profile in STOPPING state, URDF-clamped target - Velocity reference clamping to [-max_velocity, +max_velocity] - velocity_command_timeout: zero references after stale-command interval - Debug joint state pubs: atomic enable flag + pre-allocated msg fields - New params: max_velocity, max_acceleration, max_deceleration, velocity_command_timeout, upright_position - Removed: braking_deceleration (superseded by max_deceleration) Per-joint braking always runs; sync-group coordination is opt-in via the ~/sync_flipper_group action rather than automatic on every reference zero-out. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
There was a problem hiding this comment.
Pull request overview
Adds RT-safe trapezoidal motion profiles and introduces ROS2 action interfaces for coordinated multi-joint (flipper group) motion in VelocityToPositionCommandController.
Changes:
- Replace linear STOPPING ramp with per-joint trapezoidal braking profiles; add
max_velocityclamping andvelocity_command_timeout. - Add
DriveFlipperGroup/SyncFlipperGroupaction definitions and controller-side action servers + RT execution path. - Harden debug joint-state publishing by pre-allocating message fields and gating access via an atomic enable flag.
Reviewed changes
Copilot reviewed 14 out of 14 changed files in this pull request and generated 13 comments.
Show a summary per file
| File | Description |
|---|---|
| hector_ros_controllers_msgs/package.xml | Adds action_msgs dependency for new action interfaces. |
| hector_ros_controllers_msgs/CMakeLists.txt | Generates ROS interfaces for the two new actions. |
| hector_ros_controllers_msgs/action/DriveFlipperGroup.action | New action definition for driving a group to a common target. |
| hector_ros_controllers_msgs/action/SyncFlipperGroup.action | New action definition for syncing one/more groups to average positions. |
| hector_ros_controllers/include/velocity_to_position_command_controller/trapezoidal_profile.hpp | Adds header-only trapezoidal/braking profile math utilities. |
| hector_ros_controllers/include/velocity_to_position_command_controller/velocity_to_position_command_controller.hpp | Adds action infrastructure, new params/state, and braking profile storage. |
| hector_ros_controllers/src/velocity_to_position_command_controller_actions.cpp | Implements action servers, monitoring, and RT update-loop processing for actions. |
| hector_ros_controllers/src/velocity_to_position_command_controller.cpp | Integrates braking profiles, velocity clamp/timeout, action server lifecycle, debug publisher changes. |
| hector_ros_controllers/params/velocity_to_position_command_controller_parameters.yaml | Replaces braking decel param with max vel/accel/decel + timeout/upright params. |
| hector_ros_controllers/test/test_velocity_to_position_command_controller.cpp | Updates braking tests for profiles; adds tests for timeout, clamping, and action cancellation. |
| hector_ros_controllers/package.xml | Adds rclcpp_action dependency for action servers. |
| hector_ros_controllers/CMakeLists.txt | Builds and links the new actions implementation and adds test linkage. |
| README.md | Documents new features/actions/params (but currently has merge-conflict artifacts). |
| CMakeFiles/CMakeSystem.cmake | Removes a generated CMake artifact from version control. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Documentation: - Fix action file comments referencing nonexistent max_action_velocity/max_action_acceleration parameters - Correct upright_position default in README (1.517, not 3.14159265) - Resolve leftover merge-conflict markers and place Actions table in the velocity-to-position controller section Bug fixes: - Guard TrapezoidalProfile::compute against max_vel=0 to avoid division by zero producing inf/NaN profile times - Use std::abs(max_velocity_) when clamping velocity references so a negative parameter cannot violate std::clamp preconditions - Add missing <functional> and <mutex> includes that were relying on transitive headers - Fix data race on joint_position_states_ in action feedback callbacks: RT loop now publishes a snapshot via RealtimeThreadSafeBox; monitor threads read from the snapshot - Reap completed action-monitor threads at each new goal instead of letting them accumulate until on_deactivate
- Drop writeFromNonRT calls from process_group_actions; gate on the group_action_states_ atomic instead so the RT loop no longer touches the realtime buffer's mutex. - Read joint positions via rt_joint_position_snapshot_.get() in start_group_action and handle_sync_accepted to fix data races with the RT writer (earlier fix only covered feedback callbacks). - Use find() in handle_sync_accepted; abort with Unknown group instead of silently inserting empty entries via operator[]. - Pre-allocate SyncStatus message fields once in update_debug_publishers and gate publish_sync_status on debug_pubs_enabled_. - Remove the commented-out e-stop block from the update loop. - Add 19 tests (now 139 total): trapezoidal braking edge cases, group-action profile-following/completion/URDF clamping, timeout- triggers-braking, negative max_velocity safety, and a new test_velocity_to_position_command_controller_actions.cpp covering DriveFlipperGroup and SyncFlipperGroup end-to-end via rtest's ActionServerMock plus monitor-thread reaping.
- Clamp completion command to URDF limits (was unclamped on the last tick). - Roll back already-started groups when a sync goal fails partway through. - Reject overlapping goals on the same group instead of silently hijacking; defense-in-depth check in start_group_action. - Use find() in handle_drive_accepted for consistency. Tests (+5, total 144) cover all four fixes; cosmetic test cleanups. README: fix section cross-ref, accurate braking/e-stop wording, add missing sync params and ~/sync_status, document overlap rejection.
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 15 out of 15 changed files in this pull request and generated 8 comments.
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
…tics Rename the controller to better reflect its current scope (group actions, synchronization, trapezoidal braking) beyond the original vel→pos role: - Class: VelocityToPositionCommandController -> SyncGroupVelocityToPositionController - Plugin type: velocity_to_position_command_controller/* -> sync_group_velocity_to_position_controller/* - Source/header/test/param files renamed to match - Plugin description rewritten to cover the broader feature set Sync logic reworked from live offset tracking to edge-triggered capture: on the false->true transition of a group's sync state, snapshot the relative pair offset and freeze it for the duration of synced motion. Eliminates extra movement caused by FP equality flicker on chained inputs and by partner-brake transients polluting the live offset. Sync now also requires a non-zero common command, so two stopped joints no longer trivially register as synced. Fix segfault in publish_debug_joint_state_in: publishers were created in on_init() before joints_ was populated, leaving msg_.velocity sized zero. Move the initial update_debug_publishers() call to on_configure(), after read_parameters(). Drop spammy per-tick INFO logs ([BRAKE] transitions, group-action completion). Add structured comments to the params YAML grouping parameters by purpose.
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
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.
Summary
trapezoidal_profile.hpp, header-only, pure math) to theVelocityToPositionCommandController~/drive_flipper_groupand~/sync_flipper_groupmax_velocity) and a stale-command timeout (velocity_command_timeout)Why
The previous linear braking ramp produced jerky stops at high decelerations and exposed no explicit total braking time. Trapezoidal profiles give smooth, predictable braking with a known target and duration.
The two new actions are the user-facing entry points for flipper group control:
DriveFlipperGroup— drive every joint in a sync group to a single target position (defaults toupright_position) using a trapezoidal profile per joint.SyncFlipperGroup— drive each joint in one or more groups to its group's current average position. Emptygroup_namesmeans sync all groups; multiple groups can be synced in a single call.Both actions cancel immediately when any non-zero velocity command arrives for a joint in the affected group, so joystick/teleop input always wins.
Notable design choice
Per-joint braking always runs, even for joints in a sync pair. Coordinated braking that preserves a sync offset is available explicitly via
~/sync_flipper_grouprather than being triggered automatically whenever a paired joint's reference goes to zero. This keeps the brake path observable and predictable, and matches the behavior covered byIndependentBrakingWithProfiles.New parameters
max_velocity1.0max_acceleration2.0max_deceleration4.0braking_deceleration)velocity_command_timeout0.00.0disablesupright_position1.517DriveFlipperGroupwhen goal is0.0braking_decelerationis removed (replaced bymax_deceleration).max_velocity,max_acceleration,max_decelerationare dynamically reconfigurable.New action interfaces
hector_ros_controllers_msgs/action/:DriveFlipperGroup.actionSyncFlipperGroup.action