[Servo] Fix stop callback, delete pause/unpause mode

[sea-bass]

Description

This PR fixes the stop callback (stopCB()) in the MoveIt Servo node implementation by actually ensuring that the servo_calcs and collision_checking sub-components are stopped (not paused) when the stop service is triggered.

[sea-bass]

Actually, I misunderstood something here. Not a bug.

[AndyZe]

Can you leave a comment so this doesn't happen again?

[sea-bass]

I un-convinced myself and have a more principled fix that actually does stop the node and the internal servo + collision checking calculations. Check this revamped version?

[codecov]

Codecov Report

Patch coverage: 53.34% and project coverage change: -0.06 ⚠️

Comparison is base (b52bb4d) 50.50% compared to head (d3d290e) 50.44%.

❗ Current head d3d290e differs from pull request most recent head 3ab3c96. Consider uploading reports for the commit 3ab3c96 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2139      +/-   ##
==========================================
- Coverage   50.50%   50.44%   -0.06%     
==========================================
  Files         387      387              
  Lines       31823    31776      -47     
==========================================
- Hits        16070    16026      -44     
+ Misses      15753    15750       -3     

Impacted Files	Coverage Δ
...ros/moveit_servo/include/moveit_servo/servo_node.h	100.00% <ø> (ø)
moveit_ros/moveit_servo/src/servo.cpp	53.85% <0.00%> (-19.48%)	⬇️
moveit_ros/moveit_servo/src/servo_node.cpp	75.00% <0.00%> (+11.77%)	⬆️
moveit_ros/moveit_servo/src/servo_calcs.cpp	64.55% <50.00%> (-0.11%)	⬇️
...oveit_servo/include/moveit_servo/collision_check.h	100.00% <100.00%> (ø)
moveit_ros/moveit_servo/src/collision_check.cpp	85.72% <100.00%> (+0.24%)	⬆️

... and 5 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.

[AndyZe]

Code looks good, must test it.

[AndyZe]

I find one issue when testing. Call /servo_node/pause_servo --> this stops all publication of commands to the controller.

Per this PR a couple days ago, paused mode should continue publishing. (I guess that behavior is debatable, though)

[sea-bass]

I find one issue when testing. Call /servo_node/pause_servo --> this stops all publication of commands to the controller.

Per this PR a couple days ago, paused mode should continue publishing. (I guess that behavior is debatable, though)

That seems like a bug in the other PR because the JTC / vector publishers are inside an if (!paused_) block:
https://github.com/ros-planning/moveit2/blob/main/moveit_ros/moveit_servo/src/servo_calcs.cpp#L471

If you want the behavior you described, it's the calls to cartesianServoCalcs and jointServoCalcs, but not the publishing, that should be wrapped in that logic.

[AndyZe]

Hmm. Can you think of any reason not to just delete the pause/unpause functionality then? It seems identical in almost every way to stop/start

[sea-bass]

Hmm. Can you think of any reason not to just delete the pause/unpause functionality then? It seems identical in almost every way to stop/start

I can't, but I don't have as much experience using Servo compared to other users. If there is no value in keeping the robot publishing (stay at current joint state) commands, then deleting pause/unpause seems good to me.

Although we might want to keep around some deprecation behavior for a bit...

[AndyZe]

There are a couple reasons to use paused/unpaused instead of stop/start:

• stop/start involves a bit more overhead. It involves filling some messages and joining/launching a thread

• paused/unpaused keeps the getEndEffectorTransform()/getCommandFrameTransform() functions active

I'd recommend to delete start/stop, keep pause/unpause`. I'm happy to make the code changes if you are busy with other stuff.

[sea-bass]

There are a couple reasons to use paused/unpaused instead of stop/start:

• stop/start involves a bit more overhead. It involves filling some messages and joining/launching a thread
• paused/unpaused keeps the getEndEffectorTransform()/getCommandFrameTransform() functions active

I'd recommend to delete start/stop, keep pause/unpause`. I'm happy to make the code changes if you are busy with other stuff.

That's fine by me -- can you make those changes? Up to you if you want to use this PR or a new one, just let me know.

[AndyZe]

On further testing, I still see some "joint jump" behavior when Servo resumes from a paused state but not from a stopped state. I'll use this same PR to delete pause/unpause tomorrow 👍 (Please check that I have write access to your fork)

[sea-bass]

No, please don't delete low-latency mode! We need it for our Gazebo config, since for other limitations we don't have the Servo node running with sim time.

The limitation is: We have a node that doesn't use the sim clock publishing commands to servo.

[AndyZe]

OK. Bummer. It makes things so complicated. Will revert it.

[henningkayser]

This looks much better to me. @AndyZe are you good with this approach?

[sea-bass]

This looks much better to me. @AndyZe are you good with this approach?

I plan to test this w Studio today, now that I'm back from vacation. If this works well we can merge

[sea-bass]

@AndyZe @henningkayser this also worked for me on Studio, so we can merge it.

Although,I had to fight through some issues in the switch to generate_parameter_library since it seems the default namespace for the params changed from moveit_servo to no namespace.

(@ibrahiminfinite FYI)

[JafarAbdi]

@AndyZe @sea-bass This PR introduced a regression, if you servo and then call the stop service and move the robot with the joint trajectory controller then start servo, the arm to move fast to the last state before the servo was stopped, reverting this PR fixes it for me

[AndyZe]

Thanks @JafarAbdi! Pinging @ibrahiminfinite too (GSOC coder)

[sea-bass]

Maybe we should open up a separate issue to discuss this -- but hopefully there is some state that needs to be unset/reset when executing ServoCalcs::stop()?

Possibly the current state and smoother, which is initialized in the constructor of ServoCalcs but maybe we should consider doing it in the start() method instead?

[ibrahiminfinite]

Possibly the current state and smoother, which is initialized in the constructor of ServoCalcs but maybe we should consider doing it in the start() method instead?

The current and previous state variables need to be updated, but they are already updated in ServoCalcs::start() here
The smoother should be fine if we call the reset() method in ServoCalcs::start() after the current state update mentioned above.

resetLowPassFilter(current_joint_state_);

The issue is possibly due to the smoother still having its state as the robot state when ServoCalcs::stop() was called while the actual state of the robot has changed now.

EDIT :

I guess the "right" way to do the filter update is by calling the resetLowPassFilter()