Much tuning, working better

Adjust costmap radii and velocity parameters.
Check for switch on disabled in servo status messages
Move navigation pointer ahead
More tuning, this looks much better.
Merge branch 'planner_tuning'