Updating rviz planning scene when monitored planning scene is locked

[rr-mark]

Description

General description:

I'm not sure if we're configuring moveit and rviz slightly wrong, or if this is an actual problem.

We are setting up rviz with Planning Scene Topic: /move_group/monitored_planning_scene to display the robot.

We are planning and executing a series of motions, with planning and executing happening in parallel.

When a motion is executing while another motion is planning, the rviz visual of the robot freezes. This happens both when using a real robot, or using a simulated robot entirely run by moveit.

Technical details:

I have tracked the problem back to PlanningSceneMonitor::scenePublishingThread, where the scene_update_mutex_ is locked while planning is happening. This blocks the publishing of the planning scene on /move_group/monitored_planning_scene.

Possible solutions:

• Are we just hooking rviz up to the wrong planning scene? Is there a planning scene topic which is continuously updated, even when the monitored planning scene is locked?
• If not, is the right solution to change how the planning scene monitor works, to have one planning scene which is continually updated, and a second planning scene which is locked when planning?

Your environment

• ROS Distro: Noetic
• OS Version: e.g. Ubuntu 22.04
• Source or Binary build? Source
• If binary, which release version?
• If source, which branch? 1.1.16-b71e5f5ce5a9b3080dbcbb8d2e5d19cf5010e613

Steps to reproduce

Plan and execute a series of motions, where the planning for subsequent motions happens while previous motions are executing.

Expected behaviour

The rviz visualisation of the robot should follow the executing motion.

Actual behaviour

The rviz visualisation freezes when a motion starts planning.

Backtrace or Console output

Use gist.github.com to copy-paste the console output or segfault backtrace using gdb.

[rr-mark]

As ever, I'm keen to make a PR to fix this issue, if someone could point me in the right direction for how to approach it.

[rhaschke]

I have tracked the problem back to PlanningSceneMonitor::scenePublishingThread, where the scene_update_mutex_ is locked while planning is happening.

Wasn't that fixed via #3676 and #3682?

[rr-mark]

Unfortunately not - those PRs fix the current state monitor waiting on the locked planning scene, but they don't fix the planning scene publisher waiting on the locked planning scene.

So motions can now execute while planning - the robot moves, the joint state and robot state topics update, and the tf display in rviz updates - but the displayed robot in rviz doesn't update.

It feels like PlanningSceneMonitor::scenePublishingThread should be separable in the same way the current state monitor is, but as far as I can tell it's doing a fair bit more than just publishing the scene.

[rr-mark]

I wonder if it would be sensible to have the current state monitor publishing its own planning scene, separate from the one monitored by the planning scene monitor?

[rhaschke]

Unfortunately not - those PRs fix the current state monitor waiting on the locked planning scene, but they don't fix the planning scene publishing waiting on the locked planning scene.

I see. I think, planning should not lock the PSM's scene for the whole duration of planning. Rather, it just needs to compute the current scene as a new local variable, which is then used for planning, while the PSM's scene can be updated during planning.

To this end, you need to find all locations, where the PSM's scene is locked and fix the code to keep the locking time as short as possible. For example, here (but there might be more locations):

moveit/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp

Line 156 in b9ed835

planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);

moveit/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp

Line 209 in b9ed835

planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_);

[rr-mark]

That makes sense, I'll give it a go.

Thanks again.

[rhaschke]

If there are numerous locations following the same locking scheme, it might be useful to introduce a utility function:

planning_scene::PlanningScenePtr getPlanningScene(const PlanningSceneMonitorPtr& psm, const moveit_msgs::PlanningScene& diff)
{
  planning_scene_monitor::LockedPlanningSceneRO lscene(psm);
  PlanningScenePtr scene = lscene->diff();
  lscene.unlock();
  
  if (!moveit::core::isEmpty(diff))
    scene->setPlanningSceneDiffMsg(diff);
  return scene;
}