Fix traversability layer "ghost obstacles" #836
Description
With the traversability_layer costmap plugin implemented in #672, we can traverse uneven terrain by creating a costmap based on the slope of the surrounding area. However, after implementing 3D localization, an issue presented itself as the robot traversed ramps. Below are pictures illustrating the issue. When the robot is at the apex of the ramp, artifacts appear on the costmap, and thus the robot thinks that there are obstacles on the ramp though there are no obstacles.
The traversability layer uses the elevation_mapping (the rainbow box on the costmap), which is outputted from the ANYbotics elevation_mapping ROS package, to create a costmap of which areas are traversable. So, if the elevation map doesn’t say anything about an area, there shouldn’t be any obstacles on the cost map. You can find more information about the traversability layer here.
A theory for why this issue occurs is due to the robot's orientation at the top of the ramp. When the robot is at this unique orientation at the top of the ramp, the lidar can’t see the descending portion of the ramp. We need to fix this edge case hopefully have our robot traversing ramps smoothly.
AC: Robot is able to traverse ramps smoothly without any artifacts appearing in the costmap.
