Calculation for new path should start some time before the current waypoint is reached

[oswinso]

Right now, our path planning to the next waypoint doesn't happen until the current waypoint is reached. Ideally, planning to the next waypoint should happen when we are some distance away from the current waypoint, so that the robot never stops waiting for planning to happen.

This might require changing the architecture of planning a bit (moving to actionlib or services for planning or something).

Requires #423 to be finished first.