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