def test_EGO_exit_park_lot(self):
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)
# Delay the scenario for 2s
sim_connection.execute(timeout=2)
try:
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 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 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_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 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_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)