PlanningSceneDisplay speedup

[simonschmeisser]

Alternative title: "MoveIt like it's 2020!"

This utilizes the robot_state.is_diff field in PlanningScene messages to send only joint state updates and teaches PlanningSceneDisplay to simply update the transforms in OGRE's scene graph instead of rebuilding the representation of both attached objects and collision objects. This reduces the time required for handling one update from 150-250ms down to 0.5ms in a simple scene typical for us. (one attached object (240kb), a couple more of those as collision objects, two boxes (few vertices) and an octomap) (Note that updates where the scene geometry changes are still slow ...)

AttachedObjects are now attached to the corresponding link in OGRE as well, so their position no longer needs to be updated manually.

Furthermore this increases the default rate limit for joint state updates to ca 33hz, the publishing rate to 30hz and the incoming update limit in PlanningSceneDisplay to 100hz. This allows for smooth animations at 30fps

[rhaschke]

I like most of these changes. But, not sure, we should increase the default publish frequency of the PSM from 2Hz to 30Hz.

[simonschmeisser]

Well I think people expect smooth visualizations in 2020 ...

Try adding a PlanningSceneDisplay and a RobotDisplay or TFDisplayand compare the two to know what I mean. People with live capture and constantly updated octomaps can still lower that rate if necessary, all other will simply enjoy progress ;)

[v4hn]

Try adding a PlanningSceneDisplay and a RobotDisplay or TFDisplayand compare the two to know what I mean.

I've never seen a problem with having a PlanningScene "snapshot" every 0.5 seconds and a RobotDisplay showing the latest robot state at the same time... The snapshots simply trail the RobotDisplay during motions. But I do see your motivation for the patch.

People with live capture and constantly updated octomaps can still lower that rate if necessary, all other will simply enjoy progress ;)

This is not an acceptable default if it almost surely overloads systems with sensor-based live updates of the scene. Actually, even the planning scene monitor allows to subscribe directly to /joint_states instead of getting the updates from the scene topic to avoid overloading the topic for no reason.
I would propose to either utilize this method in the PlanningSceneDisplay as a boolean property or make add an update frequency in the PlanningSceneMonitor that applies to joint changes / object position changes.
I guess the latter are also not optimized by your proposal yet?

[simonschmeisser]

RobotDisplay does not display attached objects.This PR optimizes joint position changes with attached objects so they have no extra cost (a complete rebuild of the scene on every update) any longer.

Scene rebuilds still occur if GEOMETRY_CHANGED is emitted/transmitted. I plan on adding some extra caching here as well but I have no completely straightforward solution yet. I'll do a separate PR for that therefore.

The publish rate is an upper limit btw, ie if nothing happens update rates will be lower. Also all sensor interfaces have update limits themselves, so you can still tune that to what your system can handle (but then you should maybe just reduce the update rate sensor-side).

All of that being said, I'm fine with bargaining for what we change the publish rate to ;)

(I prefer having the data go via a common path so that there are no "multiple realities" and what the display shows is what the code acts upon. That's why I'm not a fan of using the /joint_states directly in the display. Also it would add more sources of confusion)

[simonschmeisser]

How about defaulting to 10 or 15Hz? (And postponing a further increase to once CollisionObject updates have been optimized equivalently)

[v4hn]

This is not an acceptable default if it almost surely overloads systems with sensor-based live updates of the scene. Actually, even the planning scene monitor allows to subscribe directly to /joint_states instead of getting the updates from the scene topic to avoid overloading the topic for no reason.
I would propose to either utilize this method in the PlanningSceneDisplay as a boolean property or make add an update frequency in the PlanningSceneMonitor that applies to joint changes / object position changes.
I guess the latter are also not optimized by your proposal yet?

How about defaulting to 10 or 15Hz? (And postponing a further increase to once CollisionObject updates have been optimized equivalently)

Are we on a market haggling? updates with less than 30Hz might look even worse than the current version because it might look like RViz stutters. Also it's still a lot of overhead if you have to sent octomaps.

