Path planner overhaul

- planning now done in real world coordinates instead of occupancy grid
coordinates.
- more intelligent path planning for single-waypoint paths.