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 _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 __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 __init__(self, ngsim_origin: np.ndarray, carla_origin: np.ndarray):
assert ngsim_origin.shape == (2,), "Expected 2d point"
assert carla_origin.shape == (2,), "Expected 2d point"
ngsim_base_vectors = np.array([[1, 0], [0, 1]])
scale = PIXELS_TO_METERS
carla_base_vectors = np.array([[1, 0], [0, 1]]) * scale
self._transformation_matrix = cv2.getAffineTransform(
src=np.float32([ngsim_origin, *(ngsim_origin + ngsim_base_vectors)]),
dst=np.float32([carla_origin, *(carla_origin + carla_base_vectors)]),
)
NO_TRANSLATION = 0
self._rotation = Vector2.from_numpy(
self._transformation_matrix @ np.array([1, 0, NO_TRANSLATION])
).yaw_radians
import numpy as np
import pandas as pd
from PIL import Image
import skimage.transform
from carla_real_traffic_scenarios.utils.transforms import Vector2
_BLACKLISTED_SESSION_NAMES_DUE_TO_WRONG_GEOREFERENCE = [
'rdb6_2DJI_0006', 'rdb6_2DJI_0007', 'rdb6_2DJI_0008', 'rdb6_2DJI_0009', 'rdb6_DJI_0016', 'rdb3_M1DJI_0021'
]
BLACKLISTED_SESSION_NAMES = _BLACKLISTED_SESSION_NAMES_DUE_TO_WRONG_GEOREFERENCE
_ROUNDABOUTS_TOPOLOGIES = {
'rdb1': {
'roundabout_center_utm': Vector2(619304.556058351, 5809147.733704892), # (52.419519, 10.754369)
'map_center_utm': Vector2(619301.201, 5809149.760), # (1920, 1080),
'roads_utm': [ # (entry_utm, exit_utm)
(Vector2(619323.465, 5809153.332), Vector2(619324.167, 5809145.383)), # ((2622, 894), (2670, 1146))
(Vector2(619292.150, 5809163.147), Vector2(619299.362, 5809166.923)), # ((1587, 681), (1806, 537))
(Vector2(619296.386, 5809130.642), Vector2(619289.733, 5809135.251)), # ((1827, 1707), (1599, 1581))
(Vector2(619312.708, 5809131.905), None) # ((2346, 1614), None)
],
'correction_transform': skimage.transform.AffineTransform(np.array(
[[0.99996232, -0.02487794, -0.27454128],
[0.02757512, 1.00178269, 0.80360617],
[0., 0., 1.]]
))
},
'rdb2': {
'roundabout_center_utm': Vector2(618082.795, 5805733.190), # (1896, 1023),
def get_velocity(self) -> Vector2:
direction = Vector2.from_numpy(self._direction)
return direction * self._speed
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
]
