Going to try and test mapper.  Should convert costmap into a map2d for easier use.