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.