def addNpcAtFixedPoints(sim, spawn_idx: int): spawns = sim.get_spawn() state = lgsvl.AgentState() forward = lgsvl.utils.transform_to_forward(spawns[0]) right = lgsvl.utils.transform_to_right(spawns[0]) # 10 meters ahead, on left lane state.transform.position = spawns[0].position + 10.0 * forward state.transform.rotation = spawns[0].rotation npc1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) state = lgsvl.AgentState() # 10 meters ahead, on right lane state.transform.position = spawns[0].position + \ 4.0 * right + 10.0 * forward state.transform.rotation = spawns[0].rotation npc2 = sim.add_agent("SUV", lgsvl.AgentType.NPC, state, lgsvl.Vector(1, 1, 0)) # If the passed bool is False, then the NPC will not moved # The float passed is the maximum speed the NPC will drive # 11.1 m/s is ~40 km/h npc1.follow_closest_lane(True, 11.1) # 5.6 m/s is ~20 km/h npc2.follow_closest_lane(True, 5.6)
def addNpcRandomAtlanes(sim, no_of_npc: int, spawn_idx: int, tf: Transform, isDrive: bool): spawns = sim.get_spawn() state = lgsvl.AgentState() spawn = tf sx = spawn.position.x sy = spawn.position.y sz = spawn.position.z mindist = 5.0 maxdist = 100.0 random.seed() for i in range(no_of_npc): # Creates a random point around the EGO angle = random.uniform(0.0, 2*math.pi) dist = random.uniform(mindist, maxdist) point = lgsvl.Vector(sx + dist * math.cos(angle), sy, sz + dist * math.sin(angle)) # Creates an NPC on a lane that is closest to the random point state = lgsvl.AgentState() state.transform = sim.map_point_on_lane(point) new_npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) new_npc.follow_closest_lane(isDrive, 6)
def set_environment(self): '''start simulator, spawn EGO and NPC''' ###################### simulator ###################### if self.sim.current_scene == "BorregasAve": self.sim.reset() else: self.sim.load("BorregasAve") ###################### EGO ###################### spawns = self.sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[0] self.ego = self.sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) # sensors = self.ego.get_sensors() # c = lgsvl.VehicleControl() # c.turn_signal_left = True # self.ego.apply_control(c, True) ###################### NPC ###################### sx = spawns[0].position.x - 8 sy = spawns[0].position.y sz = spawns[0].position.z + 100 state = lgsvl.AgentState() state.transform = spawns[0] state.transform.position.x = sx state.transform.position.z = sz state.transform.rotation.y = 180.0 self.npc = self.sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) self.npc.on_waypoint_reached(self.on_waypoint) self.vehicles = { self.ego: "EGO", self.npc: "Sedan", } self.ego.on_collision(self.on_collision) self.npc.on_collision(self.on_collision) ###################### Traffic light is kept green ###################### controllables = self.sim.get_controllables() # Pick a traffic light of intrest signal = controllables[2] # Get current controllable states # print("\n# Current control policy:") # print(signal.control_policy) # Create a new control policy control_policy = "trigger=100;green=100;yellow=0;red=0;loop" # Control this traffic light with a new control policy signal.control(control_policy) self.z_position = sz
def test_lane_change_left(self): with SimConnection(40) as sim: state = lgsvl.AgentState() state.transform = sim.map_point_on_lane( lgsvl.Vector(4.49, -1.57, 40.85)) npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state) state.transform = sim.map_point_on_lane( lgsvl.Vector(9.82, -1.79, 42.02)) sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) forward = lgsvl.utils.transform_to_forward(state.transform) target = state.position + 31 * forward npc.follow_closest_lane(True, 10) agents = [] def on_lane_change(agent): agents.append(agent) npc.on_lane_change(on_lane_change) sim.run(2) npc.change_lane(True) sim.run(3) self.assertTrue(npc == agents[0]) self.assertAlmostEqual((npc.state.position - target).magnitude(), 0, delta=2)
def test_lane_change_right_missing_lane(self): with SimConnection(40) as sim: state = lgsvl.AgentState() state.transform = sim.map_point_on_lane( lgsvl.Vector(0.63, -1.57, 42.73)) npc = sim.add_agent("Hatchback", lgsvl.AgentType.NPC, state) forward = lgsvl.utils.transform_to_forward(state.transform) target = state.position + 42.75 * forward state.transform = sim.map_point_on_lane( lgsvl.Vector(4.49, -1.57, 40.85)) sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) npc.follow_closest_lane(True, 10) agents = [] def on_lane_change(agent): agents.append(agent) npc.on_lane_change(on_lane_change) sim.run(2) npc.change_lane(False) sim.run(3) self.assertTrue(len(agents) == 0) self.assertAlmostEqual((npc.state.position - target).magnitude(), 0, delta=2)
def test_lane_change_left_opposing_traffic(self): with SimConnection(40) as sim: state = lgsvl.AgentState() state.transform = sim.map_point_on_lane( lgsvl.Vector(78.962, -3.363, -40.292)) npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state) forward = lgsvl.utils.transform_to_forward(state.transform) target = state.position + 42.75 * forward state.transform.position = state.position - 10 * forward sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) npc.follow_closest_lane(True, 10) agents = [] def on_lane_change(agent): agents.append(agent) npc.on_lane_change(on_lane_change) sim.run(2) npc.change_lane(True) sim.run(3) self.assertTrue(len(agents) == 0) self.assertAlmostEqual((npc.state.position - target).magnitude(), 0, delta=2)
def createObjectInSimulator(self, obj): # Figure out what type of LGSVL object this is if not hasattr(obj, 'lgsvlName'): raise RuntimeError(f'object {obj} does not have an lgsvlName property') if not hasattr(obj, 'lgsvlAgentType'): raise RuntimeError(f'object {obj} does not have an lgsvlAgentType property') name = obj.lgsvlName agentType = obj.lgsvlAgentType # Set up position and orientation state = lgsvl.AgentState() elevation = obj.elevation if elevation is None: elevation = self.groundElevationAt(obj.position) state.transform.position = utils.scenicToLGSVLPosition(obj.position, elevation) state.transform.rotation = utils.scenicToLGSVLRotation(obj.heading) # Create LGSVL object lgsvlObj = self.client.add_agent(name, agentType, state) obj.lgsvlObject = lgsvlObj # Initialize Data self.data[obj] = {} # Initialize Apollo if needed if getattr(obj, 'apolloVehicle', None): self.initApolloFor(obj, lgsvlObj)
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 initScene(sim, sim_map: str, ego_car_model: str, ego_state: ObjectState, ros_bridge_ip: str, ros_bridge_port: int, spawn_idx: int) -> None: if sim.current_scene == sim_map: sim.reset() else: sim.load(sim_map) spawns = sim.get_spawn() default_state = lgsvl.AgentState() default_state.transform = spawns[spawn_idx] forward = lgsvl.utils.transform_to_forward(spawns[spawn_idx]) right = lgsvl.utils.transform_to_right(spawns[spawn_idx]) default_state.transform.position += 0 * right ego = sim.add_agent(ego_car_model, lgsvl.AgentType.EGO, default_state) ego.connect_bridge(os.environ.get( "BRIDGE_HOST", ros_bridge_ip), ros_bridge_port) sensors = ego.get_sensors() for s in sensors: print(type(s), s.enabled) print("Current time = ", sim.current_time) print("Current frame = ", sim.current_frame)
def add_pedestrian_with_trigger(agent_type, offset, timing): i = 0 spawns = sim.get_spawn() forward = lgsvl.utils.transform_to_forward(spawns[i]) right = lgsvl.utils.transform_to_right(spawns[i]) up = lgsvl.utils.transform_to_up(spawns[i]) wp = [ lgsvl.WalkWaypoint( project(spawns[0].position + 5 * right + offset * forward + 2 * up), 0, 100), lgsvl.WalkWaypoint( project(spawns[0].position + 5 * right + offset * forward + 2 * up), timing, 0), lgsvl.WalkWaypoint( project(spawns[0].position - 5 * right + offset * forward + 2 * up), 0), lgsvl.WalkWaypoint( project(spawns[0].position - 11 * right + offset * forward + 2 * up), 0), ] state = lgsvl.AgentState() state.transform = copy.deepcopy(spawns[0]) state.transform.position = copy.deepcopy(wp[0].position) p = sim.add_agent(agent_type, lgsvl.AgentType.PEDESTRIAN, state) p.follow(wp, False)
def addNpcVehicle(self, posVector, vehicleType="SUV"): sim = self.sim npcList = self.npcList npcState = lgsvl.AgentState() npcState.transform = sim.map_point_on_lane(posVector) npc = sim.add_agent(vehicleType, lgsvl.AgentType.NPC, npcState) npcList.append(npc)
def test_lane_change_right(self): with SimConnection(40) as sim: state = lgsvl.AgentState() state.transform = sim.map_point_on_lane( lgsvl.Vector(40.85, -1.57, -4.49)) npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state) state.transform = sim.map_point_on_lane( lgsvl.Vector(42.73, -1.57, -0.63)) sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) forward = lgsvl.utils.transform_to_forward(state.transform) target = state.position + 31 * forward npc.follow_closest_lane(True, 10) agents = [] def on_lane_change(agent): agents.append(agent) npc.on_lane_change(on_lane_change) sim.run(2) npc.change_lane(False) sim.run(3) self.assertTrue(npc == agents[0]) self.assertAlmostEqual((npc.state.position - target).magnitude(), 0, delta=2)
def __init__(self, sim_host_ip: str, sim_host_port: int, sim_map: str, ego_car_model: str, ros_bridge_ip: str, ros_bridge_port: int, time_limit: float, time_scale: float) -> None: # initialise simulator and global params self.sim = lgsvl.Simulator( os.environ.get("SIMULATOR_HOST", sim_host_ip), sim_host_port) self.sim.load(sim_map) self.ego_car_model = ego_car_model self.ros_bridge_ip = ros_bridge_ip self.ros_bridge_port = ros_bridge_port self.time_limit = time_limit self.time_scale = time_scale # set default ego state self.default_ego_state = lgsvl.AgentState() self.default_ego_state.transform = self.sim.get_spawn()[0] self.ego_state = copy.deepcopy(self.default_ego_state) # set global direction based on map self.forward_dir_map = lgsvl.utils.transform_to_forward( self.default_ego_state.transform) self.right_dir_map = lgsvl.utils.transform_to_right( self.default_ego_state.transform) # set pedestrians and npc types self.pedestrians_names = [ "Bob", "EntrepreneurFemale", "Howard", "Johny", "Pamela", "Presley", "Robin", "Stephen", "Zoe" ] self.npc_types = ['Sedan', 'SUV', 'Jeep', 'Hatchback']
def test_AAA_NPC_no_scene(self): with SimConnection(load_scene=False) as sim: with self.assertRaises(Exception) as e: state = lgsvl.AgentState() agent = sim.add_agent("Jeep", lgsvl.AgentType.NPC, state) agent.state.position self.assertFalse(repr(e.exception).startswith(PROBLEM))
def test_GPS(self): # Check that the GPS sensor works with SimConnection() as sim: state = lgsvl.AgentState() state.transform = sim.get_spawn()[0] right = lgsvl.utils.transform_to_right(state.transform) state.velocity = -50 * right agent = sim.add_agent( lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) sensors = agent.get_sensors() initialGPSData = None gps = None for s in sensors: if s.name == "GPS": gps = s initialGPSData = gps.data sim.run(1) finalGPSData = gps.data latChanged = notAlmostEqual(initialGPSData.latitude, finalGPSData.latitude) lonChanged = notAlmostEqual(initialGPSData.longitude, finalGPSData.longitude) northingChanged = notAlmostEqual(initialGPSData.northing, finalGPSData.northing) eastingChanged = notAlmostEqual(initialGPSData.easting, finalGPSData.easting) self.assertTrue(latChanged or lonChanged) self.assertTrue(northingChanged or eastingChanged) self.assertNotAlmostEqual(gps.data.latitude, 0) self.assertNotAlmostEqual(gps.data.longitude, 0) self.assertNotAlmostEqual(gps.data.northing, 0) self.assertNotAlmostEqual(gps.data.easting, 0) self.assertNotAlmostEqual(gps.data.altitude, 0) self.assertNotAlmostEqual(gps.data.orientation, 0)
def set_env(self, env): # Set the map MAP = env.get("map") if self.sim.current_scene == MAP: self.sim.reset() self.log.debug(f"Reset the map: {MAP}") else: self.sim.load(MAP) self.log.debug(f"Loaded the map: {MAP}") # Set the time self.sim.set_time_of_day(env.get("time")) self.log.debug(f"Set the time to {env.get('time')}") # Set the vehicle vehicleState = lgsvl.AgentState() vehicleState.transform = self.sim.get_spawn()[0] self.vehicle = self.sim.add_agent(env.get("car"), lgsvl.AgentType.EGO, vehicleState) self.log.debug("Created an EGO vehicle: {env.get('car')}") self.vehiclePos = self.vehicle.state.position self.vehicle.connect_bridge(self.config.get("BRIDGE_HOST"), self.config.get("BRIDGE_PORT")) self.log.debug("Connected the vehicle to the bridge: {self.config.get('BRIDGE_HOST')}:{self.config.get('BRIDGE_PORT')}") self.vehicle.on_collision(self._on_collision) self.log.debug("Connected a collision callback")
def waypoint_follow_test(waypoints, pos, rot): state = lgsvl.AgentState() state.transform.position = pos state.transform.rotation = rot npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) npc.follow(waypoints) sim.run(10)
def test_GPS(self): with SimConnection() as sim: state = lgsvl.AgentState() state.transform = sim.get_spawn()[0] state.velocity = lgsvl.Vector(-50, 0, 0) agent = sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, state) sensors = agent.get_sensors() initialGPSData = None gps = None for s in sensors: if s.name == "GPS": gps = s initialGPSData = gps.data sim.run(1) finalGPSData = gps.data latChanged = notAlmostEqual(initialGPSData.latitude, finalGPSData.latitude) lonChanged = notAlmostEqual(initialGPSData.longitude, finalGPSData.longitude) northingChanged = notAlmostEqual(initialGPSData.northing, finalGPSData.northing) eastingChanged = notAlmostEqual(initialGPSData.easting, finalGPSData.easting) self.assertTrue(latChanged or lonChanged) self.assertTrue(northingChanged or eastingChanged) self.assertNotAlmostEqual(gps.data.latitude, 0) self.assertNotAlmostEqual(gps.data.longitude, 0) self.assertNotAlmostEqual(gps.data.northing, 0) self.assertNotAlmostEqual(gps.data.easting, 0) self.assertNotAlmostEqual(gps.data.altitude, 0) self.assertNotAlmostEqual(gps.data.orientation, 0)
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 main(): sim_host_ip = "127.0.0.1" # of your local machine running sim sim_host_port = 8181 sim_map = "CubeTown" ego_car_model = "Jaguar2015XE (Autoware)" # Lexus2016RXHybrid (Autoware) ros_bridge_ip = "192.168.1.78" # of the ros machine that runs the websocket_server ros_bridge_port = 9090 time_limit = 0 # 0 means unlimited time otherwise sim will produce this much data in virtual seconds time_scale = 1 # 1 means normal speed, 1.5 means 1.5x speed ego_pos = Vector(-22, 0, 59.9) ego_roll = 0 ego_pitch = 0 ego_yaw = 250 ego_rot = Vector(ego_pitch, ego_yaw, ego_roll) ego_tf = Transform(ego_pos, ego_rot) ego_v = Vector(0, 0, 0) ego_ang_v = Vector(0, 0, 0) ego_state = ObjectState(ego_tf, ego_v, ego_ang_v) no_of_npc = 0 spawn_idx = 0 spawn_pos = Vector(-32.6, 0, 45) spawn_tf = Transform(spawn_pos) sim = lgsvl.Simulator(os.environ.get( "SIMULATOR_HOST", sim_host_ip), sim_host_port) initScene(sim, sim_map, ego_car_model, ego_state, ros_bridge_ip, ros_bridge_port, spawn_idx) # addNpcRandomAtlanes(sim, no_of_npc, spawn_idx, ego_tf) spawns = sim.get_spawn() state = lgsvl.AgentState() forward = lgsvl.utils.transform_to_forward(spawns[0]) right = lgsvl.utils.transform_to_right(spawns[0]) state.transform.position = spawns[0].position + 30.0 * forward + 2 * right state.transform.rotation = spawns[0].rotation addNpcAtFixedPoints(sim, state, 0.0, False) state.transform.position = spawns[0].position + 20.0 * forward + 30 * right state.transform.rotation = spawns[0].rotation addNpcAtFixedPoints(sim, state, 0.0, False) state.transform.position = spawns[0].position + 60.0 * forward - 4.4 * right state.transform.rotation = spawns[0].rotation + Vector(0, 180, 0) addNpcAtFixedPoints(sim, state, 0.0, False) # start the sim input("Press Enter to run simulation") sim.run(time_limit, time_scale)
def _setup_npc(self, npc_type = None, position = None, follow_lane = True, speed = None, speed_upper = 25.0, speed_lower = 7.0, randomize = False, min_dist = 10.0, max_dist = 40.0): """ Spawns an NPC vehicle of a specific type at a specific location with an option to have it follow lane annotations in the Unity scene at a given speed. Not specifying any input results in a random selection of NPC type, a random spawn location within the [min_dist, max_dist] range of the ego vehicle, and a random speed selected within the [speed_lower, speed_upper] range. """ npc_types = {"Sedan", "Hatchback", "SUV", "Jeep", "BoxTruck", "SchoolBus"} if (not npc_type): npc_type = random.sample(npc_types, 1)[0] if (randomize or not position): sx = self.ego.transform.position.x sy = self.ego.transform.position.y sz = self.ego.transform.position.z while (not position): angle = random.uniform(0.0, 2*math.pi) dist = random.uniform(min_dist, max_dist) point = lgsvl.Vector(sx + dist * math.cos(angle), sy, sz + dist * math.sin(angle)) transform = self.env.map_point_on_lane(point) px = transform.position.x py = transform.position.y pz = transform.position.z mindist = 0.0 maxdist = 10.0 dist = random.uniform(mindist, maxdist) angle = math.radians(transform.rotation.y) position = lgsvl.Vector(px - dist * math.cos(angle), py, pz + dist * math.sin(angle)) for pos in self._occupied: if (position and self._proximity(position, pos) < 7): position = None state = lgsvl.AgentState() state.transform = self.env.map_point_on_lane(position) n = self.env.add_agent(npc_type, lgsvl.AgentType.NPC, state) if (follow_lane): if (not speed): speed = random.uniform(speed_lower, speed_upper) n.follow_closest_lane(True, speed) self.vehicles[n] = npc_type self._occupied.append(position)
def position_npc(self, transform): npc_state = lgsvl.AgentState() npc_state.transform = transform available_npcs = ['Sedan', 'SUV', 'Jeep', 'Hatchback'] # 'SchoolBus', 'DeliveryTruck' npc_type = available_npcs[random.randint(0, len(available_npcs) - 1)] npc = self.sim.add_agent(npc_type, lgsvl.AgentType.NPC, npc_state) self.npcs.append(npc) self.npcs_state.append(npc_state)
def spawn_ego(sim, pos=None): state = lgsvl.AgentState() if pos: state.transform = sim.map_point_on_lane(pos) else: spawns = sim.get_spawn() state.transform = spawns[0] ego = sim.add_agent("XE_Rigged-lgsvl", lgsvl.AgentType.EGO, state) return ego
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 spawn_pedestrian(sim, pos, name, offset=Vector(0, 0, 0), rotation=Vector(0, 0, 0)): state = lgsvl.AgentState() spawns = sim.get_spawn() state.transform = sim.map_point_on_lane(pos) state.transform.position += offset state.transform.rotation += rotation ped = sim.add_agent(name, lgsvl.AgentType.PEDESTRIAN, state) return ped
def test_multiple_lane_changes(self): with SimConnection(120) as sim: state = lgsvl.AgentState() state.transform.position = lgsvl.Vector(-180, 10, 239) state.transform.rotation = lgsvl.Vector(0, 90, 0) sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguae2015xe_autowareai, lgsvl.AgentType.EGO, state) state = lgsvl.AgentState() state.transform.position = lgsvl.Vector(-175, 10, 234.5) state.transform.rotation = lgsvl.Vector(0, 90, 0.016) npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) npc.follow_closest_lane(True, 10) agents = [] def on_lane_change(agent): agents.append(agent) npc.on_lane_change(on_lane_change) sim.run(1) npc.change_lane(True) sim.run(10) self.assertTrue(len(agents) == 1) self.assertTrue(npc == agents[0]) self.assertAlmostEqual(npc.state.position.x, 238.5, delta=1.5) npc.change_lane(True) sim.run(13) self.assertTrue(len(agents) == 2) self.assertTrue(npc == agents[1]) self.assertAlmostEqual(npc.state.position.x, 242.5, delta=1.5) npc.change_lane(False) sim.run(10) self.assertTrue(len(agents) == 3) self.assertTrue(npc == agents[2]) self.assertAlmostEqual(npc.state.position.x, 238.5, delta=1.5)
def initEV(self): sim = self.sim egoState = lgsvl.AgentState() egoState.transform = sim.map_point_on_lane(self.initEvPos) ego = sim.add_agent("XE_Rigged-apollo_3_5", lgsvl.AgentType.EGO, egoState) sensors = ego.get_sensors() for s in sensors: if s.name in [ 'velodyne', 'Main Camera', 'Telephoto Camera', 'GPS', 'IMU' ]: s.enabled = True self.ego = ego
def test_multiple_lane_changes(self): with SimConnection(120) as sim: state = lgsvl.AgentState() state.transform.position = lgsvl.Vector(239, 10, 180) state.transform.rotation = lgsvl.Vector(0, 180, 0) sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, state) state = lgsvl.AgentState() state.transform.position = lgsvl.Vector(234.5, 10, 175) state.transform.rotation = lgsvl.Vector(0.016, 180, 0) npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) npc.follow_closest_lane(True, 30) agents = [] def on_lane_change(agent): agents.append(agent) npc.on_lane_change(on_lane_change) sim.run(1) npc.change_lane(True) sim.run(10) self.assertTrue(len(agents) == 1) self.assertTrue(npc == agents[0]) self.assertAlmostEqual(npc.state.position.x, 238.5, delta=1.5) npc.change_lane(True) sim.run(13) self.assertTrue(len(agents) == 2) self.assertTrue(npc == agents[1]) self.assertAlmostEqual(npc.state.position.x, 242.5, delta=1.5) npc.change_lane(False) sim.run(10) self.assertTrue(len(agents) == 3) self.assertTrue(npc == agents[2]) self.assertAlmostEqual(npc.state.position.x, 238.5, delta=1.5)
def add_ego_car(): spawns = sim.get_spawn() forward = lgsvl.utils.transform_to_forward(spawns[0]) right = lgsvl.utils.transform_to_right(spawns[0]) state = lgsvl.AgentState() state.transform = spawns[0] a = sim.add_agent(vehicle_name, lgsvl.AgentType.EGO, state) a.connect_bridge(args.bridge, 9090) print("Waiting for connection...") while not a.bridge_connected: time.sleep(1) print("Bridge connected:", a.bridge_connected) return a
def setup_collision(self, sim, mover_name, agent_type, still_name, still_type): state = spawnState(sim) state.velocity = lgsvl.Vector(-50, 0, 0) mover = sim.add_agent(mover_name, agent_type, state) # school bus, 20m ahead, perpendicular to road, stopped state = lgsvl.AgentState() state.transform = sim.get_spawn()[0] state.transform.position.x -= 20.0 state.transform.rotation.y = 0.0 still = sim.add_agent(still_name, still_type, state) return mover, still