def get_nearest_traffic_light(vehicle: carla.Vehicle) -> Tuple[
carla.TrafficLight, float]:
"""
This method is specialized to check European style traffic lights.
:param lights_list: list containing TrafficLight objects
:return: a tuple given by (bool_flag, traffic_light), where
- bool_flag is True if there is a traffic light in RED
affecting us and False otherwise
- traffic_light is the object itself or None if there is no
red traffic light affecting us
"""
world = vehicle.get_world() # type: World
lights_list = world.get_actors().filter("*traffic_light*") # type: List[carla.TrafficLight]
ego_vehicle_location = vehicle.get_location()
"""
map = world.get_map()
ego_vehicle_waypoint = map.get_waypoint(ego_vehicle_location)
closest_traffic_light = None # type: Optional[carla.TrafficLight]
closest_traffic_light_distance = math.inf
for traffic_light in lights_list:
object_waypoint = map.get_waypoint(traffic_light.get_location())
if object_waypoint.road_id != ego_vehicle_waypoint.road_id or \
object_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
continue
distance_to_light = distance_transforms(traffic_light.get_transform(), vehicle.get_transform())
if distance_to_light < closest_traffic_light_distance:
closest_traffic_light = traffic_light
closest_traffic_light_distance = distance_to_light
return closest_traffic_light, closest_traffic_light_distance
"""
min_angle = 180.0
closest_traffic_light = None # type: Optional[carla.TrafficLight]
closest_traffic_light_distance = math.inf
min_rotation_diff = 0
for traffic_light in lights_list:
loc = traffic_light.get_location()
distance_to_light, angle = compute_magnitude_angle(loc,
ego_vehicle_location,
vehicle.get_transform().rotation.yaw)
rotation_diff = math.fabs(
vehicle.get_transform().rotation.yaw - (traffic_light.get_transform().rotation.yaw - 90))
if distance_to_light < closest_traffic_light_distance and angle < 90 and (rotation_diff < 30 or math.fabs(360 - rotation_diff) < 30):
closest_traffic_light_distance = distance_to_light
closest_traffic_light = traffic_light
min_angle = angle
min_rotation_diff = rotation_diff
# if closest_traffic_light is not None:
# print("Ego rot: ", vehicle.get_transform().rotation.yaw, "TL rotation: ", closest_traffic_light.get_transform().rotation.yaw, ", diff: ", min_rotation_diff, ", dist: ", closest_traffic_light_distance)
return closest_traffic_light, closest_traffic_light_distance
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 convert_vehicle_from_source_to_agent(self,
source: carla.Vehicle) -> Vehicle:
control: VehicleControl = self.convert_control_from_source_to_agent(
source.get_control())
# this is cheating here, vehicle does not know its own location
transform: Transform = self.convert_transform_from_source_to_agent(
source.get_transform())
velocity: Vector3D = self.convert_vector3d_from_source_to_agent(
source.get_velocity())
return Vehicle(velocity=velocity, transform=transform, control=control)
def behaviour_planner(self, leading_vehicle: carla.Vehicle,
trailing_vehicle: carla.Vehicle,
trailing_image_seg: carla.Image,
trail_cam_rgb: carla.Sensor, frame: int):
"""[summary]
Args:
leading_vehicle (carla.Vehicle): [description]
trailing_vehicle (carla.Vehicle): [description]
trailing_image_seg (carla.Image): [description]
trail_cam_rgb (carla.Sensor): [description]
Returns:
[type]: [description]
"""
# detect car in image with semantic segnmentation camera
# Car detection module
carInTheImage = self.semantic.IsThereACarInThePicture(
trailing_image_seg)
leading_location = leading_vehicle.get_transform()
trailing_location = trailing_vehicle.get_transform()
newX, newY = self.carDetector.CreatePointInFrontOFCar(
trailing_location.location.x, trailing_location.location.y,
trailing_location.rotation.yaw)
angle = self.carDetector.getAngle(
[trailing_location.location.x, trailing_location.location.y],
[newX, newY],
[leading_location.location.x, leading_location.location.y])
possibleAngle = 0
drivableIndexes = []
bbox, predicted_distance, predicted_angle = self.carDetector.getDistance(
leading_vehicle,
trail_cam_rgb,
carInTheImage,
extrapolation=self.extrapolate,
nOfFramesToSkip=self.nOfFramesToSkip)
if frame % self.behaviour_planner_frequency_divisor == 0:
# This is the bottle neck and takes times to run. But it is necessary for chasing around turns
predicted_angle, drivableIndexes = self.semantic.FindPossibleAngle(
trailing_image_seg, bbox, predicted_angle)
# Car detection module
steer, throttle = self.drivingControlAdvanced.PredictSteerAndThrottle(
predicted_distance, predicted_angle, None)
real_dist = trailing_location.location.distance(
leading_location.location)
return steer, throttle, real_dist
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 = NGSimVehiclesInCarla(
self._client, self._world, self._ngsim_dataset)
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
]
mapper = MAPPER_BY_NGSIM_DATASET[self._ngsim_dataset]
v_data = VEHICLE_BY_TYPE_ID[vehicle.type_id]
t = mapper.ngsim_to_carla(agent_ngsim_vehicle.get_transform(),
v_data.z_offset, v_data.rear_axle_offset)
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 vehicle_to_carla_measurements(
vehicle: carla.Vehicle, # pylint: disable=no-member
) -> Mapping[str, Any]:
"""Wraps all the `get_` calls from the `CARLA` interface."""
control = vehicle.get_control()
_transform = vehicle.get_transform()
location = _transform.location
rotation = _transform.rotation
velocity = vehicle.get_velocity()
acceleration = vehicle.get_acceleration()
orientation = _transform.get_forward_vector()
angular_velocity = vehicle.get_angular_velocity()
speed_limit = vehicle.get_speed_limit()
is_at_traffic_light = vehicle.is_at_traffic_light()
traffic_light_state = vehicle.get_traffic_light_state().conjugate()
return dict(
control=control,
location=location,
rotation=rotation,
velocity=velocity,
acceleration=acceleration,
orientation=orientation,
angular_velocity=angular_velocity,
speed_limit=speed_limit,
is_at_traffic_light=is_at_traffic_light,
traffic_light_state=traffic_light_state,
)
def get_vehicle_kinetic(vehicle: carla.Vehicle):
"""
todo unfinished
Get kinetics of ego vehicle.
todo use a class to encapsulate all methods about getting kinetics
"""
kinetic_dict = {}
transform = vehicle.get_transform()
vehicle.get_acceleration()
vehicle.get_angular_velocity()
def get_vehicle_velocity_vector(vehicle: carla.Vehicle, map_vehicle: carla.Map,
velocity):
"""
Function to return a velocity vector which points to the direction of the next waypoint.
:param velocity: Desired vehicle velocity
:param map_vehicle: carla.Map
:param vehicle: carla.Vehicle object
:return: carla.Vector3D
"""
# Getting current waypoint and next from vehicle
current_wp = map_vehicle.get_waypoint(vehicle.get_location())
next_wp = current_wp.next(1)[0]
# Getting localization from the waypoints
current_loc = get_localization_from_waypoint(current_wp)
next_loc = get_localization_from_waypoint(next_wp)
velocity_x = abs(next_loc.x - current_loc.x)
velocity_y = abs(next_loc.y - current_loc.y)
vector_vel0 = np.array([velocity_x, velocity_y, 0])
vector_vel = (velocity / np.linalg.norm(vector_vel0)) * vector_vel0
return carla.Vector3D(round(vector_vel[0], 3), round(vector_vel[1], 3), 0)
def run_step(self, vehicle: carla.Vehicle, next_waypoint: carla.Waypoint, left_lane_x, right_lane_x) -> float:
self.sync(vehicle=vehicle)
target_y = next_waypoint.transform.location.y
target_x = next_waypoint.transform.location.x
angle_difference = math.atan2(
target_y - self.vehicle.get_location().y,
target_x - self.vehicle.get_location().x,
) - np.radians(self.vehicle.get_transform().rotation.yaw)
velocity = vehicle.get_velocity()
speed = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2)
curr_look_forward = (
self.look_ahead_gain * speed
+ self.look_ahead_distance
)
lateral_difference = math.atan2(
2.0
* WHEEL_BASE_LENGTH
* math.sin(angle_difference)
/ curr_look_forward,
1.0,
)
pure_pursuit_control = lateral_difference
lane_keeping_control = self.kp*(right_lane_x - left_lane_x)
lane_factor = 0.5
control = lane_factor * lane_keeping_control + (1-lane_factor) * pure_pursuit_control
print('l, r: ', left_lane_x, ', ', right_lane_x,
'pure, lane: ', pure_pursuit_control, ', ', lane_keeping_control, ' control: ', control)
return np.clip(control, -1, 1)
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 __init__(self, world: carla.World, carla_vehicle: carla.Vehicle):
self.has_collided = False
def on_collision(e):
self.has_collided = True
blueprint_library = world.get_blueprint_library()
blueprint = blueprint_library.find('sensor.other.collision')
self._collision_sensor = world.spawn_actor(
blueprint, carla_vehicle.get_transform(), attach_to=carla_vehicle)
self._collision_sensor.listen(on_collision)
def reset(self, ego_vehicle: carla.Vehicle):
# Actors
self._driving_actors_manager.clean_up_all()
self._driving_actors_manager.spawn_random_assets_at_markings(
markings=self._driving_actors_markings, coverage=0.1)
# Ego vehicle
start_node = random.choice(TOWN03_ROUNDABOUT_NODES)
start_node.spawn_point.location.z = 0.1
if self._collision_sensor:
self._collision_sensor.destroy()
self._collision_sensor = CollisionSensor(self._world, ego_vehicle)
# Physics trick is necessary to prevent vehicle from keeping the velocity
ego_vehicle.set_simulate_physics(False)
ego_vehicle.set_transform(start_node.spawn_point)
ego_vehicle.set_simulate_physics(True)
# Route
take_nth_exit = random.randrange(1, 5)
self._route = route.build_roundabout_checkpoint_route(
start_node=start_node, nth_exit_to_take=take_nth_exit)
# States
self._next_route_checkpoint_idx = 0
self._command = ChauffeurCommand.LANE_FOLLOW
self._steps_to_reach_next_checkpoint = MAX_NUM_STEPS_TO_REACH_CHECKPOINT
def closest_checkpoint(actor: carla.Vehicle, checkpoints: np.array):
actor_location = location_to_numpy(actor.get_location())
distances = np.linalg.norm(checkpoints - actor_location, axis=1)
azimuths = [
abs(calc_azimuth(actor_location[:2], point[:2]))
for point in checkpoints
]
cut = np.argmin(distances)
if np.argmin(azimuths) >= cut + 1:
return checkpoints[cut + 1]
else:
return checkpoints[cut]
def __init__(self,
ego_vehicle: carla.Vehicle,
*,
trajectory=None,
max_trajectory_distance_m=None,
timeout_s=None):
self._world = ego_vehicle.get_world()
self._world_map = self._world.get_map()
self._collision_sensor: CollisionSensor = CollisionSensor(
self._world, ego_vehicle)
self._trajectory: Optional[Trajectory] = trajectory
self._max_trajectory_distance_m = max_trajectory_distance_m
self._start_timestamp_s = self._get_timestamp_s(self._world)
self._timeout_s = timeout_s
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 run_step(self, vehicle: carla.Vehicle,
next_waypoint: carla.Waypoint) -> float:
self.sync(vehicle=vehicle)
target_y = next_waypoint.transform.location.y
target_x = next_waypoint.transform.location.x
angle_difference = math.atan2(
target_y - self.vehicle.get_location().y,
target_x - self.vehicle.get_location().x,
) - np.radians(self.vehicle.get_transform().rotation.yaw)
velocity = vehicle.get_velocity()
speed = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2)
curr_look_forward = (self.look_ahead_gain * speed +
self.look_ahead_distance)
lateral_difference = math.atan2(
2.0 * WHEEL_BASE_LENGTH * math.sin(angle_difference) /
curr_look_forward,
1.0,
)
return np.clip(lateral_difference, -1, 1)
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 = [
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 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 run_step(self, vehicle: carla.Vehicle) -> float:
self.sync(vehicle)
velocity = vehicle.get_velocity()
speed = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2)
return np.clip(self.kp * (self.target_speed - speed), 0, 1)
