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 execute(sim: Simulator, config: TestConfig) -> Optional[bool]:
import logging
from common.scene import load_ego, load_scene, load_pedestrian, detect_collisions
from lgsvl.agent import Agent
from time import time
load_scene(sim, config.test_place.map_name)
pedestrian_behavior = TestCase6._generate_initial_pedestrian_behavior(
config.test_place, config.pedestrian_direction)
pedestrian = load_pedestrian(sim, config.test_place.map_name,
pedestrian_behavior.initial_state)
pedestrian.follow(pedestrian_behavior.waypoints)
ego_state_result = TestCase6._generate_initial_ego_state(
pedestrian_behavior, config.ego_distance, config.test_place,
config.ego_speed, config.pedestrian_direction)
if ego_state_result is None:
logging.warning(
"Could not generate initial ego state within allowed parameter range"
)
return None
initial_ego_state, time_to_crash_point = ego_state_result
ego = load_ego(sim, config.ego_car_name, initial_ego_state)
test_result = _TestResult()
def _on_collision(agent1: Agent, agent2: Agent,
contact: Vector) -> None:
test_result.successful = False
detect_collisions(sim, _on_collision)
start_time = time()
running_simulation_failed_once = False
while (time() - start_time) < (time_to_crash_point *
2) and test_result.successful:
try:
sim.run(1)
if running_simulation_failed_once:
logging.info(
"Continuing the execution failed once but could be restarted"
)
running_simulation_failed_once = False
except Exception:
if running_simulation_failed_once:
logging.exception(
"Continuing the execution failed again. Skipping test."
)
return None
else:
logging.exception(
"Continuing the execution failed. Retrying once.")
running_simulation_failed_once = True
# FIXME Sometimes the following print is not visible on the console (but being in debug mode)
return test_result.successful
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_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 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 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 init_configuration(lgsvl_sim: lgsvl.Simulator,
initial_state: lgsvl.AgentState, npc_speed: float):
# Init ego state
ego_state = copy.deepcopy(initial_state)
# Place Ego Car - 5 meter ahead from the initial point
ego_state = CarControl.place_car_from_the_point(dimension="vertical",
distance=5,
state=ego_state)
# Place Ego Car - 3.5m meter on the left
ego_state = CarControl.place_car_from_the_point(dimension="horizontal",
distance=-3.5,
state=ego_state)
# Drive ego car with speed 5m/s
ego_state = CarControl.drive_ego_car(state=ego_state,
directions=[("vertical", 5)])
# Add ego to simulator
ego = load_ego(lgsvl_sim, "Lincoln2017MKZ (Apollo 5.0)", ego_state)
# Init sedan state
sedan_state = copy.deepcopy(initial_state)
# Place Sedan - 5 meter ahead from the initial point
sedan_state = CarControl.place_car_from_the_point(dimension="vertical",
distance=5,
state=sedan_state)
# Add Sedan to simulator
sedan = load_npc(lgsvl_sim, "Sedan", sedan_state)
# Drive sedan follow its lane with given speed in m/s
sedan.follow_closest_lane(True, npc_speed)
# Init suv state
suv_state = copy.deepcopy(initial_state)
# Place SUV - 15 meter ahead from the initial point
suv_state = CarControl.place_car_from_the_point(dimension="vertical",
distance=20,
state=suv_state)
# Add suv to simulator
suv = load_npc(lgsvl_sim, "SUV", suv_state)
# Drive suv follow its lane with given speed in m/s
suv.follow_closest_lane(True, npc_speed)
return {
"sedan": sedan,
"suv": suv,
"ego": ego,
}
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_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 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_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 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_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)