multiple robots/humans navigation using waypoints