def __init__(self,
scenario_params: ScenarioParameters,
ctrl_params: CtrlParameters,
carla_synchronous: tuple,
eval_env: Environment,
eval_stg: Trajectron,
motion_planner_cls: MidlevelAgent,
scene_builder_cls: TrajectronPlusPlusSceneBuilder,
n_simulations=2):
self.client, self.world, self.carla_map, self.traffic_manager = carla_synchronous
self.scenario_params = scenario_params
self.ctrl_params = ctrl_params
self.eval_env = eval_env
self.eval_stg = eval_stg
self.motion_planner_cls = motion_planner_cls
self.scene_builder_cls = scene_builder_cls
self.n_simulations = n_simulations
self.map_reader = NaiveMapQuerier(self.world,
self.carla_map,
debug=True)
self.online_config = OnlineConfig(record_interval=10,
node_type=self.eval_env.NodeType)
# Mock vehicles
self.spawn_points = self.carla_map.get_spawn_points()
self.blueprints = get_all_vehicle_blueprints(self.world)
self.blueprint_audi_a2 = self.world.get_blueprint_library().find(
'vehicle.audi.a2')
def test_1_npc_appear(carla_Town03_synchronous):
"""Test case where NPC appears before EGO vehicle after awhile.
"""
client, world, carla_map, traffic_manager = carla_Town03_synchronous
ego_vehicle = None
other_vehicles = []
data_collector = None
n_burn_frames = 180
save_frequency = 40
record_interval = 5
scene_interval = 30
try:
# Mock vehicles
spawn_points = carla_map.get_spawn_points()
blueprint = world.get_blueprint_library().find('vehicle.audi.a2')
spawn_point = spawn_points[251]
ego_vehicle = world.spawn_actor(blueprint, spawn_point)
ego_vehicle.set_autopilot(True, traffic_manager.get_port())
other_vehicle_ids = []
blueprints = get_all_vehicle_blueprints(world)
for idx in [164]:
blueprint = np.random.choice(blueprints)
spawn_point = spawn_points[idx]
other_vehicle = world.spawn_actor(blueprint, spawn_point)
other_vehicle.set_autopilot(True, traffic_manager.get_port())
other_vehicles.append(other_vehicle)
other_vehicle_ids.append(other_vehicle.id)
# Setup data collector
map_reader = NaiveMapQuerier(world, carla_map, debug=True)
scene_config = SceneConfig(scene_interval=scene_interval,
record_interval=record_interval)
data_collector = DataCollector(ego_vehicle,
map_reader,
other_vehicle_ids,
scene_builder_cls=TESTSceneBuilder,
scene_config=scene_config,
save_frequency=save_frequency,
n_burn_frames=n_burn_frames,
debug=True)
data_collector.start_sensor()
assert data_collector.sensor_is_listening
# Run simulation for X steps
for idx in range(n_burn_frames + record_interval *
(save_frequency - 1)):
frame = world.tick()
data_collector.capture_step(frame)
finally:
if data_collector:
data_collector.destroy()
if ego_vehicle:
ego_vehicle.destroy()
for other_vehicle in other_vehicles:
other_vehicle.destroy()
def test_no_npcs(carla_Town03_synchronous):
client, world, carla_map, traffic_manager = carla_Town03_synchronous
ego_vehicle = None
data_collector = None
n_burn_frames = 60
save_frequency = 30
record_interval = 5
scene_interval = 20
try:
# Mock vehicles
blueprint = world.get_blueprint_library().find('vehicle.audi.a2')
spawn_point = carla_map.get_spawn_points()[0]
ego_vehicle = world.spawn_actor(blueprint, spawn_point)
ego_vehicle.set_autopilot(True, traffic_manager.get_port())
# Setup data collector
map_reader = NaiveMapQuerier(world, carla_map, debug=True)
scene_config = SceneConfig(scene_interval=scene_interval,
record_interval=record_interval)
data_collector = DataCollector(ego_vehicle,
map_reader, [],
scene_builder_cls=TESTSceneBuilder,
scene_config=scene_config,
save_frequency=save_frequency,
n_burn_frames=n_burn_frames,
debug=True)
data_collector.start_sensor()
assert data_collector.sensor_is_listening
# Run simulation for X steps
for idx in range(n_burn_frames + record_interval *
(save_frequency - 1)):
frame = world.tick()
data_collector.capture_step(frame)
finally:
if data_collector:
data_collector.destroy()
if ego_vehicle:
ego_vehicle.destroy()
def scenario(scenario_params, variables, eval_env, eval_stg):
ego_spawn_idx, other_spawn_ids, n_burn_interval, controls, \
goal, carla_synchronous = scenario_params
prediction_horizon, control_horizon, n_predictions, n_coincide = variables
ego_vehicle = None
agent = None
other_vehicles = []
client, world, carla_map, traffic_manager = carla_synchronous
try:
map_reader = NaiveMapQuerier(world, carla_map, debug=True)
online_config = OnlineConfig(node_type=eval_env.NodeType)
spawn_points = carla_map.get_spawn_points()
spawn_point = spawn_points[ego_spawn_idx]
blueprint = world.get_blueprint_library().find('vehicle.audi.a2')
ego_vehicle = world.spawn_actor(blueprint, spawn_point)
other_vehicle_ids = []
blueprints = get_all_vehicle_blueprints(world)
for idx in other_spawn_ids:
blueprint = np.random.choice(blueprints)
spawn_point = spawn_points[idx]
other_vehicle = world.spawn_actor(blueprint, spawn_point)
other_vehicle.set_autopilot(True, traffic_manager.get_port())
other_vehicles.append(other_vehicle)
other_vehicle_ids.append(other_vehicle.id)
world.tick()
agent = MidlevelAgent(ego_vehicle,
map_reader,
other_vehicle_ids,
eval_stg,
n_burn_interval=n_burn_interval,
control_horizon=control_horizon,
prediction_horizon=prediction_horizon,
n_predictions=n_predictions,
n_coincide=n_coincide,
scene_builder_cls=TrajectronPlusPlusSceneBuilder,
scene_config=online_config)
agent.start_sensor()
assert agent.sensor_is_listening
if goal:
agent.set_goal(goal.x, goal.y, is_relative=True)
"""Move the spectator to the ego vehicle.
The positioning is a little off"""
state = agent.get_vehicle_state()
goal = agent.get_goal()
world.get_spectator().set_transform(
carla.Transform(
carla.Location(x=state[0] + goal.x,
y=state[1] - goal.y,
z=state[2] + 50), carla.Rotation(pitch=-90)))
n_burn_frames = n_burn_interval * online_config.record_interval
control_frames = n_coincide * online_config.record_interval - 1
for idx in range(n_burn_frames + control_frames):
control = None
for ctrl in controls:
if ctrl.interval[0] <= idx and idx <= ctrl.interval[1]:
control = ctrl.control
break
frame = world.tick()
agent.run_step(frame, control=control)
finally:
if agent:
agent.destroy()
if ego_vehicle:
ego_vehicle.destroy()
for other_vehicle in other_vehicles:
other_vehicle.destroy()
time.sleep(1)
def scenario(scenario_params, agent_constructor, ctrl_params,
carla_synchronous, eval_env, eval_stg):
ego_vehicle = None
agent = None
other_vehicles = []
client, world, carla_map, traffic_manager = carla_synchronous
try:
map_reader = NaiveMapQuerier(world, carla_map, debug=True)
online_config = OnlineConfig(node_type=eval_env.NodeType)
# Mock vehicles
spawn_points = carla_map.get_spawn_points()
blueprints = get_all_vehicle_blueprints(world)
spawn_indices = [scenario_params.ego_spawn_idx
] + scenario_params.other_spawn_ids
other_vehicle_ids = []
for k, spawn_idx in enumerate(spawn_indices):
if k == 0:
blueprint = world.get_blueprint_library().find(
'vehicle.audi.a2')
else:
blueprint = np.random.choice(blueprints)
spawn_point = spawn_points[spawn_idx]
spawn_point = shift_spawn_point(carla_map, k,
scenario_params.spawn_shifts,
spawn_point)
# Prevent collision with road.
spawn_point.location += carla.Location(0, 0, 0.5)
vehicle = world.spawn_actor(blueprint, spawn_point)
if k == 0:
ego_vehicle = vehicle
else:
vehicle.set_autopilot(True, traffic_manager.get_port())
other_vehicles.append(vehicle)
other_vehicle_ids.append(vehicle.id)
world.tick()
agent = agent_constructor(
ego_vehicle,
map_reader,
other_vehicle_ids,
eval_stg,
n_burn_interval=scenario_params.n_burn_interval,
control_horizon=ctrl_params.control_horizon,
prediction_horizon=ctrl_params.prediction_horizon,
n_predictions=ctrl_params.n_predictions,
n_coincide=ctrl_params.n_coincide,
scene_builder_cls=TrajectronPlusPlusSceneBuilder,
log_agent=False,
log_cplex=False,
plot_scenario=True,
plot_simulation=True,
scene_config=online_config)
agent.start_sensor()
assert agent.sensor_is_listening
if scenario_params.goal:
agent.set_goal(scenario_params.goal.x,
scenario_params.goal.y,
is_relative=scenario_params.goal.is_relative)
"""Move the spectator to the ego vehicle.
The positioning is a little off"""
state = agent.get_vehicle_state()
goal = agent.get_goal()
if goal.is_relative:
location = carla.Location(x=state[0] + goal.x,
y=state[1] - goal.y,
z=state[2] + 50)
else:
location = carla.Location(x=state[0] + (state[0] - goal.x) / 2.,
y=state[1] - (state[1] + goal.y) / 2.,
z=state[2] + 50)
world.get_spectator().set_transform(
carla.Transform(location, carla.Rotation(pitch=-90)))
n_burn_frames = scenario_params.n_burn_interval * online_config.record_interval
if scenario_params.loop_type == LoopEnum.CLOSED_LOOP:
run_frames = scenario_params.run_interval * online_config.record_interval
elif isinstance(agent, OAAgent):
run_frames = ctrl_params.control_horizon * online_config.record_interval - 1
else:
run_frames = ctrl_params.n_coincide * online_config.record_interval - 1
for idx in range(n_burn_frames + run_frames):
control = None
for ctrl in scenario_params.controls:
if ctrl.interval[0] <= idx and idx <= ctrl.interval[1]:
control = ctrl.control
break
frame = world.tick()
agent.run_step(frame, control=control)
finally:
if agent:
agent.destroy()
if ego_vehicle:
ego_vehicle.destroy()
for other_vehicle in other_vehicles:
other_vehicle.destroy()
time.sleep(1)
def scenario(scenario_params, ctrl_params, carla_synchronous, eval_env,
eval_stg):
ego_vehicle = None
agent = None
spectator_camera = None
other_vehicles = []
record_spectator = False
client, world, carla_map, traffic_manager = carla_synchronous
try:
map_reader = NaiveMapQuerier(world, carla_map, debug=True)
online_config = OnlineConfig(record_interval=10,
node_type=eval_env.NodeType)
# Mock vehicles
spawn_points = carla_map.get_spawn_points()
blueprints = get_all_vehicle_blueprints(world)
spawn_indices = [scenario_params.ego_spawn_idx
] + scenario_params.other_spawn_ids
other_vehicle_ids = []
for k, spawn_idx in enumerate(spawn_indices):
if k == 0:
blueprint = world.get_blueprint_library().find(
'vehicle.audi.a2')
else:
blueprint = np.random.choice(blueprints)
spawn_point = spawn_points[spawn_idx]
spawn_point = shift_spawn_point(carla_map, k,
scenario_params.spawn_shifts,
spawn_point)
# Prevent collision with road.
spawn_point.location += carla.Location(0, 0, 0.5)
vehicle = world.spawn_actor(blueprint, spawn_point)
if k == 0:
ego_vehicle = vehicle
else:
vehicle.set_autopilot(True, traffic_manager.get_port())
if scenario_params.ignore_signs:
traffic_manager.ignore_signs_percentage(vehicle, 100.)
if scenario_params.ignore_lights:
traffic_manager.ignore_lights_percentage(vehicle, 100.)
if scenario_params.ignore_vehicles:
traffic_manager.ignore_vehicles_percentage(vehicle, 100.)
if not scenario_params.auto_lane_change:
traffic_manager.auto_lane_change(vehicle, False)
other_vehicles.append(vehicle)
other_vehicle_ids.append(vehicle.id)
frame = world.tick()
agent = MidlevelAgent(
ego_vehicle,
map_reader,
other_vehicle_ids,
eval_stg,
n_burn_interval=scenario_params.n_burn_interval,
control_horizon=ctrl_params.control_horizon,
prediction_horizon=ctrl_params.prediction_horizon,
step_horizon=ctrl_params.step_horizon,
n_predictions=ctrl_params.n_predictions,
scene_builder_cls=TrajectronPlusPlusSceneBuilder,
turn_choices=scenario_params.turn_choices,
max_distance=scenario_params.max_distance,
plot_boundary=False,
log_agent=False,
log_cplex=False,
plot_scenario=True,
plot_simulation=True,
plot_overapprox=False,
scene_config=online_config)
agent.start_sensor()
assert agent.sensor_is_listening
"""Move the spectator to the ego vehicle.
The positioning is a little off"""
state = agent.get_vehicle_state()
goal = agent.get_goal()
goal_x, goal_y = goal.x, -goal.y
if goal.is_relative:
location = carla.Location(x=state[0] + goal_x / 2.,
y=state[1] - goal_y / 2.,
z=state[2] + 50)
else:
location = carla.Location(x=(state[0] + goal_x) / 2.,
y=(state[1] + goal_y) / 2.,
z=state[2] + 50)
# configure the spectator
world.get_spectator().set_transform(
carla.Transform(location,
carla.Rotation(pitch=-70, yaw=-90, roll=20)))
record_spectator = False
if record_spectator:
# attach camera to spectator
spectator_camera = attach_camera_to_spectator(world, frame)
n_burn_frames = scenario_params.n_burn_interval * online_config.record_interval
if ctrl_params.loop_type == LoopEnum.CLOSED_LOOP:
run_frames = scenario_params.run_interval * online_config.record_interval
else:
run_frames = ctrl_params.control_horizon * online_config.record_interval - 1
for idx in range(n_burn_frames + run_frames):
control = None
for ctrl in scenario_params.controls:
if ctrl.interval[0] <= idx and idx <= ctrl.interval[1]:
control = ctrl.control
break
frame = world.tick()
agent.run_step(frame, control=control)
finally:
if spectator_camera:
spectator_camera.destroy()
if agent:
agent.destroy()
if ego_vehicle:
ego_vehicle.destroy()
for other_vehicle in other_vehicles:
other_vehicle.destroy()
if record_spectator == True:
time.sleep(5)
else:
time.sleep(1)
def run(self):
ego_vehicle = None
agent = None
spectator_camera = None
other_vehicles = []
record_spectator = False
try:
map_reader = NaiveMapQuerier(self.world,
self.carla_map,
debug=True)
online_config = OnlineConfig(record_interval=10,
node_type=self.eval_env.NodeType)
# Mock vehicles
spawn_points = self.carla_map.get_spawn_points()
blueprints = get_all_vehicle_blueprints(self.world)
spawn_indices = [self.scenario_params.ego_spawn_idx
] + self.scenario_params.other_spawn_ids
other_vehicle_ids = []
for k, spawn_idx in enumerate(spawn_indices):
if k == 0:
blueprint = self.world.get_blueprint_library().find(
'vehicle.audi.a2')
else:
blueprint = np.random.choice(blueprints)
spawn_point = spawn_points[spawn_idx]
spawn_point = shift_spawn_point(
self.carla_map, k, self.scenario_params.spawn_shifts,
spawn_point)
# Prevent collision with road.
spawn_point.location += carla.Location(0, 0, 0.5)
vehicle = self.world.spawn_actor(blueprint, spawn_point)
if k == 0:
ego_vehicle = vehicle
else:
vehicle.set_autopilot(True,
self.traffic_manager.get_port())
if self.scenario_params.ignore_signs:
self.traffic_manager.ignore_signs_percentage(
vehicle, 100.)
if self.scenario_params.ignore_lights:
self.traffic_manager.ignore_lights_percentage(
vehicle, 100.)
if self.scenario_params.ignore_vehicles:
self.traffic_manager.ignore_vehicles_percentage(
vehicle, 100.)
if not self.scenario_params.auto_lane_change:
self.traffic_manager.auto_lane_change(vehicle, False)
other_vehicles.append(vehicle)
other_vehicle_ids.append(vehicle.id)
frame = self.world.tick()
agent = self.motion_planner_cls(
ego_vehicle,
map_reader,
other_vehicle_ids,
self.eval_stg,
scene_builder_cls=self.scene_builder_cls,
scene_config=online_config,
**self.scenario_params,
**self.ctrl_params,
**self.DEBUG_SETTINGS,
)
agent.start_sensor()
assert agent.sensor_is_listening
"""Setup vehicle routes"""
if "CARLANAME" in os.environ and os.environ[
"CARLANAME"] == "carla-0.9.13":
for k, vehicle in enumerate(other_vehicles):
route = None
try:
route = self.scenario_params.other_routes[k]
len(route)
except (TypeError, IndexError) as e:
continue
self.traffic_manager.set_route(vehicle, route)
else:
logging.info("Skipping setting up OV routes.")
"""Move the spectator to the ego vehicle.
The positioning is a little off"""
state = agent.get_vehicle_state()
goal = agent.get_goal()
goal_x, goal_y = goal.x, -goal.y
if goal.is_relative:
location = carla.Location(x=state[0] + goal_x / 2.,
y=state[1] - goal_y / 2.,
z=state[2] + 50)
else:
location = carla.Location(x=(state[0] + goal_x) / 2.,
y=(state[1] + goal_y) / 2.,
z=state[2] + 50)
# configure the spectator
self.world.get_spectator().set_transform(
carla.Transform(location,
carla.Rotation(pitch=-70, yaw=-90, roll=20)))
record_spectator = False
if record_spectator:
# attach camera to spectator
spectator_camera = attach_camera_to_spectator(
self.world, frame)
n_burn_frames = self.scenario_params.n_burn_interval * online_config.record_interval
if self.ctrl_params.loop_type == LoopEnum.CLOSED_LOOP:
run_frames = self.scenario_params.run_interval * online_config.record_interval
else:
run_frames = self.ctrl_params.control_horizon * online_config.record_interval - 1
for idx in range(n_burn_frames + run_frames):
control = None
for ctrl in self.scenario_params.controls:
if ctrl.interval[0] <= idx and idx <= ctrl.interval[1]:
control = ctrl.control
break
frame = self.world.tick()
agent.run_step(frame, control=control)
finally:
if spectator_camera:
spectator_camera.destroy()
if agent:
agent.destroy()
if ego_vehicle:
ego_vehicle.destroy()
for other_vehicle in other_vehicles:
other_vehicle.destroy()
if record_spectator == True:
time.sleep(5)
else:
time.sleep(1)
def test_2_data_collectors(carla_Town03_synchronous):
"""Test case where NPCs move away from the EGO vehicle and disappear.
"""
client, world, carla_map, traffic_manager = carla_Town03_synchronous
vehicles = []
data_collectors = []
n_burn_frames = 40
save_frequency = 40
record_interval = 5
scene_interval = 30
try:
# Mock vehicles
spawn_points = carla_map.get_spawn_points()
vehicle_ids = []
blueprints = get_all_vehicle_blueprints(world)
for idx in [7, 187, 122, 221]:
blueprint = np.random.choice(blueprints)
spawn_point = spawn_points[idx]
vehicle = world.spawn_actor(blueprint, spawn_point)
vehicle.set_autopilot(True, traffic_manager.get_port())
vehicles.append(vehicle)
vehicle_ids.append(vehicle.id)
# Setup data collector
map_reader = NaiveMapQuerier(world, carla_map, debug=True)
scene_config = SceneConfig(scene_interval=scene_interval,
record_interval=record_interval)
data_collector = DataCollector(vehicles[0],
map_reader,
vehicle_ids[1:],
scene_builder_cls=TESTSceneBuilder,
scene_config=scene_config,
save_frequency=save_frequency,
n_burn_frames=n_burn_frames,
debug=True)
data_collector.start_sensor()
data_collectors.append(data_collector)
data_collector = DataCollector(vehicles[1],
map_reader,
vehicle_ids[:1] + vehicle_ids[2:],
scene_builder_cls=TESTSceneBuilder,
scene_config=scene_config,
save_frequency=save_frequency,
n_burn_frames=n_burn_frames,
debug=True)
data_collector.start_sensor()
data_collectors.append(data_collector)
# Run simulation for X steps
for idx in range(n_burn_frames + record_interval *
(save_frequency - 1)):
frame = world.tick()
for data_collector in data_collectors:
data_collector.capture_step(frame)
finally:
for data_collector in data_collectors:
data_collector.destroy()
for vehicle in vehicles:
vehicle.destroy()
def run(self):
ego_vehicle = None
agent = None
other_vehicles = []
try:
map_reader = NaiveMapQuerier(self.world,
self.carla_map,
debug=True)
online_config = OnlineConfig(record_interval=10,
node_type=self.eval_env.NodeType)
# Mock vehicles
spawn_points = self.carla_map.get_spawn_points()
blueprints = get_all_vehicle_blueprints(self.world)
spawn_indices = [self.scenario_params.ego_spawn_idx
] + self.scenario_params.other_spawn_ids
other_vehicle_ids = []
for k, spawn_idx in enumerate(spawn_indices):
if k == 0:
blueprint = self.world.get_blueprint_library().find(
'vehicle.audi.a2')
else:
blueprint = np.random.choice(blueprints)
spawn_point = spawn_points[spawn_idx]
spawn_point = shift_spawn_point(
self.carla_map, k, self.scenario_params.spawn_shifts,
spawn_point)
# Prevent collision with road.
spawn_point.location += carla.Location(0, 0, 0.5)
vehicle = self.world.spawn_actor(blueprint, spawn_point)
if k == 0:
ego_vehicle = vehicle
else:
other_vehicles.append(vehicle)
other_vehicle_ids.append(vehicle.id)
vehicle.set_autopilot(True, self.traffic_manager.get_port())
# if self.scenario_params.ignore_signs:
# self.traffic_manager.ignore_signs_percentage(vehicle, 100.)
if self.scenario_params.ignore_lights:
self.traffic_manager.ignore_lights_percentage(
vehicle, 100.)
# if self.scenario_params.ignore_vehicles:
# self.traffic_manager.ignore_vehicles_percentage(vehicle, 100.)
if not self.scenario_params.auto_lane_change:
self.traffic_manager.auto_lane_change(vehicle, False)
# Set up data collecting vehicle.
frame = self.world.tick()
agent = CapturingAgent(ego_vehicle,
map_reader,
other_vehicle_ids,
self.eval_stg,
scene_builder_cls=self.scene_builder_cls,
scene_config=online_config,
**self.scenario_params,
**self.ctrl_params)
agent.start_sensor()
assert agent.sensor_is_listening
# Set up autopilot routes
for k, vehicle in enumerate([ego_vehicle] + other_vehicles):
route = None
try:
route = self.scenario_params.other_routes[k]
len(route)
except (TypeError, IndexError):
continue
try:
self.traffic_manager.set_route(vehicle, route)
except AttributeError:
break
locations = carlautil.to_locations_ndarray([ego_vehicle] +
other_vehicles)
location = carlautil.ndarray_to_location(np.mean(locations,
axis=0))
location += carla.Location(z=50)
rotation = carla.Rotation(pitch=-90, yaw=0, roll=0)
self.world.get_spectator().set_transform(
carla.Transform(location, rotation))
n_burn_frames = self.scenario_params.n_burn_interval * online_config.record_interval
run_frames = self.scenario_params.run_interval * online_config.record_interval
for idx in range(n_burn_frames + run_frames):
frame = self.world.tick()
agent.run_step(frame)
finally:
if agent:
agent.destroy()
if ego_vehicle:
ego_vehicle.destroy()
for other_vehicle in other_vehicles:
other_vehicle.destroy()
time.sleep(1)
def scenario(scenario_params, agent_constructor, ctrl_params,
carla_synchronous, eval_env):
ego_vehicle = None
agent = None
client, world, carla_map, traffic_manager = carla_synchronous
try:
map_reader = NaiveMapQuerier(
world, carla_map, debug=True)
online_config = OnlineConfig(node_type=eval_env.NodeType)
# Mock vehicles
spawn_points = carla_map.get_spawn_points()
blueprint = world.get_blueprint_library().find('vehicle.audi.a2')
spawn_point = spawn_points[scenario_params.ego_spawn_idx]
spawn_point = shift_spawn_point(carla_map,
0, [scenario_params.spawn_shift], spawn_point)
spawn_point.location += carla.Location(0, 0, 0.5)
ego_vehicle = world.spawn_actor(blueprint, spawn_point)
world.tick()
agent = agent_constructor(
ego_vehicle,
map_reader,
n_burn_interval=scenario_params.n_burn_interval,
control_horizon=ctrl_params.control_horizon,
step_horizon=ctrl_params.step_horizon,
turn_choices=scenario_params.turn_choices,
max_distance=scenario_params.max_distance,
log_agent=False,
log_cplex=False,
plot_simulation=True,
plot_boundary=False,
road_boundary_constraints=True,
scene_config=online_config)
# if scenario_params.goal:
# agent.set_goal(scenario_params.goal.x, scenario_params.goal.y,
# is_relative=scenario_params.goal.is_relative)
"""Move the spectator to the ego vehicle.
The positioning is a little off"""
state = agent.get_vehicle_state()
goal = agent.get_goal()
goal_x, goal_y = goal.x, -goal.y
if goal.is_relative:
location = carla.Location(
x=state[0] + goal_x /2.,
y=state[1] - goal_y /2.,
z=state[2] + 50)
else:
location = carla.Location(
x=(state[0] + goal_x) /2.,
y=(state[1] + goal_y) /2.,
z=state[2] + 50)
world.get_spectator().set_transform(
carla.Transform(
location,
carla.Rotation(pitch=-90)
)
)
n_burn_frames = scenario_params.n_burn_interval*online_config.record_interval
if ctrl_params.loop_type == LoopEnum.CLOSED_LOOP:
run_frames = scenario_params.run_interval*online_config.record_interval + 1
else:
run_frames = ctrl_params.control_horizon*online_config.record_interval + 1
for idx in range(n_burn_frames + run_frames):
control = None
for ctrl in scenario_params.controls:
if ctrl.interval[0] <= idx and idx <= ctrl.interval[1]:
control = ctrl.control
break
frame = world.tick()
agent.run_step(frame, control=control)
finally:
if agent:
agent.destroy()
if ego_vehicle:
ego_vehicle.destroy()
time.sleep(1)
def scenario(carla_synchronous, scenario_params):
n_burn_frames, save_frequency, record_interval, scene_interval, \
collector_spawn, npc_spawn, spawn_shifts = scenario_params
client, world, carla_map, traffic_manager = carla_synchronous
vehicles = []
data_collectors = []
env = Environment(node_type_list=['VEHICLE'],
standardization=standardization)
scenes = []
def add_scene(scene):
scenes.append(scene)
try:
# Mock vehicles
spawn_points = carla_map.get_spawn_points()
blueprints = get_all_vehicle_blueprints(world)
spawn_indices = collector_spawn + npc_spawn
vehicle_ids = []
for k, spawn_idx in enumerate(spawn_indices):
blueprint = np.random.choice(blueprints)
spawn_point = spawn_points[spawn_idx]
spawn_point = shift_spawn_point(carla_map, k, spawn_shifts,
spawn_point)
# Prevent collision with road.
spawn_point.location += carla.Location(0, 0, 0.5)
# carlautil.debug_point(client, spawn_point.location, t=5.0)
vehicle = world.spawn_actor(blueprint, spawn_point)
vehicle.set_autopilot(True, traffic_manager.get_port())
vehicles.append(vehicle)
vehicle_ids.append(vehicle.id)
# Setup data collector
map_reader = NaiveMapQuerier(world, carla_map, debug=True)
scene_config = SceneConfig(scene_interval=scene_interval,
record_interval=record_interval,
node_type=env.NodeType)
n_data_collector = len(collector_spawn)
for collector_idx in range(n_data_collector):
other_vehicle_ids = vehicle_ids[:collector_idx] + vehicle_ids[
collector_idx + 1:]
data_collector = DataCollector(vehicles[collector_idx],
map_reader,
other_vehicle_ids,
scene_builder_cls=TESTSceneBuilder,
scene_config=scene_config,
save_frequency=save_frequency,
n_burn_frames=n_burn_frames,
callback=add_scene,
debug=True)
data_collector.start_sensor()
assert data_collector.sensor_is_listening
data_collectors.append(data_collector)
# Run simulation for X steps
for idx in range(n_burn_frames + record_interval *
(save_frequency - 1)):
frame = world.tick()
for data_collector in data_collectors:
data_collector.capture_step(frame)
assert len(scenes) == n_data_collector
for scene in scenes:
plot_trajectron_scene('out', scene)
finally:
for data_collector in data_collectors:
data_collector.destroy()
for vehicle in vehicles:
vehicle.destroy()
def test_2_data_collectors(carla_Town03_synchronous):
"""Test case where NPCs move away from the EGO vehicle and disappear.
"""
client, world, carla_map, traffic_manager = carla_Town03_synchronous
vehicles = []
data_collectors = []
env = Environment(node_type_list=['VEHICLE'],
standardization=standardization)
scenes = []
def add_scene(scene):
scenes.append(scene)
n_burn_frames = 40
save_frequency = 40
record_interval = 5
scene_interval = 30
try:
# Mock vehicles
spawn_points = carla_map.get_spawn_points()
vehicle_ids = []
blueprints = get_all_vehicle_blueprints(world)
for idx in [7, 187, 122, 221]:
blueprint = np.random.choice(blueprints)
spawn_point = spawn_points[idx]
vehicle = world.spawn_actor(blueprint, spawn_point)
vehicle.set_autopilot(True, traffic_manager.get_port())
vehicles.append(vehicle)
vehicle_ids.append(vehicle.id)
# Setup data collector
map_reader = NaiveMapQuerier(world, carla_map, debug=True)
scene_config = SceneConfig(scene_interval=scene_interval,
record_interval=record_interval)
data_collector = DataCollector(vehicles[0],
map_reader,
vehicle_ids[1:],
scene_builder_cls=TESTSceneBuilder,
scene_config=scene_config,
save_frequency=save_frequency,
n_burn_frames=n_burn_frames,
callback=add_scene,
debug=True)
data_collector.start_sensor()
data_collectors.append(data_collector)
data_collector = DataCollector(vehicles[1],
map_reader,
vehicle_ids[:1] + vehicle_ids[2:],
scene_builder_cls=TESTSceneBuilder,
scene_config=scene_config,
save_frequency=save_frequency,
n_burn_frames=n_burn_frames,
callback=add_scene,
debug=True)
data_collector.start_sensor()
data_collectors.append(data_collector)
# Run simulation for X steps
for idx in range(n_burn_frames + record_interval *
(save_frequency - 1)):
frame = world.tick()
for data_collector in data_collectors:
data_collector.capture_step(frame)
assert len(scenes) == 2
for scene in scenes:
fig, ax = plt.subplots(figsize=(15, 15))
# extent = (scene.x_min, scene.x_max, scene.y_min, scene.y_max)
ax.imshow(scene.map['VEHICLE'].as_image(), origin='lower')
spectral = cm.nipy_spectral(np.linspace(0, 1, len(scene.nodes)))
for idx, node in enumerate(scene.nodes):
# using values from scene.map['VEHICLE'].homography
# to scale points
xy = 3 * node.data.data[:, :2]
ax.scatter(xy[:, 0], xy[:, 1], color=spectral[idx])
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_aspect('equal')
fig.tight_layout()
fn = f"{ scene.name.replace('/', '_') }.png"
fp = os.path.join('out', fn)
fig.savefig(fp)
finally:
for data_collector in data_collectors:
data_collector.destroy()
for vehicle in vehicles:
vehicle.destroy()
def test_1_npc_appear(carla_Town03_synchronous):
"""Test case where NPC appears before EGO vehicle after awhile.
"""
client, world, carla_map, traffic_manager = carla_Town03_synchronous
ego_vehicle = None
other_vehicles = []
data_collector = None
env = Environment(node_type_list=['VEHICLE'],
standardization=standardization)
scenes = []
def add_scene(scene):
scenes.append(scene)
n_burn_frames = 160
save_frequency = 40
record_interval = 5
scene_interval = 30
try:
# Mock vehicles
spawn_points = carla_map.get_spawn_points()
blueprint = world.get_blueprint_library().find('vehicle.audi.a2')
spawn_point = spawn_points[251]
ego_vehicle = world.spawn_actor(blueprint, spawn_point)
ego_vehicle.set_autopilot(True, traffic_manager.get_port())
other_vehicle_ids = []
blueprints = get_all_vehicle_blueprints(world)
for idx in [164]:
blueprint = np.random.choice(blueprints)
spawn_point = spawn_points[idx]
other_vehicle = world.spawn_actor(blueprint, spawn_point)
other_vehicle.set_autopilot(True, traffic_manager.get_port())
other_vehicles.append(other_vehicle)
other_vehicle_ids.append(other_vehicle.id)
# Setup data collector
map_reader = NaiveMapQuerier(world, carla_map, debug=True)
scene_config = SceneConfig(scene_interval=scene_interval,
record_interval=record_interval)
data_collector = DataCollector(ego_vehicle,
map_reader,
other_vehicle_ids,
scene_builder_cls=TESTSceneBuilder,
scene_config=scene_config,
save_frequency=save_frequency,
n_burn_frames=n_burn_frames,
callback=add_scene,
debug=True)
data_collector.start_sensor()
assert data_collector.sensor_is_listening
# Run simulation for X steps
for idx in range(n_burn_frames + record_interval *
(save_frequency - 1)):
frame = world.tick()
data_collector.capture_step(frame)
assert len(scenes) == 1
scene = scenes[0]
fig, ax = plt.subplots(figsize=(15, 15))
# extent = (scene.x_min, scene.x_max, scene.y_min, scene.y_max)
ax.imshow(scene.map['VEHICLE'].as_image(), origin='lower')
spectral = cm.nipy_spectral(np.linspace(0, 1, len(scene.nodes)))
for idx, node in enumerate(scene.nodes):
# using values from scene.map['VEHICLE'].homography
# to scale points
xy = 3 * node.data.data[:, :2]
ax.scatter(xy[:, 0], xy[:, 1], color=spectral[idx])
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_aspect('equal')
fig.tight_layout()
fn = f"{ scene.name.replace('/', '_') }.png"
fp = os.path.join('out', fn)
fig.savefig(fp)
finally:
if data_collector:
data_collector.destroy()
if ego_vehicle:
ego_vehicle.destroy()
for other_vehicle in other_vehicles:
other_vehicle.destroy()