[Servo] Move enforcePositionLimits and enforceVelocityLimits to utilities

[ibrahiminfinite]

Description

This PR moves the functions enforcePositionLimits and enforceVelocityLimits into utilities from servo_calcs.cpp and enforce_limits.cpp respectively.
The dependency of some of the utility functions on rclcpp:Clock has also been removed.

Checklist

• Required by CI: Code is auto formatted using clang-format

[codecov]

Codecov Report

Patch coverage: 75.81% and project coverage change: -0.03 ⚠️

Comparison is base (15b2331) 50.55% compared to head (d983053) 50.51%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2180      +/-   ##
==========================================
- Coverage   50.55%   50.51%   -0.03%     
==========================================
  Files         387      386       -1     
  Lines       31740    31735       -5     
==========================================
- Hits        16044    16029      -15     
- Misses      15696    15706      +10     

Impacted Files	Coverage Δ
moveit_ros/moveit_servo/src/servo_calcs.cpp	71.08% <56.25%> (+0.81%)	⬆️
moveit_ros/moveit_servo/src/utilities.cpp	73.02% <82.61%> (-11.25%)	⬇️

... and 2 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

[henningkayser]

Is there a reason to not use RobotState::enforceBounds() or the alternatives for position/velocities?

[ibrahiminfinite]

Is there a reason to not use RobotState::enforceBounds() or the alternatives for position/velocities?

Probably because of this (from PR #451 )

Previously, the incoming robot state velocity was used to check if the robot would move beyond the joint limit. That was a problem if the robot state did not contain velocity data, i.e. if "copy dynamics" hadn't been turned on.
This PR uses the prospective robot velocity from Servo itself to check if the joint would move beyond the position limit and halt, if so. This velocity should always be available regardless of whether "copy dynamics" is ON.

Maybe @AndyZe can clarify more on this ?

Also @sea-bass , did you have any cases where you needed to use the override_velocity_scaling_factor parameter ?
If that is not useful, we can take that out as well.

[sea-bass]

We are currently using override_velocity_scaling_factor actively right now, so please keep it!

[ibrahiminfinite]

@AndyZe
Can you clarify on this #2180 (review)

[AndyZe]

I agree with your comment there

[ibrahiminfinite]

This PR is a cleanup PR, mostly to keep things cleaner in the current ServoCalcs and also pull out functions that can be reused into utilities so that it could be used in the upcoming refactor.

[sea-bass]

Just one comment here

[sea-bass]

Thanks for this! Found a few typos and suggestions.

[sea-bass]

Almost there! I found a bug in the log message printing.

I think the remaining CI failure might just require updating your branch to main since SonarCloud was just added in a little while earlier.

[sea-bass]

Awesome!

[ibrahiminfinite]

@sjahr @sea-bass
I think this can go in now, all tests passing

[sjahr]

@AndyZe Do you mind giving a final review?

[AndyZe]

Still looks good to me. Let's merge it