sdu_weeding_trial_2013: Working on waypoint navigation