Added waypoints in robotics_testlabs