Trying to get costmap to work
Before cleanup. Working version.
Cleaned up CostMap2D stuff.
Working head for MHA and ARA.