changed visualization to costmap now that occupancy grid is used