Nicer waypoint definition
Merge branch 'refactor/sergio+ed' of https://github.com/tue-robotics/robot_smach_states into refactor/sergio+ed