Right now, our mapping simple assigns a cost proportional to the robot-relative height of the terrain. That is, higher terrain gets a higher cost. However, we don't necessarily want higher terrain to get a high cost, but rather terrain with a steeper slope.
Right now, our mapping simple assigns a cost proportional to the robot-relative height of the terrain. That is, higher terrain gets a higher cost. However, we don't necessarily want higher terrain to get a high cost, but rather terrain with a steeper slope.
The relevant code is https://github.com/RoboJackets/urc-software/blob/master/urc_perception/src/elevation_mapping.cpp. We want to implement a new mapping function that is slope-conditioned while preserving this code. This may require the abstraction of some code (such as
inflateandgaussian) and de-coupling the mapping node and the mapping function.