def step(self, vehicles):
commands = []
ngsim_v: NGSimCar
for ngsim_v in vehicles:
if ngsim_v.id in self._ignored_ngsim_vehicle_ids:
continue
if ngsim_v.id in self._vehicle_by_vehicle_id:
carla_v = self._vehicle_by_vehicle_id[ngsim_v.id]
v_data = VEHICLE_BY_TYPE_ID[carla_v.type_id]
t = self._mapper.ngsim_to_carla(
ngsim_v.get_transform(),
carla_v.get_transform().location.z,
v_data.rear_axle_offset)
commands.append(
carla.command.ApplyTransform(carla_v,
t.as_carla_transform()))
else:
model = find_best_matching_model(ngsim_v)
if model is None:
LOGGER.debug(
f"Not found matching vehicle model for vehicle {ngsim_v}"
)
continue
target_transform = self._mapper.ngsim_to_carla(
ngsim_v.get_transform(), model.z_offset,
model.rear_axle_offset)
spawn_transform = Transform(
target_transform.position.with_z(500),
target_transform.orientation)
vehicle_blueprint = self._get_vehicle_blueprint(model.type_id)
vehicle_blueprint.set_attribute('role_name', 'ngsim_replay')
carla_v = self._world.try_spawn_actor(
vehicle_blueprint, spawn_transform.as_carla_transform())
if carla_v is None:
LOGGER.info(
f"Error spawning vehicle with id {ngsim_v.id}. Ignoring it now in the future. Model: {model.type_id}."
)
# Without ignoring such vehicles till the end of episode a vehicle might suddenly appears mid-road
# in future frames
self._ignored_ngsim_vehicle_ids.add(ngsim_v.id)
continue
commands.append(
carla.command.ApplyTransform(
carla_v, target_transform.as_carla_transform()))
self._vehicle_by_vehicle_id[ngsim_v.id] = carla_v
now_vehicle_ids = {v.id for v in vehicles}
previous_vehicles_ids = set(self._vehicle_by_vehicle_id.keys())
for to_remove_id in previous_vehicles_ids - now_vehicle_ids:
self._vehicle_by_vehicle_id[to_remove_id].destroy()
del self._vehicle_by_vehicle_id[to_remove_id]
# TODO batch spawn and batch destroy
self._client.apply_batch_sync(commands, False)
def ngsim_to_carla(self, ngsim_transform: Transform, z: float, rear_axle_offset: float) -> Transform:
p = ngsim_transform.position.as_vector2()
p = self._transformation_matrix @ np.array([p.x, p.y, 1])
p = Vector2.from_numpy(p)
direction = Vector2.from_yaw_radian(ngsim_transform.orientation.yaw_radians - self._rotation)
p -= direction * rear_axle_offset # in ngsim origin point is in the center of rear axle
return Transform(p.to_vector3(z), direction)
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 __init__(self,
*,
client: carla.Client,
cmd_for_changing_lane=ChauffeurCommand.CHANGE_LANE_LEFT,
speed_range_token: str,
no_columns=True):
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._done_counter: int = TARGET_LANE_ALIGNMENT_FRAMES
# 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 = (5, 30)
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 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 _map_trajectory_to_carla(self, trajectory_utm) -> List[Transform]:
trajectory_carla = []
for transform_utm in trajectory_utm:
transform_carla = self._transformer.utm2carla(transform_utm)
transform_carla = \
Transform(transform_carla.position.with_z(self._model.z_offset), transform_carla.orientation)
trajectory_carla.append(transform_carla)
return trajectory_carla
def _get_commands(self, transform: Transform):
velocity = (transform.orientation *
self._speed_mps).to_vector3(0).as_carla_vector3d()
cmds = [
carla.command.ApplyVelocity(self._actor_id, velocity),
carla.command.ApplyTransform(
self._actor_id, transform=transform.as_carla_transform()),
]
return cmds
def step(self, vehicles):
commands = []
real_vehicle: RealTrafficVehicle
for real_vehicle in vehicles:
if real_vehicle.id in self._ignored_real_traffic_vehicle_ids:
continue
target_transform = real_vehicle.transform # transform in carla coordinates
if real_vehicle.id in self._vehicle_by_vehicle_id:
carla_vehicle = self._vehicle_by_vehicle_id[real_vehicle.id]
target_transform = Transform(
target_transform.position.with_z(
carla_vehicle.get_transform().location.z),
target_transform.orientation)
commands.append(
carla.command.ApplyTransform(
carla_vehicle, target_transform.as_carla_transform()))
else:
spawn_transform = Transform(
target_transform.position.with_z(500),
target_transform.orientation)
vehicle_blueprint = self._get_vehicle_blueprint(
real_vehicle.type_id)
vehicle_blueprint.set_attribute('role_name',
'real_traffic_replay')
carla_vehicle = self._world.try_spawn_actor(
vehicle_blueprint, spawn_transform.as_carla_transform())
if carla_vehicle is None:
LOGGER.info(
f"Error spawning vehicle with id {real_vehicle.id}. "
f"Ignoring it now in the future. Model: {real_vehicle.type_id}."
)
# Without ignoring such vehicles till the end of episode a vehicle might suddenly appears mid-road
# in future frames
self._ignored_real_traffic_vehicle_ids.add(real_vehicle.id)
continue
commands.append(
carla.command.ApplyTransform(
carla_vehicle, target_transform.as_carla_transform()))
self._vehicle_by_vehicle_id[real_vehicle.id] = carla_vehicle
if real_vehicle.debug:
self._world.debug.draw_string(
(target_transform.position +
Vector3(2, 0, 4)).as_carla_location(),
str(real_vehicle.debug))
now_vehicle_ids = {v.id for v in vehicles}
previous_vehicles_ids = set(self._vehicle_by_vehicle_id.keys())
for to_remove_id in previous_vehicles_ids - now_vehicle_ids:
actor = self._vehicle_by_vehicle_id[to_remove_id]
commands.append(carla.command.DestroyActor(actor.id))
del self._vehicle_by_vehicle_id[to_remove_id]
# TODO batch spawn and batch destroy
self._client.apply_batch_sync(commands, False)
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 _transform_with_convert(
self, transform: Transform,
transformer: skimage.transform.AffineTransform):
position = transform.position.as_numpy()[:2]
position = position.reshape(-1, 2)
orientation = transform.orientation.as_numpy()
orientation = orientation.reshape(-1, 2)
position, orientation = self.transform(position, orientation,
transformer)
position = Vector2.from_numpy(position.squeeze()).to_vector3(0)
orientation = Vector2.from_numpy(orientation.squeeze())
return Transform(position, orientation)
def _resample_route(self, route: List[carla.Transform],
step_m: float) -> List[carla.Transform]:
assert len(route) > 1
try:
positions = [
Transform.from_carla_transform(transform).position
for transform in route
]
positions = unique_justseen(positions)
positions = resample_points(positions, step_m=step_m)
route = positions_to_transforms(positions)
return route
except Exception:
LOGGER.error(f'#route={len(route)} route={route}')
raise
def get_transform(self) -> Transform:
return Transform(
Vector2.from_numpy(self.front).to_vector3(0),
Vector2.from_numpy(self._direction),
)
def extract_utm_trajectory_from_df(df) -> List[Transform]:
trajectory = df[['UTM_X', 'UTM_Y', 'UTM_ANGLE']].values
return [
Transform(Vector3(x, y, 0), Vector2(np.cos(angle), np.sin(angle)))
for x, y, angle in trajectory
]