class NGSimLaneChangeScenario(Scenario):
"""
Possible improvements:
- include bikes in CARLA to model NGSim motorcycles
"""
def __init__(
self,
ngsim_dataset: NGSimDataset,
*,
dataset_mode: DatasetMode,
data_dir,
reward_type: RewardType,
client: carla.Client,
seed=None
):
super().__init__(client)
setup_carla_settings(client, synchronous=True, time_delta_s=DT)
self._ngsim_recording = NGSimRecording(
data_dir=data_dir, ngsim_dataset=ngsim_dataset,
)
self._ngsim_dataset = ngsim_dataset
self._ngsim_vehicles_in_carla = None
self._dataset_mode = dataset_mode
self._early_stop_monitor: Optional[EarlyStopMonitor] = None
self._progress_monitor: Optional[LaneChangeProgressMonitor] = None
self._lane_alignment_monitor = LaneAlignmentMonitor(lane_alignment_frames=TARGET_LANE_ALIGNMENT_FRAMES,
cross_track_error_tolerance=CROSSTRACK_ERROR_TOLERANCE,
yaw_deg_error_tolerance=YAW_DEG_ERRORS_TOLERANCE)
def determine_split(lane_change_instant: LaneChangeInstant) -> DatasetMode:
split_frac = 0.8
hash_num = int(hashlib.sha1(str(lane_change_instant).encode('utf-8')).hexdigest(), 16)
if (hash_num % 100) / 100 < split_frac:
return DatasetMode.TRAIN
else:
return DatasetMode.VALIDATION
self._lane_change_instants = [
lci for lci in self._ngsim_recording.lane_change_instants if determine_split(lci) == dataset_mode
]
LOGGER.info(
f"Got {len(self._lane_change_instants)} lane change subscenarios "
f"in {ngsim_dataset.name}_{dataset_mode.name}")
self._reward_type = reward_type
def reset(self, vehicle: carla.Vehicle):
if self._ngsim_vehicles_in_carla:
self._ngsim_vehicles_in_carla.close()
self._ngsim_vehicles_in_carla = RealTrafficVehiclesInCarla(self._client, self._world)
if self._early_stop_monitor:
self._early_stop_monitor.close()
timeout_s = (FRAMES_BEFORE_MANUVEUR + FRAMES_AFTER_MANUVEUR) * DT
self._early_stop_monitor = EarlyStopMonitor(vehicle, timeout_s=timeout_s)
while True:
epseed = os.environ.get("epseed")
if epseed:
random.seed(int(epseed))
self._lane_change: LaneChangeInstant = random.choice(self._lane_change_instants)
self._sampled_dataset_excerpt_info = dict(
episode_seed=epseed,
file_suffix=self._lane_change.timeslot.file_suffix,
frame_start=self._lane_change.frame_start,
original_veh_id=self._lane_change.vehicle_id
)
frame_manuveur_start = max(self._lane_change.frame_start - FRAMES_BEFORE_MANUVEUR, 0)
self._ngsim_recording.reset(timeslot=self._lane_change.timeslot, frame=frame_manuveur_start - 1)
ngsim_vehicles = self._ngsim_recording.step()
agent_ngsim_vehicle = find_first_matching(ngsim_vehicles, lambda v: v.id == self._lane_change.vehicle_id)
t = agent_ngsim_vehicle.transform
self._start_lane_waypoint = self._world_map.get_waypoint(t.as_carla_transform().location)
self._target_lane_waypoint = {
ChauffeurCommand.CHANGE_LANE_LEFT: self._start_lane_waypoint.get_left_lane,
ChauffeurCommand.CHANGE_LANE_RIGHT: self._start_lane_waypoint.get_right_lane,
}[self._lane_change.chauffeur_command]()
if self._start_lane_waypoint and self._target_lane_waypoint:
self._start_lane_ids = get_lane_ids(self._start_lane_waypoint)
self._target_lane_ids = get_lane_ids(self._target_lane_waypoint)
assert not (set(self._start_lane_ids) & set(self._target_lane_ids)) # ensure disjoint sets of ids
break
self._lane_alignment_monitor.reset()
self._progress_monitor = LaneChangeProgressMonitor(self._world_map,
start_lane_ids=self._start_lane_ids,
target_lane_ids=self._target_lane_ids,
lane_change_command=self._lane_change.chauffeur_command)
vehicle.set_transform(t.as_carla_transform())
v = t.orientation * agent_ngsim_vehicle.speed * PIXELS_TO_METERS
vehicle.set_velocity(v.to_vector3(0).as_carla_vector3d()) # meters per second,
other_ngsim_vehicles = [v for v in ngsim_vehicles if v.id != self._lane_change.vehicle_id]
self._ngsim_vehicles_in_carla.step(other_ngsim_vehicles)
def step(self, ego_vehicle: carla.Vehicle) -> ScenarioStepResult:
ngsim_vehicles = self._ngsim_recording.step()
other_ngsim_vehicles = []
original_veh_transform = None
for veh in ngsim_vehicles:
if veh.id == self._lane_change.vehicle_id:
original_veh_transform = veh.transform.as_carla_transform()
else:
other_ngsim_vehicles.append(veh)
self._ngsim_vehicles_in_carla.step(other_ngsim_vehicles)
ego_transform = ego_vehicle.get_transform()
waypoint = self._world_map.get_waypoint(ego_transform.location)
on_start_lane = False
on_target_lane = False
if waypoint:
lane_id = get_lane_id(waypoint)
on_start_lane = lane_id in self._start_lane_ids
on_target_lane = lane_id in self._target_lane_ids
not_on_expected_lanes = not (on_start_lane or on_target_lane)
chauffeur_command = self._lane_change.chauffeur_command if on_start_lane else ChauffeurCommand.LANE_FOLLOW
scenario_finished_with_success = on_target_lane and \
self._lane_alignment_monitor.is_lane_aligned(ego_transform, waypoint.transform)
early_stop = EarlyStop.NONE
if not scenario_finished_with_success:
early_stop = self._early_stop_monitor(ego_transform)
if not_on_expected_lanes:
early_stop |= EarlyStop.MOVED_TOO_FAR
done = scenario_finished_with_success | bool(early_stop)
reward = int(self._reward_type == RewardType.DENSE) * self._progress_monitor.get_progress_change(ego_transform)
reward += int(scenario_finished_with_success)
reward += int(bool(early_stop)) * -1
done_info = {}
if done and scenario_finished_with_success:
done_info[DONE_CAUSE_KEY] = 'success'
elif done and early_stop:
done_info[DONE_CAUSE_KEY] = early_stop.decomposed_name('_').lower()
info = {
'ngsim_dataset': {
'road': self._ngsim_dataset.name,
'timeslice': self._lane_change.timeslot.file_suffix,
'frame': self._ngsim_recording.frame,
'dataset_mode': self._dataset_mode.name,
},
'scenario_data': {
'ego_veh': ego_vehicle,
'original_veh_transform': original_veh_transform,
'original_to_ego_distance': original_veh_transform.location.distance(ego_transform.location)
},
'reward_type': self._reward_type.name,
'on_start_lane': on_start_lane,
'on_target_lane': on_target_lane,
'is_junction': waypoint.is_junction if waypoint else False,
**self._lane_alignment_monitor.info(),
**done_info
}
return ScenarioStepResult(chauffeur_command, reward, done, info)
def close(self):
if self._early_stop_monitor:
self._early_stop_monitor.close()
self._early_stop_monitor = None
if self._ngsim_vehicles_in_carla:
self._ngsim_vehicles_in_carla.close()
self._ngsim_vehicles_in_carla = None
self._lane_change_instants = []
self._lane_change = None
del self._ngsim_recording
self._ngsim_recording = None
class OpenDDScenario(Scenario):
def __init__(
self,
client: carla.Client,
*,
dataset_dir: Union[str, Path],
reward_type: RewardType,
dataset_mode: DatasetMode,
place_name: Optional[str] = None,
):
super().__init__(client)
setup_carla_settings(client, synchronous=True, time_delta_s=DT)
self._dataset = OpenDDDataset(dataset_dir)
self._recording = OpenDDRecording(dataset=self._dataset,
dataset_mode=dataset_mode)
self._dataset_mode = dataset_mode
self._reward_type = reward_type
self._place_name = place_name
self._chauffeur: Optional[Chauffeur] = None
self._early_stop_monitor: Optional[EarlyStopMonitor] = None
self._carla_sync: Optional[RealTrafficVehiclesInCarla] = None
self._current_progress = 0
def reset(self, ego_vehicle: carla.Vehicle):
if self._carla_sync:
self._carla_sync.close()
self._carla_sync = RealTrafficVehiclesInCarla(self._client,
self._world)
if self._early_stop_monitor:
self._early_stop_monitor.close()
session_names = self._dataset.session_names
if self._place_name:
session_names = [
n for n in session_names if self._place_name.lower() in n
]
epseed = os.environ.get("epseed")
if epseed:
epseed = int(epseed)
random.seed(epseed)
session_name = random.choice(session_names)
# Another random is used inside
ego_id, timestamp_start_s, timestamp_end_s = self._recording.reset(
session_name=session_name, seed=epseed)
self._sampled_dataset_excerpt_info = dict(
episode_seed=epseed,
session_name=session_name,
timestamp_start_s=timestamp_start_s,
timestamp_end_s=timestamp_end_s,
original_veh_id=ego_id,
)
env_vehicles = self._recording.step()
other_vehicles = [v for v in env_vehicles if v.id != ego_id]
self._carla_sync.step(other_vehicles)
opendd_ego_vehicle = self._recording._env_vehicles[ego_id]
opendd_ego_vehicle.set_end_of_trajectory_timestamp(timestamp_end_s)
self._chauffeur = Chauffeur(opendd_ego_vehicle,
self._recording.place.roads_utm)
ego_vehicle.set_transform(
opendd_ego_vehicle.transform_carla.as_carla_transform())
ego_vehicle.set_velocity(
opendd_ego_vehicle.velocity.as_carla_vector3d())
self._current_progress = 0
trajectory_carla = [
t.as_carla_transform() for t in opendd_ego_vehicle.trajectory_carla
]
self._trajectory = Trajectory(trajectory_carla=trajectory_carla)
timeout_s = (timestamp_end_s - timestamp_start_s) * 1.5
timeout_s = min(timeout_s,
self._recording._timestamps[-1] - timestamp_start_s)
self._early_stop_monitor = EarlyStopMonitor(
ego_vehicle,
timeout_s=timeout_s,
trajectory=self._trajectory,
max_trajectory_distance_m=MAX_DISTANCE_FROM_REF_TRAJECTORY_M)
def step(self, ego_vehicle: carla.Vehicle) -> ScenarioStepResult:
ego_transform = ego_vehicle.get_transform()
original_veh_transform = self._chauffeur.vehicle.transform_carla.as_carla_transform(
)
progress = self._get_progress(ego_transform)
progress_change = max(
0,
_quantify_progress(progress) -
_quantify_progress(self._current_progress))
self._current_progress = progress
scenario_finished_with_success = progress >= 1.0
early_stop = EarlyStop.NONE
if not scenario_finished_with_success:
early_stop = self._early_stop_monitor(ego_transform)
if self._recording.has_finished:
early_stop |= EarlyStop.TIMEOUT
done = scenario_finished_with_success | bool(early_stop)
reward = int(self._reward_type == RewardType.DENSE) * progress_change
reward += int(scenario_finished_with_success)
reward += int(bool(early_stop)) * -1
cmd = self._chauffeur.get_cmd(ego_transform)
done_info = {}
if done and scenario_finished_with_success:
done_info[DONE_CAUSE_KEY] = 'success'
elif done and early_stop:
done_info[DONE_CAUSE_KEY] = early_stop.decomposed_name('_').lower()
info = {
'opendd_dataset': {
'session': self._recording.session_name,
'timestamp_s': f'{self._recording.timestamp_s:0.3f}',
'objid': self._chauffeur.vehicle.id,
'dataset_mode': self._dataset_mode.name,
},
'scenario_data': {
'ego_veh':
ego_vehicle,
'original_veh_transform':
original_veh_transform,
'original_to_ego_distance':
original_veh_transform.location.distance(
ego_transform.location)
},
'reward_type': self._reward_type.name,
**done_info
}
env_vehicles = self._recording.step()
other_vehicles = [
v for v in env_vehicles if v.id != self._chauffeur.vehicle.id
]
self._carla_sync.step(other_vehicles)
return ScenarioStepResult(cmd, reward, done, info)
def close(self):
if self._early_stop_monitor:
self._early_stop_monitor.close()
self._early_stop_monitor = None
if self._carla_sync:
self._carla_sync.close()
if self._recording:
self._recording.close()
del self._recording
self._recording = None
def _get_progress(self, ego_transform: carla.Transform):
s, *_ = self._trajectory.find_nearest_trajectory_point(ego_transform)
return s / self._trajectory.total_length_m
class ArtificialLaneChangeScenario(Scenario):
def __init__(self, *, client: carla.Client, cmd_for_changing_lane=ChauffeurCommand.CHANGE_LANE_LEFT,
speed_range_token: str, no_columns=True, reward_type: RewardType = RewardType.DENSE):
super().__init__(client=client)
start_point = Transform(Vector3(-144.4, -22.41, 0), Vector2(-1.0, 0.0))
self._find_lane_waypoints(cmd_for_changing_lane, start_point.position.as_carla_location())
self._cmd_for_changing_lane = cmd_for_changing_lane
self._reward_type = reward_type
self._progress_monitor: Optional[LaneChangeProgressMonitor] = None
self._lane_alignment_monitor = LaneAlignmentMonitor(lane_alignment_frames=TARGET_LANE_ALIGNMENT_FRAMES,
cross_track_error_tolerance=CROSSTRACK_ERROR_TOLERANCE,
yaw_deg_error_tolerance=YAW_DEG_ERRORS_TOLERANCE)
self._early_stop_monitor: Optional[EarlyStopMonitor] = None
# vehicles shall fill bird view + first vehicle shall reach end of route forward part
# till the last one reaches bottom of the bird view; assume just VEHICLE_SLOT_LENGTH_M spacing
max_env_vehicles_number = int(BIRD_VIEW_HEIGHT_M * 1.2 // VEHICLE_SLOT_LENGTH_M)
env_vehicles = [] if no_columns else self._spawn_env_vehicles(max_env_vehicles_number)
self._controllers = self._wrap_with_controllers(env_vehicles)
env_vehicles_speed_range_mps = SPEED_RANGE_NAMES[speed_range_token]
self._speed_range_mps = env_vehicles_speed_range_mps
self._env_vehicle_column_ahead_range_m = (30, 50)
route_length_m = max(MAX_MANEUVER_LENGTH_M + BIRD_VIEW_HEIGHT_M,
max_env_vehicles_number * (MAX_VEHICLE_RANDOM_SPACE_M + VEHICLE_SLOT_LENGTH_M)) * 3
self._topology = Topology(self._world_map)
self._routes: List[List[carla.Transform]] = self._obtain_routes(self._target_lane_waypoint, route_length_m)
def _obtain_routes(self, pass_through_waypoint: carla.Waypoint, total_length_m: float) \
-> List[List[carla.Transform]]:
part_length_m = total_length_m / 2
forward_routes = self._topology.get_forward_routes(pass_through_waypoint, part_length_m)
backward_routes = self._topology.get_backward_routes(pass_through_waypoint, part_length_m)
routes = [
backward_route + forward_route
for backward_route in backward_routes for forward_route in forward_routes
]
return [list(unique_justseen(r)) for r in routes]
def _find_lane_waypoints(self, cmd_for_changing_lane, start_location: carla.Location):
self._start_lane_waypoint = self._world_map.get_waypoint(start_location)
if self._start_lane_waypoint is None:
raise RuntimeError(f'Could not find matching lane for starting location {start_location}')
self._target_lane_waypoint = {
ChauffeurCommand.CHANGE_LANE_LEFT: self._start_lane_waypoint.get_left_lane,
ChauffeurCommand.CHANGE_LANE_RIGHT: self._start_lane_waypoint.get_right_lane,
}[cmd_for_changing_lane]()
if self._target_lane_waypoint is None:
raise RuntimeError(f'Could not find {cmd_for_changing_lane} lane for {start_location} location')
self._start_lane_ids = get_lane_ids(self._start_lane_waypoint, max_distances=(300, 10))
self._target_lane_ids = get_lane_ids(self._target_lane_waypoint, max_distances=(300, 10))
common_lanes = set(self._start_lane_ids) & set(self._target_lane_ids)
assert not (common_lanes) # ensure disjoint sets of ids
def _spawn_env_vehicles(self, n: int):
blueprints = self._world.get_blueprint_library().filter('vehicle.*')
blueprints = [b for b in blueprints if int(b.get_attribute('number_of_wheels')) == 4]
spawn_points = self._world_map.get_spawn_points()
max_ticks = 4
def _get_env_vehicles():
return [
v for v in self._world.get_actors().filter('vehicle.*')
if v.attributes.get('role_name') != 'hero' and v.is_alive
]
def _wait_for_env_vehicles(total_number, max_ticks):
env_vehicles = _get_env_vehicles()
while len(env_vehicles) < total_number and max_ticks > 0:
self._world.tick()
max_ticks -= 1
env_vehicles = _get_env_vehicles()
return env_vehicles
env_vehicles = _get_env_vehicles()
missing_vehicles = n - len(env_vehicles)
while missing_vehicles:
cmds = [
carla.command.SpawnActor(blueprint, spawn_point)
for blueprint, spawn_point in zip(random.choices(blueprints, k=missing_vehicles),
random.sample(spawn_points, k=missing_vehicles))
]
self._client.apply_batch_sync(cmds, do_tick=True)
env_vehicles = _wait_for_env_vehicles(n, max_ticks)
missing_vehicles = n - len(env_vehicles)
return env_vehicles
def _wrap_with_controllers(self, env_vehicles):
return {
str(v.id): TeleportCommandsController(v)
for v in env_vehicles
}
def reset(self, ego_vehicle: carla.Vehicle):
if self._early_stop_monitor:
self._early_stop_monitor.close()
timeout_s = (FRAMES_BEFORE_MANUVEUR + FRAMES_AFTER_MANUVEUR) * DT
self._early_stop_monitor = EarlyStopMonitor(ego_vehicle, timeout_s=timeout_s)
self._route = random.choice(self._routes)
min_speed, max_speed = self._speed_range_mps
range_size = max_speed - min_speed
speed_mps = min_speed + np.random.random() * range_size
column_ahead_of_ego_m = np.random.randint(*self._env_vehicle_column_ahead_range_m)
cmds = self._setup_controllers(self._start_lane_waypoint.transform.location, speed_mps,
self._route, column_ahead_of_ego_m)
ego_speed_mps = min_speed + np.random.random() * range_size
t = Transform.from_carla_transform(self._start_lane_waypoint.transform)
velocity = (t.orientation * ego_speed_mps).to_vector3(0).as_carla_vector3d()
cmds.append(carla.command.ApplyTransform(ego_vehicle.id, transform=self._start_lane_waypoint.transform))
cmds.append(carla.command.ApplyVelocity(ego_vehicle.id, velocity))
if cmds:
self._client.apply_batch_sync(cmds, do_tick=False)
self._lane_alignment_monitor.reset()
self._progress_monitor = LaneChangeProgressMonitor(self._world_map,
start_lane_ids=self._start_lane_ids,
target_lane_ids=self._target_lane_ids,
lane_change_command=self._cmd_for_changing_lane)
def step(self, ego_vehicle: carla.Vehicle) -> ScenarioStepResult:
ego_transform = ego_vehicle.get_transform()
self._move_env_vehicles(ego_transform.location)
waypoint = self._world_map.get_waypoint(ego_transform.location)
on_start_lane = False
on_target_lane = False
if waypoint:
lane_id = get_lane_id(waypoint)
on_start_lane = lane_id in self._start_lane_ids
on_target_lane = lane_id in self._target_lane_ids
not_on_expected_lanes = not (on_start_lane or on_target_lane)
chauffeur_command = self._cmd_for_changing_lane if on_start_lane else ChauffeurCommand.LANE_FOLLOW
scenario_finished_with_success = on_target_lane and \
self._lane_alignment_monitor.is_lane_aligned(ego_transform, waypoint.transform)
early_stop = EarlyStop.NONE
if not scenario_finished_with_success:
early_stop = self._early_stop_monitor(ego_transform)
if not_on_expected_lanes:
early_stop |= EarlyStop.MOVED_TOO_FAR
done = scenario_finished_with_success | bool(early_stop)
reward = int(self._reward_type == RewardType.DENSE) * self._progress_monitor.get_progress_change(ego_transform)
reward += int(scenario_finished_with_success)
reward += int(bool(early_stop)) * -1
done_info = {}
if done and scenario_finished_with_success:
done_info[DONE_CAUSE_KEY] = 'success'
elif done and early_stop:
done_info[DONE_CAUSE_KEY] = EarlyStop(early_stop).decomposed_name('_').lower()
info = {
'reward_type': self._reward_type.name,
'on_start_lane': on_start_lane,
'on_target_lane': on_target_lane,
'is_junction': waypoint.is_junction if waypoint else False,
**self._lane_alignment_monitor.info(),
**done_info
}
return ScenarioStepResult(chauffeur_command, reward, done, info)
def _setup_controllers(self, ego_vehicle_location, speed_mps, route, column_ahead_of_ego_m):
resolution_m = np.median([t1.location.distance(t2.location) for t1, t2 in windowed(route, 2)])
m2idx = 1 / resolution_m
ego_vehicle_idx = int(np.argmin([t.location.distance(ego_vehicle_location) for t in route]))
column_start_idx = ego_vehicle_idx + int(column_ahead_of_ego_m * m2idx)
ncontrollers = len(self._controllers)
current_idx = column_start_idx
cmds = []
for idx, controller in enumerate(self._controllers.values()):
initial_location = route[current_idx].location
cmds.extend(controller.reset(speed_mps=speed_mps, route=route, initial_location=initial_location))
controllers_left = ncontrollers - idx
offset = _calc_offset(current_idx, controllers_left, resolution_m)
current_idx = current_idx - offset
assert current_idx >= 0
return cmds
def _move_env_vehicles(self, ego_vehicle_location: carla.Location):
column_end_location = None
column_end_idx = None
route_resolution_m = None
cmds = []
for controller_idx, controller in enumerate(self._controllers.values()):
finished, cmds_ = controller.step()
if not finished and _is_behind_ego_or_inside_birdview(controller, ego_vehicle_location):
cmds.extend(cmds_)
else:
if column_end_location is None:
# obtain location of last vehicle in column
idxes = [c.idx for c in self._controllers.values()]
locations = [c.location for c in self._controllers.values()]
column_end_location = locations[int(np.argmin(idxes))]
# resolution of not resampled route
route_resolution_m = \
np.median([t1.location.distance(t2.location) for t1, t2 in windowed(self._route, 2)])
# idx of nearest point on not resamples route
column_end_idx = np.argmin([t.location.distance(column_end_location) for t in self._route])
# obtain reset location
offset = _calc_offset(column_end_idx, 1, route_resolution_m)
column_end_idx = column_end_idx - offset
assert column_end_idx >= 0
column_end_location = self._route[column_end_idx].location
cmds.extend(controller.reset(initial_location=column_end_location))
if cmds:
self._client.apply_batch_sync(cmds, do_tick=False)
def close(self):
if self._early_stop_monitor:
self._early_stop_monitor.close()
self._early_stop_monitor = None