adding waypoints works

Signed-off-by: Robert C. Helling <helling@atdotde.de>
Staring to rewrite planner. Looping.

Signed-off-by: Robert C. Helling <helling@atdotde.de>