moveit_servo: Fix ACM for collision checking & PSM's scene monitor topic

[JafarAbdi]

Description

This fixes two bugs:
1- The collision checking for servo is only using the initial ACM and doesn't reflect the updates PSM is receiving in the scene monitor topic
2- When using servo with move_group, servo's scene monitor should point to the move_group's one, otherwise we can't sync the planning scene between them

For the second point, I'm not sure whether we should have a parameter or always use monitored_planning_scene @AndyZe what do you think .? I think this depends on whether the PSM in servo is the primary one or not, right .?

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]

Codecov Report

Merging #673 (4a7b840) into main (812a805) will decrease coverage by 0.01%.
The diff coverage is 87.50%.

❗ Current head 4a7b840 differs from pull request most recent head f6d45c2. Consider uploading reports for the commit f6d45c2 to get more accurate results

@@            Coverage Diff             @@
##             main     #673      +/-   ##
==========================================
- Coverage   54.88%   54.87%   -0.00%     
==========================================
  Files         196      196              
  Lines       21359    21363       +4     
==========================================
+ Hits        11720    11721       +1     
- Misses       9639     9642       +3     

Impacted Files	Coverage Δ
...oveit_servo/include/moveit_servo/collision_check.h	100.00% <ø> (ø)
...veit_servo/include/moveit_servo/servo_parameters.h	75.00% <ø> (ø)
moveit_ros/moveit_servo/src/servo_node.cpp	69.39% <75.00%> (-0.17%)	⬇️
moveit_ros/moveit_servo/src/collision_check.cpp	85.25% <100.00%> (-0.23%)	⬇️
moveit_ros/moveit_servo/src/servo_parameters.cpp	70.11% <100.00%> (+0.63%)	⬆️
...rajectory_processing/src/ruckig_traj_smoothing.cpp	80.62% <0.00%> (-4.08%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	46.28% <0.00%> (-0.47%)	⬇️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	75.93% <0.00%> (+1.12%)	⬆️
...include/moveit/robot_trajectory/robot_trajectory.h	93.03% <0.00%> (+6.98%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 834f6da...f6d45c2. Read the comment docs.

[AndyZe]

I don't understand what this line does. Are you using it to wait for an update?

If so, please add a comment to that effect:

// Request a planning scene update and wait for the service response

[nbbrooks]

I think it is just to make sure the PSM was started on that topic. +1 to a comment

[AndyZe]

For the second point, I'm not sure whether we should have a parameter or always use monitored_planning_scene @AndyZe what do you think .? I think this depends on whether the PSM in servo is the primary one or not, right .?

I'm not sure I know enough about how the PSM works to answer the question.

It looks like there can be multiple update topics that each PSM listens to, judging by this function. So I would say, it would be very hard to handle all possible topic combinations.

I think we should leave it as you have it now. It's a good default. In the future, if somebody wants to do something wild like publish planning scene updates on 4 different topics, and maintain 3 separate planning scenes, we can figure it out then.

I'm open to opinions from others.

[nbbrooks]

I think using same default PSM topic is good default behavior. Supporting a separate PSM, and providing documentation, etc, on how to set that up (especially if they aren't starting at the same time and there might be missed collision object messages) sounds like a separate PR to me. I'm not aware of any MoveIt applications where separate PSMs are desired

[nbbrooks]

I think it is just to make sure the PSM was started on that topic. +1 to a comment

[henningkayser]

#713 should fix CI

[tylerjw]

Please rebase this change as I recently made some changes that affect it.

[AndyZe]

I gave this a final test with the Rolling tutorial. LGTM