Fix planning_scene_monitor sync when passed empty robot state

[MarqRazz]

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.

[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 45.66%. Comparing base (7390ebc) to head (2b56c75).
Report is 1 commits behind head on main.

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             @@
##             main    #3187      +/-   ##
==========================================
- Coverage   46.01%   45.66%   -0.34%     
==========================================
  Files         714      714              
  Lines       62302    62280      -22     
  Branches     7531     7530       -1     
==========================================
- Hits        28659    28433     -226     
- Misses      33477    33680     +203     
- Partials      166      167       +1     

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

[sea-bass]

Seems to make sense, though I don't know enough about MoveIt's guts to tell you if there's a better way. So I'd prefer for someone who knows more had the final approve here.

Besides my usual nitpicks in the comments, it seems like every other piece of code in this file that uses current_state_monitor_ first checks whether the pointer is not null. So would recommend also including that in your if-condition.

[sea-bass]

it seems like every other piece of code in this file that uses current_state_monitor_ first checks whether the pointer is not null. So would recommend also including that in your if-condition.

EDITED: I was also going to say we may need to check that the CSM also has a current state... the following isn't quite what you need on second thought, but maybe some of the logic can extend?

moveit2/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

Line 1500 in 06b171f

void PlanningSceneMonitor::updateSceneWithCurrentState()

[MarqRazz]

EDITED: I was also going to say we may need to check that the CSM also has a current state... the following isn't quite what you need on second thought, but maybe some of the logic can extend?

PlanningSceneMonitor::updateSceneWithCurrentState() updates the diff scene_ which is the same object that is being cleared in this section of code before setting the parent_scene_. I want to use the current_state_monitor_ because I have found issues with the way the diff scene is updated so I feel like it may contain data that is a little out of date.

I updated the logic here to first attempt to use the current_state_monitor_ and if it is not valid or does not have complete state then try to use the diff scene_ for the latest data before it is cleared.

[sea-bass]

LGTM -- would still appreciate a second look.

Also, backport to Jazzy and Humble ok here?

[MarqRazz]

Also, backport to Jazzy and Humble ok here?

👍 to Jazzy!

I don't see any issues backporting this to Humble, but FYI I did not see this sync issue when running Humble (and Iron) binaries because it is missing other updates that have been applied to the main branch.

[sjahr]

I think this makes sense but I am not sure about back porting it since this is a breaking change. Before this, sending a planning scene diff means the rest of the maintained scene is reset (world, robot state, acm ...). Now, we're not resetting robot states if the current_state monitor holds a value. @rhaschke @henningkayser Do you have any thoughts?

I am wondering though, why this is fixing your issue in MTC. Isn't this code block only executed when the scene is not a diff? All planning scene changes that are propagated from MTC to the move group are applied as diffs:
https://github.com/moveit/moveit_task_constructor/blob/325c4012f56208e37010cc85d94b4dca92f34345/capabilities/src/execute_task_solution_capability.cpp#L191
So wouldn't that mean this code is not executed in the error case you're describing?

[MarqRazz]

I am wondering though, why this is fixing your issue in MTC. Isn't this code block only executed when the scene is not a diff? All planning scene changes that are propagated from MTC to the move group are applied as diffs: https://github.com/moveit/moveit_task_constructor/blob/325c4012f56208e37010cc85d94b4dca92f34345/capabilities/src/execute_task_solution_capability.cpp#L191 So wouldn't that mean this code is not executed in the error case you're describing?

That line of code is setting scene.robot_state.is_diff which is not the same as scene.is_diff. From my testing it appears that some operations like adding or deleting objects causes the solution to send a complete scene (not a diff).

[sjahr]

I am wondering though, why this is fixing your issue in MTC. Isn't this code block only executed when the scene is not a diff? All planning scene changes that are propagated from MTC to the move group are applied as diffs: https://github.com/moveit/moveit_task_constructor/blob/325c4012f56208e37010cc85d94b4dca92f34345/capabilities/src/execute_task_solution_capability.cpp#L191 So wouldn't that mean this code is not executed in the error case you're describing?

That line of code is setting scene.robot_state.is_diff which is not the same as scene.is_diff. From my testing it appears that some operations like adding or deleting objects causes the solution to send a complete scene (not a diff).

Oh you're right sorry. I did not look carefully enough. The field in the message is explicitly called scene_diff, we should make sure that is always the case. Do you think that is relatively easy to fix based on your investigation so far

[MarqRazz]

The field in the message is explicitly called scene_diff, we should make sure that is always the case. Do you think that is relatively easy to fix based on your investigation so far

Sorry I'm not following, I don't know of a message parameter called scene_diff. This section of code is only executed if the incoming scene is not a diff and we have a valid parent_scene_

if (!scene.is_diff && parent_scene_)

[sjahr]

I am talking the MTC trajectory execution action: https://github.com/moveit/moveit_task_constructor/blob/master/msgs/action/ExecuteTaskSolution.action
It contains a task solution to be executed and that task solution contains the stage solution each containing a trajectory and/or a planning scene diff (by name). So I am wondering why MTC sends non diff scenes 🤔

The reason I am bringing this up is that while this patch fixes your problem, I don't think it necessarily fixes the root cause in MTC.

[MarqRazz]

Sorry for the confusion, that makes sense.

The reason I think this change is still applicable is it that someone could call newPlanningSceneMessage with a scene message that does not contain a robot state (as you can see this is always true with MTC). If that is the case then we should use the most up to date information for robot state when we applying the new scene. The prior approach clears the diff scene without applying it and replaces the parent_scene which means it could be applied with stale data.

[sjahr]

You're right, looking into setCurrentState() and the functions it calls, it will re-use the probably outdated parent scene to set the robot state rather than the most recent value. Thanks for your patience and explanations. Let's get this in. Do you mind opening an issue in MTC about the planning scene not always being a diff?

[rhaschke]

Do you mind opening an issue in MTC about the planning scene not always being a diff?

Sorry for being late to the party. Originally, MTC was sending diffs only - therefore the message member scene_diff.
However, when planning "backwards", this failed. In these cases a full scene is sent: moveit/moveit_task_constructor#432.

[rhaschke]

Why didn't you simply use this line only? All you wanted to achieve is to not loose RobotState information from existing planning scene diffs.

[MarqRazz]

When I was testing I found that the diff scene is not updated at a constant rate and wanted to ensure I used the most up to date information. I can make this a single line if you think making the diff scene update run at a fixed rate is worthwhile.

[MarqRazz]

If we go to the single line would it be better to push the diffs to the parent scene? Could there be other changes in the diff that might need to be applied before we replace the scene?

[rhaschke]

We need to deal with both issues separately.
This PR was about correctly maintaining the RobotState from the diffs. If the planning scene didn't have newer updates from the state monitor yet, we shouldn't care to pull them in early.

That the scene is not updated at a constant rate is another issue. Updates to the PSM's scene are only performed if 1) there are new updates received by CSM and 2) the throttling frequency is met. If both was true in your tests, then their is an issue with those updates, which should get fixed.
As far as I understood, you were observing issues with rviz' PlanningScene. This is updated from move_group's (PSM's) planning scene via a separate topic at a low rate of 2Hz:

moveit2/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

Line 448 in 80c7f61

void PlanningSceneMonitor::scenePublishingThread()

[rhaschke]

I suggest to cleanup this PR as I did for the MoveIt1 backport (moveit/moveit#3683).

[MarqRazz]

We need to deal with both issues separately.

That is what I though too which is why I did not include any of those changes here. I will work on investigating the update rate and try to include a test to ensure it is working as expected.

As far as I understood, you were observing issues with rviz' PlanningScene. This is updated from move_group's (PSM's) planning scene via a separate topic at a low rate of 2Hz:

Yes this issues is visible in Rviz but is also effects future plans because the parent_scene contained stale robot state data after executing a move causing it to jump to the last known position on the next motion plan (shown in the plot below).

[rhaschke]

Sure. This is fixed with the present PR. However, the other issue, i.e. the scene not being updated regularly, is a different story.

[sjahr]

Do you mind opening an issue in MTC about the planning scene not always being a diff?

Sorry for being late to the party. Originally, MTC was sending diffs only - therefore the message member scene_diff. However, when planning "backwards", this failed. In these cases a full scene is sent: moveit/moveit_task_constructor#432.

That's good to know, thanks!