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()
