Add layered costmap
Some code cleanup