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 step(self, ego_vehicle: carla.Vehicle) -> ScenarioStepResult:
assert self._steps_to_reach_next_checkpoint is not None, (
"No more steps are allowed to be done in this episode."
"Must call `reset(ego_vehicle=...)` first"
)
ego_transform = ego_vehicle.get_transform()
ego_location = ego_transform.location
next_checkpoint = self._route[self._next_route_checkpoint_idx]
if DEBUG:
lightgreen_color = carla.Color(153, 255, 51)
for route_checkpoint in self._route:
debug_draw(
route_checkpoint.area,
self._world,
color=lightgreen_color,
life_time=1.0 / FPS,
)
debug_draw(next_checkpoint.area, self._world, life_time=0.01)
checkpoint_area = next_checkpoint.area
reward = 0
done = False
info = {}
if self._collided is True:
reward = 0
done = True
if ego_location in checkpoint_area:
if not self._sparse_reward_mode:
num_checkpoints_excluding_final = len(self._route) - 1
reward = 1 / num_checkpoints_excluding_final
self._command = next_checkpoint.command
self._steps_to_reach_next_checkpoint = MAX_NUM_STEPS_TO_REACH_CHECKPOINT
self._next_route_checkpoint_idx += 1
is_ego_offroad = (
self._world_map.get_waypoint(ego_location, project_to_road=False) is None
)
if is_ego_offroad:
reward = 0
done = True
if self._next_route_checkpoint_idx == len(self._route):
reward = 1
done = True
if self._steps_to_reach_next_checkpoint > 0:
self._steps_to_reach_next_checkpoint -= 1
else:
self._steps_to_reach_next_checkpoint = None
reward = 0
done = True
return ScenarioStepResult(self._command, reward, done, info)
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 step(self, ego_vehicle: carla.Vehicle) -> ScenarioStepResult:
ego_vehicle_transform = ego_vehicle.get_transform()
self._move_env_vehicles(ego_vehicle_transform.location)
ego_vehicle_transform = Transform.from_carla_transform(
ego_vehicle_transform)
current_lane_waypoint = self._world_map.get_waypoint(
ego_vehicle_transform.position.as_carla_location())
current_lane = get_lane_id(current_lane_waypoint)
# TODO: in fact there are many different lanes which are allowed to go
allowed_lanes = list(
map(get_lane_id,
[self._start_lane_waypoint, self._target_lane_waypoint]))
on_target_lane = current_lane == get_lane_id(
self._target_lane_waypoint)
offroad = current_lane not in allowed_lanes
info = {"target_lane_aligmnent_counter": self._done_counter}
chauffeur_cmd = self._cmd_for_changing_lane
done = offroad
if on_target_lane: # not finished yet
current_lane_transform = Transform.from_carla_transform(
current_lane_waypoint.transform)
crosstrack_error, yaw_error = self._calculate_errors(
current_lane_transform, ego_vehicle_transform)
aligned_with_target_lane = crosstrack_error < CROSSTRACK_ERROR_TOLERANCE and \
yaw_error < np.deg2rad(YAW_DEG_ERRORS_TOLERANCE)
if aligned_with_target_lane:
self._done_counter -= 1
else:
self._done_counter = TARGET_LANE_ALIGNMENT_FRAMES
chauffeur_cmd = ChauffeurCommand.LANE_FOLLOW
done = self._done_counter == 0
reward = int(done and not offroad)
return ScenarioStepResult(chauffeur_cmd, reward, done, info)
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 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)