Servo Node - pause service: check if request is different than current state.

[Jelmerdw]

Description

When using the servo_node from moveit_servo, the service call to /servo_node/pause_servo hangs for a while (sometimes even up to a minute on my machine) when requesting pause = false while the pause status was already false. This behavior can be experienced by running the demo:

ros2 launch moveit_servo demo_ros_api.launch.py

And calling the service:

ros2 service call /servo_node/pause_servo std_srvs/srv/SetBool '{data: false}'

After a while, the following error is logged:

[moveit_servo_demo_container.moveit.ros.move_group.collision_monitor]: Collision monitor could not be started

To avoid this behavior, I propose this change to first check whether the requested pause state is different compared to the current pause state. If not, the response will be returned with the message that nothing is changed since the requested pause stated was already active.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[codecov-commenter]

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

Attention: Patch coverage is 0% with 6 lines in your changes missing coverage. Please review.

Project coverage is 45.59%. Comparing base (870b23d) to head (c1391d6).
Report is 15 commits behind head on main.

Files with missing lines	Patch %	Lines
moveit_ros/moveit_servo/src/servo_node.cpp	0.00%	6 Missing ⚠️

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3265      +/-   ##
==========================================
- Coverage   45.94%   45.59%   -0.34%     
==========================================
  Files         716      716              
  Lines       62393    62379      -14     
  Branches     7543     7544       +1     
==========================================
- Hits        28659    28435     -224     
- Misses      33568    33777     +209     
- Partials      166      167       +1     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[sea-bass]

This is a good change, but also noting that #3259 was recently merged which may help with this. Checking if you have this commit locally?

Besides that, just one small comment to reduce duplication.

[Jelmerdw]

This is a good change, but also noting that #3259 was recently merged which may help with this. Checking if you have this commit locally?

Thanks for this suggestion, I didn't have that commit locally indeed. Now I get the Collision monitor could not be started error immediately, which is good enough for me.

I did apply you suggestion. Feel free to close this PR (since my actual problem was already solved by #3259), or to still merge it if you like to have this check on the servo pause state.