sdu_grassbots_2014: Working on the waypoint navigation, still in beta.