Problem with Voxel plugin: clearing logic seems to be wrong #662
Description
Hi,
something pretty weird happened to me when I try to use the Voxel Layer.
To give you an idea, please take a look to this video:
As you can see, a moving object (in this case, two cars) are moving in front of the robot and, as expected, they are detected and the local costmap is updated accordingly.
What is "wrong" is that the voxel grid is NOT cleaned up when the cars disappear. Needless to say that this prevents the local/global planner to work correctly.
After some debugging and testing I am pretty sure that I found the problem. Let me illustrate it using the following scientific representation:
The max_obstacle_height is set to 2.0 meters. All the points above this threshold are discarded.
A moving obstacle is detected below the threshold, but once the obstacle disappears, the ray will hit a wall at a Z coordinate that is above the threshold. The ray is discarded and is not used for cleaning the VoxelGrid.
Note that the same is true for min_obstacle_height. We don't want to discard clearing rays that hit the ground!
In short: the min/max obstacle layers should be applied only to marking, but we should use ALL the rays for clearing, no matter their height.