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)
def reset(self, vehicle: carla.Vehicle): if self._collision_sensor: self._collision_sensor.destroy() self._collision_sensor = None self._collided = False if self._ngsim_vehicles_in_carla: self._ngsim_vehicles_in_carla.close() # attack collision sensor blueprint_library = self._world.get_blueprint_library() blueprint = blueprint_library.find('sensor.other.collision') self._collision_sensor = self._world.spawn_actor( blueprint, vehicle.get_transform(), attach_to=vehicle) def on_collided(e): self._collided = True self._collision_sensor.listen(on_collided) self._lane_change: LaneChangeInstant = random.choice( self._lane_change_instants) self._ngsim_vehicles_in_carla = RealTrafficVehiclesInCarla( self._client, self._world) self._target_alignment_counter = 0 self._previous_chauffeur_command = self._lane_change.chauffeur_command self._remaining_steps = FRAMES_BEFORE_MANUVEUR + FRAMES_AFTER_MANUVEUR 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) other_ngsim_vehicles = [ v for v in ngsim_vehicles if v.id != self._lane_change.vehicle_id ] t = agent_ngsim_vehicle.transform 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, self._ngsim_vehicles_in_carla.step(other_ngsim_vehicles)
def main(): import argparse parser = argparse.ArgumentParser( description='Replay real-traffic scenario on carla simulator') parser.add_argument( '--server', '-s', help='Address where carla simulator is listening; (host:[port])', required=False, default='localhost:2000') parser.add_argument('dataset', help='Name of dataset to replay') args = parser.parse_args() hostname, port = _parse_server_endpoint(args.server) carla_client = carla.Client(hostname, port) carla_client.set_timeout(60) carla_synchronizer = None try: simulator = create_simulator(args.dataset) print( "Trying to connect to CARLA server. Make sure its up and running.") world = carla_setup(carla_client, simulator.place_params.name) carla_synchronizer = RealTrafficVehiclesInCarla(carla_client, world) for vehicles in SimulatorIterator(simulator): carla_synchronizer.step(vehicles) world.tick() finally: if carla_synchronizer: carla_synchronizer.close()
def main(): ngsim_vehicles_in_carla = None try: ngsim_dataset = NGSimDatasets.US101 data_dir = '/home/adam/src/rl/xy-trajectories' ngsim_recording = NGSimRecording(data_dir, ngsim_dataset) ngsim_recording.reset(US101Timeslots.TIMESLOT_3, 1350) carla_client = carla.Client('localhost', 2000) carla_client.set_timeout(60) ngsim_dataset = NGSimDatasets.I80 print( "Trying to connect to CARLA server. Make sure its up and running.") world = carla_client.load_world(ngsim_dataset.carla_map.level_path) settings = world.get_settings() settings.synchronous_mode = True settings.fixed_delta_seconds = DT world.apply_settings(settings) ngsim_vehicles_in_carla = RealTrafficVehiclesInCarla( carla_client, world) for _ in range(10000): vehicles = ngsim_recording.step() ngsim_vehicles_in_carla.step(vehicles) world.tick() finally: if ngsim_vehicles_in_carla is not None: ngsim_vehicles_in_carla.close()
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 NGSimLaneChangeScenario(Scenario): """ Possible improvements: - include bikes in CARLA to model NGSim motorcycles """ def __init__(self, ngsim_dataset: NGSimDataset, dataset_mode: DatasetMode, data_dir, client: carla.Client): super().__init__(client) settings = self._world.get_settings() if not settings.synchronous_mode: LOGGER.warning( "Only synchronous_mode==True supported. Will change to synchronous_mode==True" ) settings.synchronous_mode = True if settings.fixed_delta_seconds != DT: LOGGER.warning( f"Only fixed_delta_seconds=={DT} supported. Will change to fixed_delta_seconds=={DT}" ) settings.fixed_delta_seconds = DT self._world.apply_settings(settings) self._ngsim_recording = NGSimRecording( data_dir=data_dir, ngsim_dataset=ngsim_dataset, ) self._ngsim_dataset = ngsim_dataset self._ngsim_vehicles_in_carla = None self._collision_sensor = None self._target_alignment_counter: int self._dataset_mode = dataset_mode 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 in {ngsim_dataset.name}_{dataset_mode.name}" ) def reset(self, vehicle: carla.Vehicle): if self._collision_sensor: self._collision_sensor.destroy() self._collision_sensor = None self._collided = False if self._ngsim_vehicles_in_carla: self._ngsim_vehicles_in_carla.close() # attack collision sensor blueprint_library = self._world.get_blueprint_library() blueprint = blueprint_library.find('sensor.other.collision') self._collision_sensor = self._world.spawn_actor( blueprint, vehicle.get_transform(), attach_to=vehicle) def on_collided(e): self._collided = True self._collision_sensor.listen(on_collided) self._lane_change: LaneChangeInstant = random.choice( self._lane_change_instants) self._ngsim_vehicles_in_carla = RealTrafficVehiclesInCarla( self._client, self._world) self._target_alignment_counter = 0 self._previous_chauffeur_command = self._lane_change.chauffeur_command self._remaining_steps = FRAMES_BEFORE_MANUVEUR + FRAMES_AFTER_MANUVEUR 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) other_ngsim_vehicles = [ v for v in ngsim_vehicles if v.id != self._lane_change.vehicle_id ] t = agent_ngsim_vehicle.transform 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, 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 = [ v for v in ngsim_vehicles if v.id != self._lane_change.vehicle_id ] self._ngsim_vehicles_in_carla.step(other_ngsim_vehicles) waypoint = self._world_map.get_waypoint( ego_vehicle.get_transform().location) done = False reward = 0 on_start_lane = waypoint.lane_id == self._ngsim_dataset.carla_lane_by_ngsim_lane( self._lane_change.lane_from) on_target_lane = waypoint.lane_id == self._ngsim_dataset.carla_lane_by_ngsim_lane( self._lane_change.lane_to) if on_start_lane: chauffeur_command = self._lane_change.chauffeur_command elif on_target_lane: chauffeur_command = ChauffeurCommand.LANE_FOLLOW crosstrack_error = distance_between_on_plane( ego_vehicle.get_transform().location, waypoint.transform.location) yaw_error = normalize_angle( np.deg2rad(ego_vehicle.get_transform().rotation.yaw - waypoint.transform.rotation.yaw)) aligned_with_target_lane = \ crosstrack_error < CROSSTRACK_ERROR_TOLERANCE and yaw_error < np.deg2rad(YAW_DEG_ERRORS_TOLERANCE) if aligned_with_target_lane: self._target_alignment_counter += 1 else: self._target_alignment_counter = 0 if self._target_alignment_counter == TARGET_LANE_ALIGNMENT_FRAMES: done = True reward = 1 else: # off road done = True chauffeur_command = ChauffeurCommand.LANE_FOLLOW if self._collided: done = True self._remaining_steps -= 1 if self._remaining_steps == 0: done = True self._previous_chauffeur_command = chauffeur_command 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 }, 'target_alignment_counter': self._target_alignment_counter, } return ScenarioStepResult(chauffeur_command, reward, done, info) def close(self): if self._ngsim_vehicles_in_carla: self._ngsim_vehicles_in_carla.close() self._ngsim_vehicles_in_carla = None if self._collision_sensor and self._collision_sensor.is_alive: self._collision_sensor.destroy() self._lane_change_instants = [] self._lane_change = None del self._ngsim_recording self._ngsim_recording = None