added logging, tidied up waypoint follower