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 test_EGO_creation(self): # Check if EGO Apollo is created
with SimConnection() as sim:
agent_state = spawn_state(sim)
agent = load_ego(sim, "Lincoln2017MKZ (Apollo 5.0)", agent_state)
expected = agent.name
target = "Lincoln2017MKZ (Apollo 5.0)"
self.assertEqual(expected, target)
def test_NPC_creation(self):
with SimConnection(60) as sim:
agent_state = spawn_state(sim)
agent = load_npc(sim, "Sedan", agent_state)
expected = agent.name
target = "Sedan"
self.assertEqual(expected, target)
def setup_cars(sim_connection: SimConnection, ego_speed: float,
npc_speed: float):
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", ego_speed)])
ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
return {
"school_bus": school_bus,
"ego": ego,
}
def test_placing_EGO_and_NPC_on_different_lane(self):
with SimConnection(scene="SingleLaneRoad") as sim:
# Placing the sedan on the starting point
state = spawn_state(sim)
sedan = load_npc(sim, "Sedan", state)
# Placing the ego on the left hand side of starting point - distance 3.5
ego_state = CarControl.place_car_from_the_point(dimension="horizontal", distance=-3.5, state=state)
ego = load_ego(sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
# sedan and ego on different line
self.assertNotEqual(ego.state.position.z, sedan.state.position.z, "sedan and ego on different lane")
def generate_vehicles(self, sim: lgsvl.Simulator, vehicles: list):
agents = []
for vehicle in vehicles:
state = spawn_state(sim)
state = CarControl.place_car_from_the_point(
dimension="vertical",
distance=vehicle["distance"],
state=state)
agent = vehicle["load_vehicle"](sim, vehicle["type"], state)
agent.on_collision(self.on_collision)
agents.append(agent)
return agents
def setup_cars(simc: SimConnection, npc_speed: float):
lgsvl_sim = simc.connect()
sedan_state = spawn_state(lgsvl_sim)
sedan_state = CarControl.place_car_from_the_point(dimension="vertical", distance=0, state=sedan_state)
sedan = load_npc(lgsvl_sim, "Sedan", sedan_state)
sedan.follow_closest_lane(True, npc_speed)
suv_state = spawn_state(lgsvl_sim)
suv_state = CarControl.place_car_from_the_point(dimension="vertical", distance=17, state=suv_state)
suv = load_npc(lgsvl_sim, "Sedan", suv_state)
suv.follow_closest_lane(True, npc_speed)
ego_state = spawn_state(lgsvl_sim)
ego_state = CarControl.place_car_from_the_point(dimension="horizontal", distance=-3.5, state=ego_state)
ego_state = CarControl.place_car_from_the_point(dimension="vertical", distance=5, state=ego_state)
ego_state = CarControl.drive_ego_car(state=ego_state, directions=[("vertical", 5)])
ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
return {
"sedan": sedan,
"suv": suv,
"ego": ego,
}
def test_placing_EGO_between_TRUCK_and_SEDAN_on_same_lane(self):
with SimConnection(scene="SingleLaneRoad") as sim:
# Placing the sedan on the starting point
state = spawn_state(sim)
sedan = load_npc(sim, "Sedan", state)
# Placing the ego - 5m ahead from the starting point
state = spawn_state(sim)
ego_state = CarControl.place_car_from_the_point(dimension="vertical", distance=5, state=state)
ego = load_ego(sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
# Placing the suv - 10m ahead from the starting point
state = spawn_state(sim)
suv_state = CarControl.place_car_from_the_point(dimension="vertical", distance=10, state=state)
suv = load_npc(sim, "SUV", suv_state)
# sedan and ego and suv on same line
self.assertEqual(sedan.state.position.z, ego.state.position.z, "sedan and ego on same lane")
self.assertEqual(ego.state.position.z, suv.state.position.z, "ego and suv on same lane")
# ego is behind suv
self.assertLess(suv.state.position.x, ego.state.position.x, "ego behind suv")
self.assertLess(ego.state.position.x, sedan.state.position.x, "ego in front of sedan")
def test_EGO_following_NPC_without_crash(self):
simConnection = SimConnection()
sim = simConnection.connect()
# Placing the suv - 10m ahead from the starting point
state = spawn_state(sim)
truck_state = CarControl.place_car_from_the_point(dimension="vertical",
distance=10,
state=state)
truck = load_npc(sim, "BoxTruck", truck_state)
# Driving the truck - speed 5m/s from the starting point
truck.follow_closest_lane(True, 5)
# Driving the ego - speed 1m/s from the starting point
state = spawn_state(sim)
ego_state = CarControl.drive_ego_car(state=state,
directions=[("vertical", 4.5)])
ego = load_ego(sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
# Run the simulator for 10 seconds with debug mode
simConnection.execute(timeout=10)
self.assertEqual(True, True)
simConnection.sim.close()
def setup_cars(sim_connection: SimConnection, ego_speed: float):
lgsvl_sim = sim_connection.connect()
# Placing the sedan
sedan_state = spawn_state(lgsvl_sim)
sedan_state = CarControl.place_car_from_the_point(dimension="vertical",
distance=15,
state=sedan_state)
sedan_state = CarControl.place_car_from_the_point(dimension="horizontal",
distance=-8,
state=sedan_state)
sedan_state = CarControl.rotate_car_by_degree(state=sedan_state,
degree=-45)
sedan = load_npc(lgsvl_sim, "Sedan", sedan_state)
# Placing the ego on the starting point
ego_state = spawn_state(lgsvl_sim)
ego_state = CarControl.drive_ego_car(ego_state, [("vertical", ego_speed)])
ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
return {
"sedan": sedan,
"ego": ego,
}
def test_placing_EGO_behind_NPC_on_same_lane(self):
with SimConnection(scene="SingleLaneRoad") as sim:
# Placing the sedan on the starting point
state = spawn_state(sim)
sedan = load_npc(sim, "Sedan", state)
# Placing the sedan in front of the ego
# Both vehicles are on the same lane
ego_state = CarControl.place_car_from_the_point(dimension="vertical", distance=-3.5, state=state)
ego = load_ego(sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
# sedan and ego on same line
self.assertEqual(ego.state.position.z, sedan.state.position.z, "sedan and ego on same lane")
# ego is behind sedan
self.assertLess(sedan.state.position.x, ego.state.position.x, "ego behind sedan")
def test_placing_EGO_on_different_land_and_ego_between_SUV_and_SEDAN(self):
with SimConnection(scene="SingleLaneRoad") as sim:
# Placing the sedan on the starting point
state = spawn_state(sim)
sedan = load_npc(sim, "Sedan", state)
# Placing the ego:
# - 5m ahead from the starting point
# - 3.5m on the left hand side
state = spawn_state(sim)
ego_state = CarControl.place_car_from_the_point(dimension="horizontal", distance=-3.5, state=state)
ego_state = CarControl.place_car_from_the_point(dimension="vertical", distance=5, state=ego_state)
ego = load_ego(sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
# Placing the suv - 10m ahead from the starting point
state = spawn_state(sim)
suv_state = CarControl.place_car_from_the_point(dimension="vertical", distance=10, state=state)
suv = load_npc(sim, "SUV", suv_state)
self.assertNotEqual(ego.state.position.z, sedan.state.position.z, "ego and sedan on different lane")
self.assertNotEqual(ego.state.position.z, suv.state.position.z, "ego and suv on different lane")
self.assertEqual(suv.state.position.z, sedan.state.position.z, "suv and sedan on same lane")
self.assertLess(suv.state.position.x, ego.state.position.x, "ego behind suv")
self.assertLess(ego.state.position.x, sedan.state.position.x, "ego in front of sedan")
def drive_ego_no_apollo(simConnection: SimConnection, npc_speed: float):
lgsvl_sim = simConnection.connect()
initial_state = spawn_state(lgsvl_sim) # Mutable object
configuration = init_configuration(lgsvl_sim,
initial_state,
npc_speed=npc_speed)
sedan = configuration["sedan"]
ego = configuration["ego"]
suv = configuration["suv"]
# Run the simulator for 5 seconds with debug mode
# simConnection.execute(timeout=5, vehicles=[ego, sedan, suv], debug=True)
# Run the simulator for 5 seconds
simConnection.execute(timeout=5)
# Record collision if happen
collisions = []
def on_collision(agent1, agent2, contact):
collisions.append([agent1, agent2, contact])
# print("{} collided with {}".format(agent1, agent2))
sedan.on_collision(on_collision)
suv.on_collision(on_collision)
ego.on_collision(on_collision)
# Drive ego to make lane change
simConnection.execute(timeout=5)
control = lgsvl.VehicleControl()
control.steering = 0.037
ego.apply_control(control, True)
simConnection.execute(timeout=5)
control.steering = -0.041
ego.apply_control(control, True)
simConnection.execute(timeout=5)
control.steering = 0
ego.apply_control(control, True)
simConnection.execute(timeout=7)
control.braking = 1
ego.apply_control(control, True)
simConnection.execute(timeout=3)
return {"sedan": sedan, "suv": suv, "ego": ego, "collisions": collisions}
def drive_ego_with_apollo(simConnection: SimConnection):
lgsvl_sim = simConnection.connect()
initial_state = spawn_state(lgsvl_sim) # Mutable object
configuration = init_configuration(lgsvl_sim, initial_state, npc_speed=3.9)
sedan = configuration["sedan"]
ego = configuration["ego"]
suv = configuration["suv"]
# Run the simulator for 5 seconds with debug mode
simConnection.execute(timeout=5)
# Record collision if happen
collisions = []
def on_collision(agent1, agent2, contact):
collisions.append([agent1, agent2, contact])
print("{} collided with {}".format(agent1, agent2))
sedan.on_collision(on_collision)
suv.on_collision(on_collision)
ego.on_collision(on_collision)
# Drive ego to make lane change
simConnection.execute(timeout=5)
control = lgsvl.VehicleControl()
control.steering = 0.037
ego.apply_control(control, True)
simConnection.execute(timeout=5)
# Start to drive the ego by Apollo
try:
dv_connection = connect_to_dreamview(ego, TARGET_POINT,
LGSVL__APOLLO_HOST,
LGSVL__APOLLO_PORT,
LGSVL__DREAMVIEW_PORT)
dv_connection.set_hd_map(config.test_place.map_name)
dv_connection.enable_module(ApolloModule.Control.value)
dv_connection.setup_apollo(TARGET_POINT.x, TARGET_POINT.z, [])
except Exception:
simConnection.sim.close()
return {"sedan": sedan, "suv": suv, "ego": ego, "collisions": collisions}
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_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'
]
EGO_TARGET = Vector(114.219772338867, -0.003660649061203,
-1.39988207817078)
COLLISIONS = []
# Setup environment
sim_connection = SimConnection()
lgsvl_sim = sim_connection.connect()
NPC_state = spawn_state(lgsvl_sim)
NPC = load_npc(lgsvl_sim, "Sedan", NPC_state)
sedan2_state = spawn_state(lgsvl_sim)
sedan2_state = CarControl.place_car_from_the_point(
dimension="vertical", distance=10, state=sedan2_state)
sedan2 = load_npc(lgsvl_sim, "Sedan", sedan2_state)
sedan3_state = spawn_state(lgsvl_sim)
sedan3_state = CarControl.place_car_from_the_point(
dimension="vertical", distance=15, state=sedan3_state)
sedan3 = load_npc(lgsvl_sim, "Sedan", sedan3_state)
# Placing the ego on the starting point
ego_state = spawn_state(lgsvl_sim)
ego_state = CarControl.place_car_from_the_point(dimension="vertical",
distance=5,
state=ego_state)
ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
sim_connection.execute(timeout=5, debug=True, vehicles=[ego])
def on_collision(agent1, agent2, contact):
COLLISIONS.append([agent1, agent2, contact])
sim_connection.sim.close()
# print("Exception: {} collided with {}".format(agent1, agent2))
raise Exception()
ego.on_collision(on_collision)
NPC.on_collision(on_collision)
sedan2.on_collision(on_collision)
sedan3.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(EGO_TARGET.x, EGO_TARGET.z, MODULES)
# Start the scenario
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)
def test_EGO_encroach_NPC_speed_20_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(4.85003185272217, -0.0120296478271484,
22.7699680328369)
COLLISIONS = []
SEDAN_WPS = [
Vector(-3.15000438690186, 0, 37.7700042724609),
Vector(4.85003137588501, -0.0120288729667664, 22.7699680328369)
]
NPC_SPEED = 10
sim_connection = SimConnection(scene="CubeTown")
lgsvl_sim = sim_connection.connect()
# Placing the sedan
sedan_state = spawn_state(lgsvl_sim)
sedan_state = CarControl.place_car_from_the_point(dimension="vertical",
distance=15,
state=sedan_state)
sedan_state = CarControl.place_car_from_the_point(
dimension="horizontal", distance=-8, state=sedan_state)
sedan_state = CarControl.rotate_car_by_degree(state=sedan_state,
degree=-45)
sedan = load_npc(lgsvl_sim, "Sedan", sedan_state)
# Placing the ego on the starting point
ego_state = spawn_state(lgsvl_sim)
# ego_state = CarControl.drive_ego_car(ego_state, [("vertical", 3)])
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)
sedan.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
waypoints = []
for point in SEDAN_WPS:
waypoints.append(
lgsvl.DriveWaypoint(point, NPC_SPEED,
sedan.state.transform.rotation))
sedan.follow(waypoints)
sim_connection.execute(timeout=15)
except Exception:
sim_connection.sim.close()
self.fail("Failed!")
sim_connection.sim.close()
self.assertTrue(True, True)
