Move group PS scene update is not correctly received/updated by Rviz2

[Danilrivero]

Description

This issue is directly related to an issue I posted a while ago related to the octomap which used the PS: #2797

A comment from rhaschke here in this issue moveit/moveit#3380 perfectly explains the behaviour that is happening:

Looks like the scene update is not correctly received by rviz. In the move_group's PS, the octomap is correctly cleared. This can be easily verified by disabling and re-enabling the display in rviz, which re-creates everything.

This is to have traceability for a backport of the fixes made here: moveit/moveit#3380, moveit/moveit#3342, moveit/moveit#3045, moveit/moveit#439 in MoveIt1, these changes are required to be made in MoveIt2.

ROS Distro

Humble

OS and version

Ubuntu 22.04

Source or binary build?

Binary

If binary, which release version?

2.5.5.1

If source, which branch?

No response

Which RMW are you using?

CycloneDDS

Steps to Reproduce

If a PlanningScene message is published on the monitored_planning_scene topic with an empty msg.world.octomap.octomap.data field, the octomap is skipped altogether in if it is a msg.is_diff is True. An empty data field is interpreted as the octomap field not being filled. But this makes it impossible to clear the octomap by sending an empty octomap message (for example via the "Clear octomap" button in the MotionPlanning panel in Rviz2).

When calling the /clear_octomap service the module should clear the octomap even though it would presumably update within the next update if the pointcloud is being published. This also happens when trying to update the Octomap without clearing it beforehand.

This can be verified by disabling and re-enabling the display in Rviz2, which re-creates everything and shows the updated PS.

Expected behavior

When calling the "/clear_octomap" service the module should clear the octomap even though it would presumably update within the next update if the pointcloud is being published. This also happens when trying to update the Octomap.

Actual behavior

The module does not update the octomap unless being updated with the publishing of a new pointcloud forcing the update.

Backtrace or Console output

Direct fix is made here moveit/moveit#3134 & moveit/moveit#3385

Backtrace to issues fixed in MoveIt1 that would be fixed here: moveit/moveit#3134 (comment)

[Danilrivero]

I do not have the bandwidth to port these changes right now, opening this issue to let it be known in case anyone is willing.