Cleaned up CostMap2D stuff.