def test_waypoint_speed(self): with SimConnection(60) as sim: state = lgsvl.AgentState() state.transform = sim.map_point_on_lane( lgsvl.Vector(78.962, -3.363, -40.292)) sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) forward = lgsvl.utils.transform_to_forward(state.transform) up = lgsvl.utils.transform_to_up(state.transform) state.transform.position = state.position + 10 * forward npc = sim.add_agent("Hatchback", lgsvl.AgentType.NPC, state) waypoints = [] layer_mask = 0 layer_mask |= 1 << 0 hit = sim.raycast(state.position + 10 * forward + 50 * up, lgsvl.Vector(0, -1, 0), layer_mask) waypoints.append( lgsvl.DriveWaypoint( hit.point, 5, lgsvl.Vector(0, 0, 0), 0, 0)) # this waypoint to allow NPC to get up to speed hit = sim.raycast(state.position + 30 * forward + 50 * up, lgsvl.Vector(0, -1, 0), layer_mask) waypoints.append( lgsvl.DriveWaypoint(hit.point, 5, lgsvl.Vector(0, 0, 0), 0, 0)) def on_waypoint(agent, index): sim.stop() npc.on_waypoint_reached(on_waypoint) npc.follow(waypoints) sim.run() t0 = time.time() sim.run() t1 = time.time() waypoints = [] hit = sim.raycast(state.position + 40 * forward + 50 * up, lgsvl.Vector(0, -1, 0), layer_mask) waypoints.append( lgsvl.DriveWaypoint( hit.point, 20, lgsvl.Vector(0, 0, 0), 0, 0)) # this waypoint to allow NPC to get up to speed hit = sim.raycast(state.position + 120 * forward + 50 * up, lgsvl.Vector(0, -1, 0), layer_mask) waypoints.append( lgsvl.DriveWaypoint(hit.point, 20, lgsvl.Vector(0, 0, 0), 0, 0)) npc.follow(waypoints) sim.run() t2 = time.time() sim.run() t3 = time.time() lowSpeed = t1 - t0 highSpeed = t3 - t2 self.assertAlmostEqual(lowSpeed, highSpeed, delta=0.5) self.assertAlmostEqual(lowSpeed, 4.5, delta=0.5) self.assertAlmostEqual(highSpeed, 4.5, delta=0.5)
def add_line_loop_car_with_underground_trigger(car_type, spawn_id, trigger_offset, offsets, trigger_distance, delay, angle_offset, speed, no_loop=False): """ offset is (forward, right) 1. wait underground of trigger_offsets to be triggered. 2. visit offsets[0] -> offsets[n-1] accordingly 3. back to the offsets[0] through the underground of offsets[n-1] -> that of offset[0] 4. play again from 2 """ # not redefine spawns rotation = spawns[spawn_id].rotation angle = copy.deepcopy(rotation) angle.y += angle_offset speed_invisible = 100 underground_offset = 3 up = lgsvl.utils.transform_to_up(spawns[spawn_id]) underground = [ lgsvl.DriveWaypoint( get_ground_point(spawn_id, trigger_offset) - underground_offset * up, speed_invisible, angle, 0, 0, trigger_distance), lgsvl.DriveWaypoint( get_ground_point(spawn_id, offsets[0]) - underground_offset * up, speed_invisible, angle, delay), ] forward = [ lgsvl.DriveWaypoint(get_ground_point(spawn_id, offset), speed, angle, 0) for offset in offsets ] backward = [ lgsvl.DriveWaypoint( get_ground_point(spawn_id, offsets[-1]) - underground_offset * up, speed_invisible, angle, 0), lgsvl.DriveWaypoint( get_ground_point(spawn_id, offsets[0]) - underground_offset * up, speed_invisible, angle, 0), ] if no_loop: waypoints = underground + forward + backward * 100 else: loop = forward + backward N = 50 # the number of loop iteration waypoints = underground + loop * N state = lgsvl.AgentState() state.transform.position = get_ground_point( spawn_id, trigger_offset) - underground_offset * up state.transform.rotation = angle npc = sim.add_agent(car_type, lgsvl.AgentType.NPC, state) npc.follow(waypoints, loop=False)
def test_high_waypoint( self): # Check that a NPC will drive to under a high waypoint with SimConnection(15) as sim: state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) right = lgsvl.utils.transform_to_right(state.transform) up = lgsvl.utils.transform_to_up(state.transform) state.transform.position = state.position - 5 * right sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) spawns = sim.get_spawn() agent = self.create_NPC(sim, "Sedan") px = 4 pz = 12 py = 50 speed = 6 destination = spawns[ 0].position + px * right + py * up + pz * forward wp = [lgsvl.DriveWaypoint(destination, speed)] def on_waypoint(agent, index): sim.stop() agent.on_waypoint_reached(on_waypoint) agent.follow(wp) sim.run(10) self.assertLess((agent.state.position - destination).magnitude(), 1)
def turn(): waypoints = [] x_max = 2 z_delta = 12 layer_mask = 0 layer_mask |= 1 << 0 # 0 is the layer for the road (default) for i in range(20): speed = 24# if i % 2 == 0 else 12 px = 0 pz = (i + 1) * z_delta # Waypoint angles are input as Euler angles (roll, pitch, yaw) angle = spawns[0].rotation # Raycast the points onto the ground because BorregasAve is not flat hit = sim.raycast(spawns[0].position + pz * forward, lgsvl.Vector(0,-1,0), layer_mask) # NPC will wait for 1 second at each waypoint wp = lgsvl.DriveWaypoint(hit.point, speed, angle, 1) waypoints.append(wp) # When the NPC is within 0.5m of the waypoint, this will be called def on_waypoint(agent, index): print("waypoint {} reached".format(index)) # The above function needs to be added to the list of callbacks for the NPC ego.on_waypoint_reached(on_waypoint) # The NPC needs to be given the list of waypoints. # A bool can be passed as the 2nd argument that controls whether or not the NPC loops over the waypoints (default false) ego.follow(waypoints)
def test_high_waypoint( self): # Check that a NPC will drive to under a high waypoint try: with SimConnection(15) as sim: state = spawnState(sim) state.position.x += 10 sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, state) spawns = sim.get_spawn() sx = spawns[0].position.x sy = spawns[0].position.y sz = spawns[0].position.z agent = self.create_NPC(sim, "Sedan") px = 12 pz = 4 py = 5 speed = 6 wp = [ lgsvl.DriveWaypoint( lgsvl.Vector(sx - px, sy + py, sz - pz), speed) ] def on_waypoint(agent, index): raise TestException("Waypoint reached?") agent.on_waypoint_reached(on_waypoint) agent.follow(wp) sim.run(10) except TestException as e: self.assertNotIn("Waypoint reached?", repr(e))
def evaluate(self): # Setup vehicles vehicles = self.setup_vehicles(self.simConnection, self.ego_speed) self.sedan = vehicles["sedan"] self.ego = vehicles["ego"] # Delay the simulator for 2 seconds self.simConnection.execute(timeout=2) self.sedan.on_collision(self.on_collision) self.ego.on_collision(self.on_collision) # Start the scenario waypoints = [] for point in self.npc_waypoints: waypoints.append( lgsvl.DriveWaypoint(point, self.npc_speed, self.sedan.state.transform.rotation)) self.sedan.follow(waypoints) self.simConnection.execute(timeout=15) print( f'Congratulation! A speed helps ago car avoiding crash is: {self.ego_speed} m/s' ) # Close the simulator self.simConnection.sim.close()
def run(self): # Setup environment lgsvl_sim = self.simConnection.connect() control = lgsvl.NPCControl() ego_control = lgsvl.VehicleControl() # Placing the school_bus school_bus_state = spawn_state(lgsvl_sim) school_bus_state = CarControl.place_car_on_the_point( state=school_bus_state, sim=lgsvl_sim, point=self.npc_source) school_bus = load_npc(lgsvl_sim, "SchoolBus", school_bus_state) # Placing the ego on the starting point ego_state = spawn_state(lgsvl_sim) ego_state = CarControl.place_car_from_the_point(dimension="horizontal", distance=-6, state=ego_state) ego_state = CarControl.drive_ego_car(ego_state, [("vertical", self.ego_speed)]) ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state) # Callback collision function ego.on_collision(self.on_collision) school_bus.on_collision(self.on_collision) # Set waypoints for School Bus waypoints = [] for point in [self.npc_source, self.npc_target]: waypoints.append( lgsvl.DriveWaypoint(point, self.npc_speed, school_bus.state.transform.rotation)) try: # Start the scenario # The School Bus is parked on the street control.headlights = 2 control.e_stop = True school_bus.apply_control(control) # Let the ego running for 2 seconds self.simConnection.execute(timeout=2) # The school bus turns on signal to prepare for the turn control.headlights = 0 # turn off headlight control.turn_signal_left = True school_bus.apply_control(control) self.simConnection.execute(timeout=2) # Brake the ego CarControl.brake_ego(ego=ego, control=ego_control, brake_value=self.ego_brake, sticky=True) # The school bus starts to turn right school_bus.follow(waypoints) self.simConnection.execute(timeout=10) except Exception: print("Failed!")
def sample_waypoint(self): '''sample a waypoint that depends on self.z_position''' self.y_position += 0.2 position = lgsvl.Vector(13.81, self.y_position, self.z_position) waypoint = lgsvl.DriveWaypoint(position=position, angle=lgsvl.Vector(0, 180, 0), speed=self.npc_speed) return waypoint
def test_waypoint_speed(self): with SimConnection(60) as sim: sim.add_agent("XE_Rigged-apollo_3_5", lgsvl.AgentType.EGO, spawnState(sim, 1)) npc = self.create_NPC(sim, "HatchBack") npcX = npc.state.position.x npcY = npc.state.position.y npcZ = npc.state.position.z waypoints = [] waypoints.append( lgsvl.DriveWaypoint( lgsvl.Vector(npcX - 10, npcY, npcZ), 5)) # this waypoint to allow NPC to get up to speed waypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(npcX - 30, npcY, npcZ), 5)) def on_waypoint(agent, index): sim.stop() npc.on_waypoint_reached(on_waypoint) npc.follow(waypoints) sim.run() t0 = time.time() sim.run() t1 = time.time() waypoints = [] waypoints.append( lgsvl.DriveWaypoint( lgsvl.Vector(npcX - 40, npcY, npcZ), 20)) # this waypoint to allow NPC to get up to speed waypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(npcX - 120, npcY, npcZ), 20)) npc.follow(waypoints) sim.run() t2 = time.time() sim.run() t3 = time.time() lowSpeed = t1 - t0 highSpeed = t3 - t2 self.assertAlmostEqual(lowSpeed, highSpeed, delta=0.5) self.assertAlmostEqual(lowSpeed, 4, delta=0.5) self.assertAlmostEqual(highSpeed, 4, delta=0.5)
def get_npc_event(sim, npc, way_vecs, speeds): waypoints = [] for i, vec in enumerate(way_vecs): on_lane_vec = sim.map_point_on_lane(vec).position wp = lgsvl.DriveWaypoint(on_lane_vec, speeds[i]) waypoints.append(wp) def npc_event_func(): npc.follow(waypoints) npc_event = Event(func=npc_event_func, params=None, only_once=True) return npc_event
def generate_NPCs(self): # Encroaching Vehicle in opposite lane npcState = lgsvl.AgentState() x = self.EGO_start.x y = self.EGO_start.y z = self.EGO_start.z ##### NPC1 ##### npcState.transform = self.sim.map_point_on_lane( lgsvl.Vector(x + 7, y, z)) self.npc1 = self.sim.add_agent("Sedan", lgsvl.AgentType.NPC, npcState) npc1Speed = 5 n1x = self.npc1.state.position.x n1y = self.npc1.state.position.y n1z = self.npc1.state.position.z self.npc1Waypoints = [] self.npc1Waypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(n1x - 12, n1y, n1z + 0.5), npc1Speed)) self.npc1Waypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(n1x - 14, n1y, n1z + 2.5), npc1Speed)) self.npc1Waypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(n1x - 16, n1y, n1z + 6), npc1Speed)) self.npc1Waypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(n1x - 18, n1y, n1z + 9), npc1Speed)) self.npc1Waypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(n1x - 20, n1y, n1z + 13), npc1Speed)) ##### NPC2 ##### npcState.transform = self.sim.map_point_on_lane( lgsvl.Vector(x - INITIAL_HEADWAY, y, 1.0)) npcState.transform.rotation = lgsvl.Vector(0, 90, 0) self.npc2 = self.sim.add_agent("Jeep", lgsvl.AgentType.NPC, npcState) npc2Speed = 12 n2x = self.npc2.state.position.x n2y = self.npc2.state.position.y n2z = self.npc2.state.position.z self.npc2Waypoints = [] self.npc2Waypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(n2x + 50, n2y, n2z), npc2Speed - 5)) # ready for left turn self.npc2Waypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(n2x + 65, n2y, n2z + 30), npc2Speed))
def __init__(self, waypoints): self.waypoints = tuple(waypoints) if not isinstance(self.waypoints[0], lgsvl.DriveWaypoint): pts = [] for wp in self.waypoints: elev = veneer.simulation().groundElevationAt(wp.position) pos = utils.scenicToLGSVLPosition(wp.position, y=elev) rot = utils.scenicToLGSVLRotation(wp.heading) pt = lgsvl.DriveWaypoint(pos, wp.speed, rot) pts.append(pt) self.waypoints = tuple(pts) self.lastTime = -2
def get_npc_event(sim, npc): waypoints_vec = [] waypoints_vec.append(Vector(46.3, 0, -103)) # Setup waypoints for the NPC to follow speed = 30 waypoints = [] for v in waypoints_vec: waypoints.append(lgsvl.DriveWaypoint(v, speed)) # This function will be triggered def npc_event_func(): npc.follow(waypoints) npc_event = saze.Event(func=npc_event_func, params=None, only_once=True) return npc_event
def sample_uniform_waypoints(start, end, n): '''Uniformly sample n waypoints from start to end ''' z_locations = np.random.uniform(low=start, high=end, size=n) z_locations.sort() z_locations = np.flip(z_locations) y_position = -3.15 u_waypoints = [] for i in range(n): if i < 5: y_position += 0.28 else: y_position += 0.08 position = lgsvl.Vector(13.81, y_position, z_locations[i]) waypoint = lgsvl.DriveWaypoint(position=position, angle=lgsvl.Vector(0, 180, 0), speed=0) u_waypoints.append(waypoint) return u_waypoints
def test_follow_waypoints(self): # Check that the NPC can follow waypoints with SimConnection(60) as sim: state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) right = lgsvl.utils.transform_to_right(state.transform) state.transform.position = state.position - 5 * right sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) spawns = sim.get_spawn() agent = self.create_NPC(sim, "Sedan") # snake-drive layer_mask = 0 layer_mask |= 1 << 0 waypointCommands = [] waypoints = [] x_max = 4 z_delta = 12 for i in range(5): speed = 6 if i % 2 == 0 else 12 pz = (i + 1) * z_delta px = x_max * (-1 if i % 2 == 0 else 1) hit = sim.raycast( spawns[0].position + pz * forward + px * right, lgsvl.Vector(0, -1, 0), layer_mask) wp = lgsvl.DriveWaypoint(hit.point, speed) waypointCommands.append(wp) waypoints.append(hit.point) def on_waypoint(agent, index): msg = "Waypoint " + str(index) mEqual(self, agent.state.position, waypoints[index], msg) if index == len(waypoints) - 1: sim.stop() agent.on_waypoint_reached(on_waypoint) agent.follow(waypointCommands) sim.run()
def evaluate(self): # Setup vehicles vehicles = self.setup_vehicles(self.simConnection, self.ego_speed, self.npc_speed) self.school_bus = vehicles["school_bus"] self.ego = vehicles["ego"] self.school_bus.on_collision(self.on_collision) self.ego.on_collision(self.on_collision) # Start the scenario waypoints = [] for point in self.npc_waypoints: waypoints.append(lgsvl.DriveWaypoint(point, self.npc_speed, self.school_bus.state.transform.rotation)) self.school_bus.follow_closest_lane(follow=True, max_speed=10) self.simConnection.execute(timeout=2) self.school_bus.follow(waypoints) self.simConnection.execute(timeout=15) print(f'Congratulation! A speed helps ago car avoiding crash is: {self.ego_speed} m/s') # Close the simulator self.simConnection.sim.close()
def waypoint_collector(pos, rot, buffer_time): #spawn the npc vehicle on intersection state = lgsvl.AgentState() state.transform.position = pos state.transform.rotation = rot npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) #creating a list of waypoint for the npc vehicle waypoints = [] npc.follow_closest_lane(True, SPEED) sim.run(0.5) #waypoint collector for i in range(20): tf = npc.transform wp = lgsvl.DriveWaypoint(tf.position, speed=SPEED, angle=tf.rotation, idle=0) waypoints.append(wp) sim.run(buffer_time) sim.remove_agent(npc) return waypoints
def test_follow_waypoints(self): # Check that the NPC can follow waypoints with SimConnection(60) as sim: state = spawnState(sim) state.position.x += 10 sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, state) spawns = sim.get_spawn() sx = spawns[0].position.x sy = spawns[0].position.y sz = spawns[0].position.z agent = self.create_NPC(sim, "Sedan") # snake-drive waypointCommands = [] waypoints = [] z_max = 4 x_delta = 12 for i in range(5): speed = 6 if i % 2 == 0 else 12 px = (i + 1) * x_delta pz = z_max * (-1 if i % 2 == 0 else 1) wp = lgsvl.DriveWaypoint(lgsvl.Vector(sx - px, sy, sz - pz), speed) waypointCommands.append(wp) waypoints.append(lgsvl.Vector(sx - px, sy, sz - pz)) def on_waypoint(agent, index): msg = "Waypoint " + str(index) mEqual(self, agent.state.position, waypoints[index], msg) if index == len(waypoints) - 1: sim.stop() agent.on_waypoint_reached(on_waypoint) agent.follow(waypointCommands) sim.run()
.format(modules)) destination = egoState.position + 135 * forward dv.setup_apollo(destination.x, destination.z, modules) finalPOVWaypointPosition = egoState.position - 2.15 * right POVState = lgsvl.AgentState() POVState.transform.position = egoState.position + ( 4.5 + INITIAL_HEADWAY) * forward - 2.15 * right POVState.transform.rotation = lgsvl.Vector(0, -180, 0) POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) POVWaypoints = [] POVWaypoints.append( lgsvl.DriveWaypoint(POVState.transform.position, MAX_POV_SPEED, POVState.transform.rotation)) POVWaypoints.append( lgsvl.DriveWaypoint(finalPOVWaypointPosition, 0, POVState.transform.rotation)) def on_collision(agent1, agent2, contact): raise lgsvl.evaluator.TestException("Ego collided with {}".format(agent2)) ego.on_collision(on_collision) POV.on_collision(on_collision) try: t0 = time.time() sim.run(TIME_DELAY)
# Waypoint angles are input as Euler angles (roll, pitch, yaw) angle = npc_rotation # Raycast the points onto the ground because BorregasAve is not flat npc_position.x += 6 npc_position.z += 21 hit = sim.raycast(npc_position, lgsvl.Vector(0, -1, 0), layer_mask) trigger = None if i == 0: parameters = {"max_distance": 4.0} effector = lgsvl.TriggerEffector("WaitForDistance", parameters) trigger = lgsvl.WaypointTrigger([effector]) wp = lgsvl.DriveWaypoint( position=hit.point, speed=speed, angle=angle, idle=0, trigger_distance=0, trigger=trigger, ) waypoints.append(wp) def on_waypoint(agent, index): print("waypoint {} reached".format(index)) def agents_traversed_waypoints(): print("All agents traversed their waypoints.") sim.stop()
speed_range = (5 / 3.6, 40 / 3.6) acc_range = (5, 10) for i in range(-4, 10): acc = random.uniform(*acc_range) if 4 * (speed**2) + 8 * acc < 0: end_speed = 0 else: t = (-2 * speed + (4 * (speed**2) + 4 * acc)**0.5) / (2 * acc) end_speed = speed + acc * t if end_speed < speed_range[0]: end_speed = speed_range[0] elif end_speed > speed_range[1]: end_speed = speed_range[1] speed = end_speed waypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(i, 0, 18.8 - i * 2.69), speed, lgsvl.Vector(0, 159, 0), 0, True, 0)) acc_range = (-10, -5) for i in range(10, 24): acc = random.uniform(*acc_range) if 4 * (speed**2) + 8 * acc < 0: end_speed = 0 else: t = (-2 * speed + (4 * (speed**2) + 4 * acc)**0.5) / (2 * acc) end_speed = speed + acc * t if end_speed < speed_range[0]: end_speed = speed_range[0] elif end_speed > speed_range[1]: end_speed = speed_range[1] speed = end_speed waypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(i, 0, 18.8 - i * 2.69), speed,
def test_EGO_encroach_schoolbus_speed_6_with_apollo(self): LGSVL__APOLLO_HOST = config("LGSVL__APOLLO_HOST") LGSVL__APOLLO_PORT = int(config("LGSVL__APOLLO_PORT")) LGSVL__DREAMVIEW_HOST = config("LGSVL__DREAMVIEW_HOST") MODULES = [ 'Recorder', 'Localization', 'Perception', 'Transform', 'Routing', 'Prediction', 'Planning', 'Traffic Light', 'Control' ] TARGET = Vector(3.03013730049133, -0.00637590885162354, -16.5673313140869) COLLISIONS = [] SEDAN_WPS = [ Vector(-3.31942486763, -0.0809718370437622, 24.2049713134766), Vector(-3.84999561309814, -0.00320455431938171, 14.877103805542), Vector(3.84999561309814, -0.00320455431938171, -20.877103805542) ] NPC_SPEED = 6 sim_connection = SimConnection(scene="CubeTown") lgsvl_sim = sim_connection.connect() # Placing the school_bus school_bus_state = spawn_state(lgsvl_sim) school_bus = load_npc(lgsvl_sim, "SchoolBus", school_bus_state) # Placing the ego on the starting point ego_state = spawn_state(lgsvl_sim) ego_state = CarControl.place_car_from_the_point(dimension="horizontal", distance=-6, state=ego_state) # ego_state = CarControl.drive_ego_car(ego_state, [("vertical", 6)]) ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state) def on_collision(agent1, agent2, contact): COLLISIONS.append([agent1, agent2, contact]) sim_connection.sim.close() print("Exception: {} collided with {}".format(agent1, agent2)) ego.on_collision(on_collision) school_bus.on_collision(on_collision) try: ego.connect_bridge(LGSVL__APOLLO_HOST, LGSVL__APOLLO_PORT) dv = lgsvl.dreamview.Connection(lgsvl_sim, ego, LGSVL__DREAMVIEW_HOST) dv.set_hd_map("CubeTown") dv.set_vehicle("Lincoln2017MKZ (Apollo 5.0)") dv.setup_apollo(TARGET.x, TARGET.z, MODULES) # Start the scenario school_bus.follow_closest_lane(follow=True, max_speed=10) waypoints = [] for point in SEDAN_WPS: waypoints.append( lgsvl.DriveWaypoint(point, NPC_SPEED, school_bus.state.transform.rotation)) school_bus.follow(waypoints) sim_connection.execute(timeout=15) except Exception: sim_connection.sim.close() self.fail("Failed!") sim_connection.sim.close() self.assertTrue(True, True)
def test_EGO_exit_park_with_incoming_NPC(self): NPC_START = lgsvl.geometry.Vector(120, -0.0120140314102173, -2) NPC_TARGET = lgsvl.geometry.Vector(60, -0.0121138095855713, -2) NPC_SPEED = 4 VEHICLE_SET = [ { "name": "Sedan", "load_vehicle": load_npc, "distance": 0, "type": "Sedan" }, { "name": "Ego", "load_vehicle": load_ego, "distance": 10, "type": "Lincoln2017MKZ (Apollo 5.0)" }, { "name": "SUV", "load_vehicle": load_npc, "distance": 5, "type": "SUV" }, { "name": "Jeep", "load_vehicle": load_npc, "distance": 20, "type": "Jeep" }, ] # Setup environment sim_connection = SimConnection() lgsvl_sim = sim_connection.connect() scenario = Scenario(sim_connection) # Setup vehicles position sedan, ego, suv, jeep = scenario.generate_vehicles( lgsvl_sim, VEHICLE_SET) # Setup a new NPC NPC_state = spawn_state(lgsvl_sim) NPC_state = CarControl.place_car_on_the_point(sim=lgsvl_sim, point=NPC_START, state=NPC_state) NPC_state = CarControl.rotate_car_by_degree(state=NPC_state, degree=-90) NPC = load_npc(lgsvl_sim, "Sedan", NPC_state) NPC.on_collision(scenario.on_collision) waypointsCommand = [ lgsvl.DriveWaypoint(NPC_START, NPC_SPEED, NPC_state.transform.rotation), lgsvl.DriveWaypoint(NPC_TARGET, NPC_SPEED, NPC_state.transform.rotation) ] # Delay the scenario for 2s sim_connection.execute(timeout=2) try: NPC.follow(waypointsCommand) scenario.drive_ego(sim_connection, ego) except Exception: sim_connection.sim.close() self.fail("Failed!") # Passed! sim_connection.sim.close() self.assertTrue(True, True)
np.round(distance / relative_speed, 3)) + " seconds" else: if distance != 0: ttc = npc.uid.split("(Clone)")[0] + " is moving away from EGO" else: ttc = npc.uid.split("(Clone)")[0] + " " + str( np.round(distance / relative_speed, 3)) + " seconds" print(ttc) return distance, ttc ###################### NPC waypoints ###################### waypoints = [ \ lgsvl.DriveWaypoint(position=lgsvl.Vector(13.81, -3.15, 63.50), angle=lgsvl.Vector(0, 180, 0), speed=10), lgsvl.DriveWaypoint(position=lgsvl.Vector(13.81, -2.2, 17), angle=lgsvl.Vector(0, 180, 0), speed=10), lgsvl.DriveWaypoint(position=lgsvl.Vector(13.81, -1.12, -46.505), angle=lgsvl.Vector(0, 180, 0), speed=10), ] def on_waypoint(agent, index): print("=======") print("waypoint {} reached".format(index))
sim.stop() controlReceived = True ego.on_custom(on_control_received) # Run Simulator for at most 30 seconds for the AD stack to to initialize sim.run(30) # If a Control message was not received, then the AD stack is not ready and the scenario should not continue if not controlReceived: raise Exception("AD stack is not ready") sys.exit() # Create the list of waypoints for the npc to follow waypoints = [] # First waypoint is the same position that the npc is spawned. The npc will wait here until the EGO is within 50m waypoints.append(lgsvl.DriveWaypoint(position=npcState.position, speed=20.1168, angle=npcState.rotation, idle=0, deactivate=False, trigger_distance=35)) # Second waypoint is across the intersection endPoint = sim.map_point_on_lane(egoState.position + 51.1 * forward - 63.4 * right) waypoints.append(lgsvl.DriveWaypoint(position=endPoint.position, speed=0, angle=endPoint.rotation)) npc.follow(waypoints) # Set all the traffic lights to green. # It is possible to set only the lights visible by the EGO to green, but positions of the lights must be known signals = sim.get_controllables("signal") for signal in signals: signal.control("green=3") # Run the simulation for 30 seconds sim.run(30)
py = 0.5 for i in range(75): speed = 6 px = 0 py = ((i + 1) * 0.05) if (i < 50) else (50 * 0.05 + (50 - i) * 0.05) pz = i * z_delta * 0.05 angle = lgsvl.Vector(i, 0, 3 * i) # Raycast the points onto the ground because BorregasAve is not flat hit = sim.raycast(spawns[0].position + px * right + pz * forward, lgsvl.Vector(0, -1, 0), layer_mask) wp = lgsvl.DriveWaypoint( spawns[0].position + px * right + py * up + pz * forward, speed, angle, 0, 0) waypoints.append(wp) # When the NPC is within 1m of the waypoint, this will be called def on_waypoint(agent, index): print("waypoint {} reached".format(index)) # The above function needs to be added to the list of callbacks for the NPC npc.on_waypoint_reached(on_waypoint) # The NPC needs to be given the list of waypoints. # A bool can be passed as the 2nd argument that controls whether or not the NPC loops over the waypoints (default false) npc.follow(waypoints)
ego = sim.add_agent("Jaguar2015XE (Apollo 5.0)", lgsvl.AgentType.EGO, egoState) ego.connect_bridge(os.environ.get("BRIDGE_HOST", "127.0.0.1"), 9090) finalPOVWaypointPosition = lgsvl.Vector(0, 0, -125) POVState = lgsvl.AgentState() POVState.transform.position = lgsvl.Vector( finalPOVWaypointPosition.x, finalPOVWaypointPosition.y, finalPOVWaypointPosition.z + 4.5 + INITIAL_HEADWAY) POVState.transform.rotation = lgsvl.Vector(0, 180, 0) POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) POVWaypoints = [] POVWaypoints.append( lgsvl.DriveWaypoint(finalPOVWaypointPosition, MAX_POV_SPEED)) def on_collision(agent1, agent2, contact): raise evaluator.TestException("Ego collided with {}".format(agent2)) ego.on_collision(on_collision) POV.on_collision(on_collision) try: t0 = time.time() sim.run(TIME_DELAY) POV.follow(POVWaypoints) while True:
layer_mask = 0 layer_mask |= 1 << 0 # 0 is the layer for the road (default) for i in range(20): speed = 24 # if i % 2 == 0 else 12 px = 0 pz = (i + 1) * z_delta # Waypoint angles are input as Euler angles (roll, pitch, yaw) angle = spawns[0].rotation # Raycast the points onto the ground because BorregasAve is not flat hit = sim.raycast(spawns[0].position + pz * forward, lgsvl.Vector(0, -1, 0), layer_mask) # NPC will wait for 1 second at each waypoint wp = lgsvl.DriveWaypoint(hit.point, speed, angle, 1) waypoints.append(wp) # When the NPC is within 0.5m of the waypoint, this will be called def on_waypoint(agent, index): print("waypoint {} reached".format(index)) # The above function needs to be added to the list of callbacks for the NPC npc.on_waypoint_reached(on_waypoint) # The NPC needs to be given the list of waypoints. # A bool can be passed as the 2nd argument that controls whether or not the NPC loops over the waypoints (default false) npc.follow(waypoints)
if s.name in ['velodyne', 'Main Camera', 'Telephoto Camera', 'GPS', 'IMU']: s.enabled = True ego.connect_bridge(os.environ.get("BRIDGE_HOST", "127.0.0.1"), 9090) POVState = lgsvl.AgentState() POVState.transform.position = lgsvl.Vector() POVState.transform.rotation = lgsvl.Vector() POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) POVWaypoints = [] laneX = -89.17999 # If the following waypoints are turned into a lane in Unity editor, this is the Transform Position of the lane laneY = 0.15 laneZ = -85.25668 POVWaypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(laneX - 10.854, laneY, laneZ + 53.2), MAX_POV_SPEED)) POVWaypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(laneX - 10.854, laneY, laneZ + 45.4), MAX_POV_SPEED)) POVWaypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(laneX - 10.854, laneY, laneZ + 37.3), MAX_POV_SPEED)) POVWaypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(laneX - 10.854, laneY, laneZ + 30.484), MAX_POV_SPEED)) POVWaypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(laneX - 10.764, laneY, laneZ + 23.5), MAX_POV_SPEED)) POVWaypoints.append( lgsvl.DriveWaypoint(lgsvl.Vector(laneX - 10.527, laneY, laneZ + 18.2), MAX_POV_SPEED))
state.transform = spawns[0] state.transform.position.x = sx state.transform.position.x = sz npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) # snake-drive waypoints = [] z_max = 4 x_delta = 12 for i in range(20): speed = 6 if i % 2 == 0 else 12 px = (i + 1) * x_delta pz = z_max * (-1 if i % 2 == 0 else 1) wp = lgsvl.DriveWaypoint(lgsvl.Vector(sx - px, sy, sz - pz), speed) waypoints.append(wp) def on_waypoint(agent, index): print("waypoint {} reached".format(index)) npc.on_waypoint_reached(on_waypoint) npc.follow(waypoints) input("press enter to run") sim.run()