rviz is slow, probably because published planning scene diffs are not being interpreted as diffs.

[riv-mjohnson]

Description

Observed problem:

rviz constantly rebuilds the entire planning scene, rather than just updating joint states. This eats loads of cpu.

Speculative details:

The move group publishes planning scene diffs:

• When PlanningSceneMonitor::scenePublishingThread publishes diffs with new_scene_update_ == UPDATE_STATE, it strips out msg.robot_state.attached_collision_objects.
• However, it does not strip out msg.link_padding or msg.link_scale.

rviz subscribes to these planning scene diffs:

• When PlanningSceneMonitor::newPlanningSceneMessage receives a planning scene diff, it checks if attached_collision_objects, link_padding and link_scale are all empty, and if they aren't it treats the scene update type as UPDATE_SCENE.
• This triggers a complete rebuild in rviz.

Proposed solution:

Change

            if (new_scene_update_ == UPDATE_STATE)
            {
              msg.robot_state.attached_collision_objects.clear();
              msg.robot_state.is_diff = true;
            }

to

            if (new_scene_update_ == UPDATE_STATE)
            {
              msg.link_padding.clear();
              msg.link_scale.clear();
              msg.robot_state.attached_collision_objects.clear();
              msg.robot_state.is_diff = true;
            }

in PlanningSceneMonitor::scenePublishingThread.

Questions:

• Does my analysis of the problem look correct?
• Does this proposed fix look sensible?
• Are there any issues I should be aware of before making this change?

ROS Distro

Jazzy

OS and version

24.04

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

Jazzy (Technically a fork of Jazzy, but it looks like this has been an issue on trunk for years)

Which RMW are you using?

CycloneDDS

Steps to Reproduce

Put a print statement in PlanningSceneDisplay::onSceneMonitorReceivedUpdate to see what kind of update_type it is receiving.

Expected behavior

update_state should vary depending on what has changed in the planning scene.

Actual behavior

update_state is always UPDATE_SCENE.

Backtrace or Console output

No response

[riv-mjohnson]

Okay, trying this fix doesn't actually help anything, because of the issues outlined in moveit/moveit#2805.

[riv-mjohnson]

Slightly more complete itemising of causes of this problem:

• In Jazzy, current_scene_time_ is in nanoseconds, but scene_display_time_property_ is in seconds. This is fixed from Lyrical onwards.
• robot_state_needs_render_ is always false. Unclear what the desired behaviour is.
• planning_scene_needs_render_ is constantly being set to true by planning scene diffs. This is because:
  • scene.link_padding and scene.link_scale are not empty in diffs from the move group.
  • moveit_core::isEmpty(scene.world) does not return true for diffs from the move group.