planning scene object collide with octomap

[jsupratman13]

Ideally, if an object in the planning scene overlaps the octomap, the boxes overlapping the object should be removed like the one below. This is the core feature.

However, here are some examples where the boxes still overlaps with the object even after the cut out process is done.

I believe that this is because the robot or the sensor updating the octomap cannot see the overlapped boxes at that moment; hence the boxes are not removed. Here is an example where the generated octomap that is not updated because obstacle is not in view, are not removed even if the object overlaps with the octomap

[jsupratman13]

Maybe orientation has an impact too. Need more testing