[PSM] Add proccess Collision Object to PSM and request planning scene to moveit py to allow syncing of mutliple PSM

[JensVanhooydonck]

Add proccess collision object and proccess attached collision object to PlanningSceneMonitor and add request planning scene to moveit py to allow syncing of multiple planning scene monitors.

Description

When altering a(n) (attached-)collision object in the planning scene by using the applyCollisionObject or processAttachedCollisionObject function on the planning scene object, this does not trigger an updated in the PSM.

When using the same logic in PSM we can alter a collsiion object directly, and sync this to other planning scene monitors.

To make this complete I also added the request planning scene function to the MoveitPy interface

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

Attention: 24 lines in your changes are missing coverage. Please review.

Comparison is base (c2292a7) 50.89% compared to head (a71b56c) 50.87%.
Report is 1 commits behind head on main.

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

Files	Patch %	Lines
...nning_scene_monitor/src/planning_scene_monitor.cpp	0.00%	24 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2536      +/-   ##
==========================================
- Coverage   50.89%   50.87%   -0.01%     
==========================================
  Files         388      388              
  Lines       32245    32247       +2     
==========================================
- Hits        16408    16403       -5     
- Misses      15837    15844       +7     

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

[JensVanhooydonck]

Known issues when trying to sync multiple planning scene:

• When using an acm, this does not allow for diff planning scene messages and triggers a full planning scene each time. Which results in an infinity loop when subscribing to a master PSM. [PSM] When using an ACM, Diff planning scene messages are not possible and always trigger a full scene update #2541

[peterdavidfagan]

These changes look good to me as they enable a more expressive Python API.

@henningkayser are you ok with the changes made to the planning_scene_monitor cpp code? They should be mostly inconsequential from what I can see, as they simply split a callback into a method which is called by the callback and the original callback (changes to collision and attached collision object callbacks on the psm). The purpose of this is to create a method for processing collision objects directly in the Python API essentially exposing the callback functionality in the binded Python method.

[sjahr]

Thanks for your contribution @JensVanhooydonck! Just a small comment and then I think this is good to go

[sjahr]

I think it would be even nicer to completely get rid of the callback member functions and just call the new function with a lambda when the services are initialized

[JensVanhooydonck]

Shall I just remove the callback functions and change the call of these functions from

  collision_object_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::CollisionObject>(
        collision_objects_topic, rclcpp::SystemDefaultsQoS(),
        [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return collisionObjectCallback(obj); });
    RCLCPP_INFO(logger_, "Listening to '%s'", collision_objects_topic.c_str());

to

  collision_object_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::CollisionObject>(
        collision_objects_topic, rclcpp::SystemDefaultsQoS(),
        [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return processCollisionObjectMsg(obj); });
    RCLCPP_INFO(logger_, "Listening to '%s'", collision_objects_topic.c_str());

And the same for the AttachedCollisionObjects?

These functions do not seem to be used somewhere else. I've added this to make sure there would not be any breaking changes.

[sjahr]

Yes 👍 They aren't used anywhere else and if they only call another function we don't need them. I think the lambda needs be

-   [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return processCollisionObjectMsg(obj); }
+   [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { processCollisionObjectMsg(obj); }

because the create_subscription expects a function argument that returns void but just use whatever compiles

[JensVanhooydonck]

Changed it