Fix planning_scene_monitor sync when passed empty robot state (backport #3187)

[mergify]

Description

Fixes issue #3174.

When motion planning and executing solutions with MTC I have stages where objects are attached/removed and deleted from the scene at the beginning or end of a task. I have been having issues with the planning scene not updating correctly and the final state of the robot in the planning scene having offsets compared to the final state of trajectory given.

When executing an MTC solution that contains changes to the planning scene they are sent to the planning_scene_monitor_ with no robot_state information. When MoveIt receives this newPlanningSceneMessage that is not marked as a scene_diff (because things are being added or removed from the scene) the first thing is does is clear out the maintained diff scene_. The problem with this approach is that the planning_scene_monitor_ is only updating it's parent_scene_ at a 2Hz rate. Depending on when the trajectory finishes vs the last time the parent_scene_ was updated it could contain stale robot state information and by calling scene_->clearDiffs() we throw all of our updated robot state information away before we setPlanningSceneMsg with a message that does not contain robot_state so it uses the old values it had from the last sync.

To fix the issue this PR checks if the newPlanningSceneMessage contains a robot_state. If it does not it updates the parent_scene_ with the latest information from the current_state_monitor_ before calling setPlanningSceneMsg(scene) to ensure that the new scene is applied with the most up to date robot_state information.

---

This is an automatic backport of pull request #3187 done by Mergify.

[codecov-commenter]

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

Codecov Report

Attention: Patch coverage is 75.00000% with 1 line in your changes missing coverage. Please review.

Project coverage is 44.17%. Comparing base (3468981) to head (1bebcf4).

Files with missing lines	Patch %	Lines
...nning_scene_monitor/src/planning_scene_monitor.cpp	75.00%	1 Missing ⚠️

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

Additional details and impacted files

@@            Coverage Diff             @@
##            jazzy    #3204      +/-   ##
==========================================
+ Coverage   44.16%   44.17%   +0.01%     
==========================================
  Files         698      698              
  Lines       61572    61577       +5     
  Branches     7457     7459       +2     
==========================================
+ Hits        27188    27195       +7     
+ Misses      34215    34214       -1     
+ Partials      169      168       -1     

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