it works.... not sure what changed. hopefully it's not just for this example
works well except for last waypoint it sometimes doesn't reach the goal