Are you unhappy with both of my proposed solutions?

[simonschmeisser]

I'm not particularily keen on consuming the joint_states directly yes, as I'm a bit afraid of the visualization diverging from the logic. Also I think there are too many options already.

For the second proposal that exists already via rate limiting your input. Octomaps are only send when they have changed. They are unfortunately currently redrawn/repopulated whenever the geometry changes (ie not when there is just a joint position changing but when changing any CollisionObject). Selectively rate limiting some part of the output could also create weird divergences (at least for some frames).

I'll drop changing the publishing rate and re-propose that once I got some input /experiments on #2079 and on octomap "rendering". Would that be an acceptable intermediate step?

it might look like RViz stutters

it doesn't now?

[v4hn]

it might look like RViz stutters

it doesn't now?

My impression was always that it is obviously throttled by program logic, but if you suspect your system is under heavy load, it might look like that too, yes...

I'll drop changing the publishing rate and re-propose that once I got some input /experiments on #2079 and on octomap "rendering". Would that be an acceptable intermediate step?

Sounds good to me. Your other changes here are a definite improvement.

[codecov-commenter]

Codecov Report

Merging #2049 into master will decrease coverage by 0.44%.
The diff coverage is 100.00%.

@@            Coverage Diff             @@
##           master    #2049      +/-   ##
==========================================
- Coverage   57.80%   57.35%   -0.45%     
==========================================
  Files         328      328              
  Lines       25663    25664       +1     
==========================================
- Hits        14834    14720     -114     
- Misses      10829    10944     +115     

Impacted Files	Coverage Δ
...nning_scene_monitor/src/planning_scene_monitor.cpp	70.10% <100.00%> (-2.06%)	⬇️
...ls/include/moveit/py_bindings_tools/gil_releaser.h	0.00% <0.00%> (-100.00%)	⬇️
.../include/moveit/py_bindings_tools/py_conversions.h	0.00% <0.00%> (-86.67%)	⬇️
...s/include/moveit/py_bindings_tools/serialize_msg.h	0.00% <0.00%> (-80.00%)	⬇️
...ove_group_interface/src/wrap_python_move_group.cpp	31.96% <0.00%> (-11.08%)	⬇️
.../move_group_interface/src/move_group_interface.cpp	44.44% <0.00%> (-3.64%)	⬇️
...anning_scene_monitor/src/current_state_monitor.cpp	51.28% <0.00%> (-1.54%)	⬇️
...meterization/work_space/pose_model_state_space.cpp	82.31% <0.00%> (-0.69%)	⬇️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 11cbc82...2fc2876. Read the comment docs.

[v4hn]

@simonschmeisser Looks good to me.
You added some quite nice standalone commits here but afterwards added fixups.
Could you squash the last three ones into their respective commits?

[simonschmeisser]

@v4hn should be clean now, sorry for the mess.

[v4hn]

Looks good to me.

Please merge via Merge commit. This request addresses a number of related but independent points.

[rhaschke]

In general, I still approve these changes. However, I'm still not sure about the increase of the update frequency:
PSM will publish with 30Hz instead of 10Hz now, and this includes the full octomap (if published at all):
https://github.com/ros-planning/moveit/pull/2049/files#diff-fb6e26ecc9a73d59dbdae3f3e08145e6R370-R377

@simonschmeisser states that the octomap is only published on changes. In principle that's true, but given some depth camera publishing at 30Hz, we effectively will have a change in each cycle...

In #2049 (comment), @simonschmeisser promised to not increase the publishing rate, but the commit is still there: 1df820b. Am I missing something?

[v4hn]

In #2049 (comment), @simonschmeisser promised to not increase the publishing rate, but the commit is still there: 1df820b. Am I missing something?

The commit you cite decreases the timer that includes update from joint_state in the monitored scene.
This is separate from this parameter which you are worried about.

[simonschmeisser]

Thanks for review and merging, I'll prepare a backport PR later on. Now if I could have your opinion on #2079 that would be great (but not sure when I get around to it though)