Waypoint navigation
Merge branch 'master' of https://github.com/RSD2014-Group-5/FroboMind