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)
