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_ped_circle_waypoints( self): # Check if pedestrians can follow waypoints with SimConnection(60) as sim: sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, spawnState(sim)) state = spawnState(sim) sx = state.position.x - 20 sy = state.position.y sz = state.position.z + 10 radius = 5 count = 3 waypointCommands = [] waypoints = [] for i in range(count): x = radius * math.cos(i * 2 * math.pi / count) z = radius * math.sin(i * 2 * math.pi / count) idle = 1 if i < count // 2 else 0 waypointCommands.append( lgsvl.WalkWaypoint(lgsvl.Vector(sx + x, sy, sz + z), idle)) waypoints.append(lgsvl.Vector(sx + x, sy, sz + z)) state.transform.position = waypoints[0] zoe = self.create_ped(sim, "Zoe", state) def on_waypoint(agent, index): msg = "Waypoint " + str(index) mEqual(self, zoe.state.position, waypoints[index], msg) if index == len(waypoints) - 1: sim.stop() zoe.on_waypoint_reached(on_waypoint) zoe.follow(waypointCommands, True) sim.run()
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 get_npc_random_transform(self): """Finds a random point near the EGO on the map. Once that is done, randomly find another nearby point so that the NPCs are spawned on different lanes. """ ego_transform = self.ego_state.transform sx = ego_transform.position.x sy = ego_transform.position.y sz = ego_transform.position.z ry = ego_transform.rotation.y if ry < 0: ry = 360 + ry hfov = self.camera_intrinsics["horizontal_fov"] mindist = 0.0 maxdist = 100.0 dist = random.uniform(mindist, maxdist) angle = random.uniform(math.radians(ry - hfov / 2), math.radians(ry + hfov / 2)) point = lgsvl.Vector(sx + dist * math.sin(angle), sy, sz + dist * math.cos(angle)) transform = self.sim.map_point_on_lane(point) sx = transform.position.x sy = transform.position.y sz = transform.position.z mindist = 0.0 maxdist = 10.0 dist = random.uniform(mindist, maxdist) angle = math.radians(transform.rotation.y) point = lgsvl.Vector(sx - dist * math.cos(angle), sy, sz + dist * math.sin(angle)) transform = self.sim.map_point_on_lane(point) return transform
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 is_npc_obscured(self, npc_transform): lidar_mat = np.dot(transform_to_matrix(self.sensor_lidar.transform), transform_to_matrix(self.ego_state.transform)) start = lgsvl.Vector( lidar_mat[3][0], lidar_mat[3][1], lidar_mat[3][2], ) end = npc_transform.position direction = lgsvl.Vector( end.x - start.x, end.y - start.y, end.z - start.z, ) distance = np.linalg.norm( np.array([direction.x, direction.y, direction.z])) layer_mask = 0 for bit in [0]: layer_mask |= 1 << bit hit = self.sim.raycast(start, direction, layer_mask, distance) if hit: return True return False
def get_npc_random_transform(self): ego_transform = self.ego_state.transform sx = ego_transform.position.x sy = ego_transform.position.y sz = ego_transform.position.z ry = ego_transform.rotation.y if ry < 0: ry = 360 + ry hfov = self.camera_intrinsics["horizontal_fov"] mindist = 0.0 maxdist = 100.0 dist = random.uniform(mindist, maxdist) angle = random.uniform(math.radians(ry - hfov / 2), math.radians(ry + hfov / 2)) point = lgsvl.Vector(sx + dist * math.sin(angle), sy, sz + dist * math.cos(angle)) transform = self.sim.map_point_on_lane(point) sx = transform.position.x sy = transform.position.y sz = transform.position.z mindist = 0.0 maxdist = 10.0 dist = random.uniform(mindist, maxdist) angle = math.radians(transform.rotation.y) point = lgsvl.Vector(sx - dist * math.cos(angle), sy, sz + dist * math.sin(angle)) transform = self.sim.map_point_on_lane(point) return transform
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 test_vector_multiply(self): # Check that vector_multiply calculates the right values inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1,2,3), lgsvl.Vector(4,5,6))) inputVector = lgsvl.Vector(10,20,30) expectedVector = lgsvl.Vector(11.560345883724906, 20.792029959836434, 33.58330761714148) vector = lgsvl.utils.vector_multiply(inputVector, inputMatrix) self.assertAlmostEqual(vector.x, expectedVector.x) self.assertAlmostEqual(vector.y, expectedVector.y) self.assertAlmostEqual(vector.z, expectedVector.z)
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 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 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 test_transform_to_matrix(self): transform = lgsvl.Transform(lgsvl.Vector(1,2,3), lgsvl.Vector(4,5,6)) expectedMatrix = [[0.9913729386253347, 0.10427383718471564, -0.07941450396586013, 0.0], \ [-0.0980843287345578, 0.9920992900156518, 0.07822060602635744, 0.0], \ [0.08694343573875718, -0.0697564737441253, 0.9937680178757644, 0.0], \ [1, 2, 3, 1.0]] matrix = lgsvl.utils.transform_to_matrix(transform) for i in range(4): for j in range(4): self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j])
def get_pedesrian_waypoints(pedestrian): waypoints = [] ped_pos = pedestrian.transform.position wp1 = lgsvl.WalkWaypoint(lgsvl.Vector(ped_pos.x, ped_pos.y, ped_pos.z), 0) wp2 = lgsvl.WalkWaypoint( lgsvl.Vector(ped_pos.x - 40, ped_pos.y, ped_pos.z), 0) waypoints.append(wp1) waypoints.append(wp2) return waypoints
def test_matrix_inverse( self): # Check that matrix_inverse calculates the right values inputMatrix = lgsvl.utils.transform_to_matrix( lgsvl.Transform(lgsvl.Vector(1, 2, 3), lgsvl.Vector(4, 5, 6))) expectedMatrix = [[0.9913729386253347, -0.0980843287345578, 0.08694343573875718, 0.0], \ [0.10427383718471564, 0.9920992900156518, -0.0697564737441253, 0.0], \ [-0.07941450396586013, 0.07822060602635744, 0.9937680178757644, 0.0], \ [-0.9616771010971856, -2.120776069375818, -2.9287345418778, 1.0]] matrix = lgsvl.utils.matrix_inverse(inputMatrix) for i in range(4): for j in range(4): self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j])
def test_matrix_multiply( self): # Check that matrix_multiply calculates the right values inputMatrix = lgsvl.utils.transform_to_matrix( lgsvl.Transform(lgsvl.Vector(1, 2, 3), lgsvl.Vector(4, 5, 6))) expectedMatrix = [[0.9656881042915112, 0.21236393599051254, -0.1494926216255657, 0.0], \ [-0.18774677387638924, 0.9685769782741936, 0.1631250626244768, 0.0], \ [0.1794369920860106, -0.12946117505974142, 0.9752139098799174, 0.0], \ [2.0560345883724906, 3.8792029959836434, 6.058330761714148, 1.0]] matrix = lgsvl.utils.matrix_multiply(inputMatrix, inputMatrix) for i in range(4): for j in range(4): self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j])
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 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 test_ped_circle_waypoints(self): # Check if pedestrians 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 - 4 * forward sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) state = spawnState(sim) state.transform.position = state.position + 10 * forward radius = 5 count = 3 waypointCommands = [] waypoints = [] layer_mask = 0 | (1 << 0) for i in range(count): x = radius * math.cos(i * 2 * math.pi / count) z = radius * math.sin(i * 2 * math.pi / count) idle = 1 if i < count//2 else 0 hit = sim.raycast(state.position + z * forward + x * right, lgsvl.Vector(0, -1, 0), layer_mask) waypointCommands.append(lgsvl.WalkWaypoint(hit.point, idle)) waypoints.append(hit.point) state.transform.position = waypoints[0] zoe = self.create_ped(sim, "Zoe", state) def on_waypoint(agent,index): msg = "Waypoint " + str(index) mEqual(self, zoe.state.position, waypoints[index], msg) if index == len(waypoints)-1: sim.stop() zoe.on_waypoint_reached(on_waypoint) zoe.follow(waypointCommands, True) sim.run()
def groundElevationAt(self, pos): origin = utils.scenicToLGSVLPosition(pos, 100000) result = self.client.raycast(origin, lgsvl.Vector(0, -1, 0), 1) if result is None: warnings.warn(f'no ground at position {pos}') return 0 return result.point.y
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 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 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 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 test_waypoint_idle_time(self): with SimConnection(60) as sim: sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(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 + 10 * forward zoe = self.create_ped(sim, "Zoe", state) def on_waypoint(agent, index): sim.stop() layer_mask = 0 | (1 << 0) waypoints = [] hit = sim.raycast(state.position - 2 * right, lgsvl.Vector(0, -1, 0), layer_mask) waypoints.append(lgsvl.WalkWaypoint(hit.point, 0)) hit = sim.raycast(state.position - 5 * right, lgsvl.Vector(0, -1, 0), layer_mask) waypoints.append(lgsvl.WalkWaypoint(hit.point, 0)) zoe.on_waypoint_reached(on_waypoint) zoe.follow(waypoints) t0 = time.time() sim.run() # reach the first waypoint sim.run() # reach the second waypoint t1 = time.time() noIdleTime = t1 - t0 zoe.state = state waypoints = [] hit = sim.raycast(state.position - 2 * right, lgsvl.Vector(0, -1, 0), layer_mask) waypoints.append(lgsvl.WalkWaypoint(hit.point, 2)) hit = sim.raycast(state.position - 5 * right, lgsvl.Vector(0, -1, 0), layer_mask) waypoints.append(lgsvl.WalkWaypoint(hit.point, 0)) zoe.follow(waypoints) t2 = time.time() sim.run() # reach the first waypoint sim.run() # reach the second waypoint t3 = time.time() idleTime = t3 - t2 self.assertAlmostEqual(idleTime - noIdleTime, 2.0, delta=0.5)
def right_lane_check(simulator, position): egoLane = simulator.map_point_on_lane(position) rightLane = simulator.map_point_on_lane( lgsvl.Vector(position.x - 0.6993, position.y, position.z + 9.9755)) return almost_equal(egoLane.position.x, rightLane.position.x) and \ almost_equal(egoLane.position.y, rightLane.position.y) and \ almost_equal(egoLane.position.z, rightLane.position.z)
def main(): sim_host_ip = "127.0.0.1" # of your local machine running sim sim_host_port = 8181 sim_map = "Shalun" ego_car_model = "Lexus2016RXHybrid (Autoware)" # Jaguar2015XE (Autoware) ros_bridge_ip = "192.168.1.100" # 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 = 0.5 # 1 means normal speed, 1.5 means 1.5x speed ego_pos = Vector(-32.6, 0, 45) ego_roll = 0 ego_pitch = 0 ego_yaw = 160 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, spawn_tf) pedestrian_pos = Vector(-35, 0, 30) pedestrian_state = ObjectState(Transform(pedestrian_pos)) waypoints = [ lgsvl.WalkWaypoint(lgsvl.Vector(-16, 0, 34), 5, 0), lgsvl.WalkWaypoint(lgsvl.Vector(-14, 0, 12), 5, 0), lgsvl.WalkWaypoint(lgsvl.Vector(-30, 0, 12), 5, 0), lgsvl.WalkWaypoint(lgsvl.Vector(-30, 0, 30), 5, 0), ] createPedestrian(sim, pedestrian_state, waypoints) input("Press Enter to run simulation") sim.run(time_limit, time_scale)
def test_npc_different_directions( self): # Check that specified velocities match the NPC's movement with SimConnection() as sim: state = spawnState(sim) state.position.x += 10 sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, state) state = spawnState(sim) state.velocity = lgsvl.Vector(-10, 0, 0) npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state) sim.run(1) self.assertNotAlmostEqual(state.position.x, npc.state.position.x, delta=0.2) self.assertAlmostEqual(state.position.y, npc.state.position.y, delta=0.2) self.assertAlmostEqual(state.position.z, npc.state.position.z, delta=0.2) sim.remove_agent(npc) state.velocity = lgsvl.Vector(0, 10, 0) npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state) sim.run(1) self.assertNotAlmostEqual(state.position.y, npc.state.position.y, delta=0.2) self.assertAlmostEqual(state.position.x, npc.state.position.x, delta=0.2) self.assertAlmostEqual(state.position.z, npc.state.position.z, delta=0.2) sim.remove_agent(npc) state.velocity = lgsvl.Vector(0, 0, -10) npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state) sim.run(1) self.assertNotAlmostEqual(state.position.z, npc.state.position.z, delta=0.2) self.assertAlmostEqual(state.position.y, npc.state.position.y, delta=0.2) self.assertAlmostEqual(state.position.x, npc.state.position.x, delta=0.2)
def test_ego_different_directions( self ): # Check that the xyz velocities equate to xyz changes in position with SimConnection() as sim: state = spawnState(sim) state.velocity = lgsvl.Vector(-10, 0, 0) ego = sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, state) sim.run(.5) self.assertNotAlmostEqual(state.position.x, ego.state.position.x, places=1) self.assertAlmostEqual(state.position.y, ego.state.position.y, places=1) self.assertAlmostEqual(state.position.z, ego.state.position.z, places=1) sim.remove_agent(ego) state.velocity = lgsvl.Vector(0, 10, 0) ego = sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, state) sim.run(.5) self.assertNotAlmostEqual(state.position.y, ego.state.position.y, places=1) self.assertAlmostEqual(state.position.x, ego.state.position.x, places=1) self.assertAlmostEqual(state.position.z, ego.state.position.z, places=1) sim.remove_agent(ego) state.velocity = lgsvl.Vector(0, 0, -10) ego = sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, state) sim.run(.5) self.assertNotAlmostEqual(state.position.z, ego.state.position.z, places=1) self.assertAlmostEqual(state.position.y, ego.state.position.y, places=1) self.assertAlmostEqual(state.position.x, ego.state.position.x, places=1)
def _setup_agent(self, v, agent_name="Jeep", agent_type=lgsvl.AgentType.NPC): state = lgsvl.AgentState() x = v.position[0] + self.config["map_offset"][0] y = 0.0 z = v.position[1] + self.config["map_offset"][1] vx = v.velocity * cos(v.heading) vy = 0.0 vz = v.velocity * sin(v.heading) state.transform.position = lgsvl.Vector(z, y, x) state.transform.rotation = lgsvl.Vector( 0.0, np.rad2deg(v.heading) + self.config["map_offset"][2], 0.0) state.velocity = lgsvl.Vector(vz, vy, vx) state.angular_velocity = lgsvl.Vector(0.0, 0.0, 0.0) return self.sim.add_agent(agent_name, agent_type, state)