Octomap Update Delay - Voxel debris

[pawelir]

Description

When adding a collision object to the planning scene, the octomap voxels that collide with this object are not cleared instantaneously.

Extensive description
https://github.com/orgs/moveit/discussions/3613

ROS Distro

Humble

OS and version

Ubuntu 22.04

Source or binary build?

Binary

If binary, which release version?

2.5.9

If source, which branch?

No response

Which RMW are you using?

CycloneDDS

Steps to Reproduce

1. Use point cloud to create octomap
2. Add collision object to the planning scene, object should be positioned among voxels representing collisions
3. Observe octomap updates

PS. Corresponding sensor_3d config linked in the discussion

Expected behavior

Voxels colliding with newly added collision object should be removed with first octomap update

Actual behavior

Not all voxels are cleared instantaneously

• Most of them are cleared right away
• To see remaining voxels removed, it usually takes a couple of seconds

Backtrace or Console output

No response

[nbbrooks]

We have observed slow robot model filtering in the octomap with high resolution pointcloud sources as well. I do not have specific metrics, but I do not believe your situation is unique.

Until a MoveIt contributor has bandwidth to investigate, a potential workaround would be to decrease the octomap resolution.

[pawelir]

Hi @nbbrooks, thanks. Just to confirm: in my case, it's not about robot body filtration, but rather the object we want to grasp (a CollisionObject added to the planning scene). Do you think the cause is the same?

Also, if you have any ideas to investigate off the top of your head, I'd appreciate you sharing them.