Adding waypoints to mavros test