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 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 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)
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 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 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