Make moveit_servo listen to Octomap updates

[amalnanavati]

Description

As explained in #2548, the current moveit_servo does not respect Octomap collisions. Responding to that, this PR makes the following one-line changes:

1. [Addressing the issue] Start the world geometry monitor when initializing the servo node.

(Instead of enabling the world geometry monitor by default, I'm happy to change it to being controlled by a boolean parameter. Maintainers, let me know if you'd prefer that.)

Testing

I have tested this code on our local robot setup in ROS2 Humble, where a move_group node is the primary planning scene monitor, and a servo_node subscribes to its updates over the "monitored_planning_scene" topic. As expected, with these changes the robot treats octomap collisions as any other collision in the planning scene, and without these changes it ignores octomap collisions.

Importantly, I have not tested the code in cases where the servo_node is the primary planning scene monitor. I imagine it should work as long as developers pass in the save octomap and sensors_3d.yaml configuration parameters as they would to a move_group with an octomap.

Checklist

• Required by CI: Code is auto formatted using clang-format
• [N/A] 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
• [N/A] 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

[mergify]

Please target the main branch for development, we will backport the changes to humble for you if approved and if they don't break API.

[sjahr]

Thanks! Please address the comment above

Please target the main branch for development, we will backport the changes to humble for you if approved and if they don't break API.

This looks fine to me. @AndyZe is there any reason not to activate the WorldGeometryMonitor?

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Comparison is base (8d2eb90) 50.33% compared to head (660ffa3) 50.73%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2601      +/-   ##
==========================================
+ Coverage   50.33%   50.73%   +0.41%     
==========================================
  Files         386      386              
  Lines       32086    32087       +1     
==========================================
+ Hits        16147    16276     +129     
+ Misses      15939    15811     -128     

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

[amalnanavati]

Done; I rebased onto main. Note that I haven't been able to test the version on main (since I don't use ROS2 Rolling), but given that it is a one-line change and worked on Humble, and you both have looked it over, I imagine it should be fine.

Thanks for the prompt review!

[sjahr]

Thanks for your contribution!

[amalnanavati]

@sjahr any idea what might be causing humble-ci-testing to fail? Looking at the logs, it seems like ros2_control_node had a segmentation fault, although my change should not impact that.

The only error relevant to my change is that no sensor is configured for the Octomap (log here) although I believe that is a non-fatal error.

The only pointer I have for differences between Humble and Rolling is that in Humble, the correct line is planning_scene_monitor_->startWorldGeometryMonitor(); whereas in Rolling it is planning_scene_monitor->startWorldGeometryMonitor(); (no underscore), which may impact the automatic backport.

[sjahr]

I don't think that error is related to your change since it occurred on main too https://github.com/ros-planning/moveit2/actions/runs/7198483518/job/19608037907.