def split_polygon_by_mask(X, mask):
# split X by inclusion or exclusion
indices = np.where(np.diff(mask, prepend=np.nan))[0]
l = np.split(X, indices)[1:]
X_inclusion = l[::2] if mask[0] else l[1::2]
X_exclusion = l[::2] if not mask[0] else l[1::2]
if len(X_inclusion) % 2 == 0:
# fold all
n = len(X_inclusion)
X_inclusion = util.map_to_list(
np.concatenate,
zip(X_inclusion[:n // 2], X_inclusion[:n // 2 - 1:-1]))
# take middle and fold rest
n = len(X_exclusion)
X_exclusion = util.map_to_list(np.concatenate, zip(X_exclusion[:n // 2], X_exclusion[:n // 2:-1])) \
+ [X_exclusion[n // 2]]
else:
# take middle and fold rest
n = len(X_inclusion)
X_inclusion = util.map_to_list(np.concatenate, zip(X_inclusion[:n // 2], X_inclusion[:n // 2:-1])) \
+ [X_inclusion[n // 2]]
# fold all
n = len(X_exclusion)
X_exclusion = util.map_to_list(
np.concatenate,
zip(X_exclusion[:n // 2], X_exclusion[:n // 2 - 1:-1]))
return X_inclusion, X_exclusion
def keep_blueprint(bp):
if int(bp.get_attribute('number_of_wheels')) != 4:
return False
name = bp.id
endswith = util.map_to_list(lambda skip_name: name.endswith(skip_name),
SKIP_VEHICLE_NAMES)
return not any(endswith)
def __init__(self):
"""Load map data from cache and collect shapes of all the intersections."""
self.map_datum = {}
# map to Shapely MultiPolygon for junction entrance/exits
self.map_to_smpolys = {}
# map to Shapely Circle covering junction region
self.map_to_scircles = {}
logger.info("Retrieve map from cache.")
for map_name in CARLA_MAP_NAMES:
cachepath = f"{CACHEDIR}/map_data.{map_name}.pkl"
with open(cachepath, "rb") as f:
payload = dill.load(f, encoding="latin1")
self.map_datum[map_name] = util.AttrDict(
road_polygons=payload["road_polygons"],
white_lines=payload["white_lines"],
yellow_lines=payload["yellow_lines"],
junctions=payload["junctions"],
)
logger.info("Retrieving some data from map.")
for map_name in CARLA_MAP_NAMES:
for _junction in self.map_datum[map_name].junctions:
f = lambda x, y, yaw, l: util.vertices_from_bbox(
np.array([x, y]), yaw, np.array([5.0, 0.95 * l]))
vertex_set = util.map_to_ndarray(
lambda wps: util.starmap(f, wps), _junction.waypoints)
smpolys = util.map_to_list(vertex_set_to_smpoly, vertex_set)
util.setget_list_from_dict(self.map_to_smpolys,
map_name).extend(smpolys)
x, y = _junction.pos
scircle = shapely.geometry.Point(x, y).buffer(
self.TLIGHT_DETECT_RADIUS)
util.setget_list_from_dict(self.map_to_scircles,
map_name).append(scircle)
def __lidar_snapshot_to_populate_vehicle_visibility(
self, lidar_measurement, points, labels, object_ids):
"""Called by capture_lidar() to obtain vehicle visibility from
segmented LIDAR points.
"""
frame = lidar_measurement.frame
if (frame -
self.__first_frame) % self.__scene_config.record_interval == 0:
frame_id = int((frame - self.__first_frame) /
self.__scene_config.record_interval)
vehicle_label_mask = labels == SegmentationLabel.Vehicle.value
object_ids = np.unique(object_ids[vehicle_label_mask])
self.__vehicle_visibility[frame_id] = set(
util.map_to_list(str, object_ids))
self.__vehicle_visibility[frame_id].add("ego")
def get_straight_lanes(start_wp, tol=2.0):
"""Get the points corresponding to the straight parts of all
lanes in road starting from start_wp going in the same direction.
Parameters
==========
start_wp : carla.Waypoint
The starting point of the road lane.
tol : float (optional)
The tolerance to select straight part of the road in meters.
Returns
=======
list of np.array
Each item in the list are points for one lane of of shape (N_i, 2).
Each (x, y) point is in meters in world coordinates.
"""
_, start_yaw, _ = carlautil.to_rotation_ndarray(start_wp)
wps = get_adjacent_waypoints(start_wp)
f = lambda wp: get_straight_line(wp, start_yaw, tol=tol)
return np.concatenate(util.map_to_list(f, wps))
def __init__(self,
frame,
phi,
world,
other_vehicles,
player_transforms,
others_transforms,
player_bbox,
other_id_ordering=None,
radius=200):
"""
1. generates a history of player and other vehicle position coordinates
of size len(player_transforms)
Parameters
----------
frame : int
Frame ID of observation
world : carla.World
other_vehicles : list of carla.Vehicle
player_transform : collections.deque of carla.Transform
Collection of transforms of player
Ordered by timestep where last in deque is current timestep
others_transforms : collections.deque of (dict of int : carla.Trajectory)
Collection of transforms of other vehicles by vehicle ID
Ordered by timestep where last in deque is current timestep
A : int
Number of vehicles in observation, including ego vehicle. Must be A > 1.
other_id_ordering : list of int
IDs of other (not player) vehicles (not walkers).
All IDs in other_id_ordering belong to some vehicle ID
radius : int
"""
_, _, self.T_past, _ = tensoru.shape(self.phi.S_past_world_frame)
self.B, self.A, self.T, self.D = tensoru.shape(
self.phi.S_future_world_frame)
self.B, self.H, self.W, self.C = tensoru.shape(
self.phi.overhead_features)
assert (len(player_transforms) == len(others_transforms))
assert (self.A > 0)
# player_transform : carla.Transform
# transform of current player
self.player_transform = player_transforms[-1]
# player_positions_world : ndarray of shape (len(player_transforms), 3)
self.player_positions_world = carlautil.transforms_to_location_ndarray(
self.player_transforms)
# player_positions_local : ndarray of shape (len(player_transforms), 3)
self.player_positions_local = self.player_positions_world \
- carlautil.transform_to_location_ndarray(self.player_transform)
# player_yaw : float
self.player_yaw = carlautil.transform_to_yaw(self.player_transform)
if self.other_id_ordering is None:
# get list of A agents within radius close to us
# note that other_id_ordering may have size smaller than A-1
ids = self.__get_nearby_agent_ids()
self.other_id_ordering = ids[:self.A - 1]
if self.other_id_ordering.shape != (0, ):
others_positions_list = [None] * len(self.others_transforms)
for idx, others_transform in enumerate(self.others_transforms):
others_positions_list[idx] = util.map_to_list(
lambda aid: carlautil.transform_to_location_ndarray(
others_transform[aid]), self.other_id_ordering)
# agent_positions_world : ndarray of shape (A-1, len(self.others_transforms), 3)
self.agent_positions_world = np.array(
others_positions_list).transpose(1, 0, 2)
others_transform = self.others_transforms[-1]
# agent_yaws : ndarray of shape (A-1,)
self.agent_yaws = util.map_to_ndarray(
lambda aid: carlautil.transform_to_yaw(others_transform[aid]),
self.other_id_ordering)
self.n_missing = max(
self.A - 1 - self.agent_positions_world.shape[0], 0)
self.has_other_agents = True
else:
self.agent_positions_world = np.array([])
self.agent_yaws = np.array([])
self.n_missing = self.A - 1
self.has_other_agents = False
if self.n_missing > 0:
faraway_position = carlautil.transform_to_location_ndarray(self.player_transform) \
+ np.array([0, 300, 0])
faraway_tile = np.tile(
faraway_position,
(self.n_missing, len(self.others_transforms), 1))
if self.n_missing == self.A - 1:
self.agent_positions_world = faraway_tile
self.agent_yaws = np.zeros(self.A - 1)
else:
self.agent_positions_world = np.concatenate(
(self.agent_positions_world, faraway_tile), axis=0)
self.agent_yaws = np.concatenate(
(self.agent_yaws, np.zeros(self.n_missing)), axis=0)
# agent_positions_local : ndarray of shape (A-1, len(self.others_transforms), 3)
self.agent_positions_local = self.agent_positions_world \
- carlautil.transform_to_location_ndarray(self.player_transform)
def get_road_segment_enclosure(start_wp, tol=2.0):
"""Get rectangle that tightly inner approximates of the road segment
containing the starting waypoint.
Parameters
==========
start_wp : carla.Waypoint
A starting waypoint of the road.
tol : float (optional)
The tolerance to select straight part of the road in meters.
Returns
=======
np.array
The position and the heading angle of the starting waypoint
of the road of form [x, y, angle] in (meters, meters, radians).
np.array
The 2D bounding box enclosure in world coordinates of shape (4, 2)
enclosing the road segment.
np.array
The parameters of the enclosure of form
(b_length, f_length, r_width, l_width)
If the enclosure is in the reference frame such that the starting
waypoint points along +x-axis, then the enclosure has these length
and widths:
____________________________________
| l_width |
| | |
| | |
| b_length -- (x, y)-> -- f_length |
| | |
| | |
| r_width |
------------------------------------
"""
_LENGTH = -2
_, start_yaw, _ = carlautil.to_rotation_ndarray(start_wp)
adj_wps = get_adjacent_waypoints(start_wp)
# mtx : np.array
# Rotation matrix from world coordinates, both frames in UE orientation
mtx = util.rotation_2d(-start_yaw)
# rev_mtx : np.array
# Rotation matrix to world coordinates, both frames in UE orientation
rev_mtx = util.rotation_2d(start_yaw)
s_x, s_y, _ = carlautil.to_location_ndarray(start_wp)
# Get points of lanes
f = lambda wp: get_straight_line(wp, start_yaw, tol=tol)
pc = util.map_to_list(f, adj_wps)
# Get length of bb for lanes
def g(points):
points = points - np.array([s_x, s_y])
points = (rev_mtx @ points.T)[0]
return np.abs(np.max(points) - np.min(points))
lane_lengths = util.map_to_ndarray(g, pc)
length = np.min(lane_lengths)
# Get width of bb for lanes
lwp, rwp = adj_wps[0], adj_wps[-1]
l_x, l_y, _ = carlautil.to_location_ndarray(lwp)
r_x, r_y, _ = carlautil.to_location_ndarray(rwp)
points = np.array([[l_x, l_y], [s_x, s_y], [r_x, r_y]])
points = points @ rev_mtx.T
l_width = np.abs(points[0, 1] - points[1, 1]) + lwp.lane_width / 2.
r_width = np.abs(points[1, 1] - points[2, 1]) + rwp.lane_width / 2.
# construct bounding box of road segment
x, y, _ = carlautil.to_location_ndarray(start_wp)
vec = np.array([[0, 0], [_LENGTH, 0]]) @ mtx.T
dx0, dy0 = vec[1, 0], vec[1, 1]
vec = np.array([[0, 0], [length, 0]]) @ mtx.T
dx1, dy1 = vec[1, 0], vec[1, 1]
vec = np.array([[0, 0], [0, -l_width]]) @ mtx.T
dx2, dy2 = vec[1, 0], vec[1, 1]
vec = np.array([[0, 0], [0, r_width]]) @ mtx.T
dx3, dy3 = vec[1, 0], vec[1, 1]
bbox = np.array([[x + dx0 + dx3, y + dy0 + dy3],
[x + dx1 + dx3, y + dy1 + dy3],
[x + dx1 + dx2, y + dy1 + dy2],
[x + dx0 + dx2, y + dy0 + dy2]])
start_wp_spec = np.array([s_x, s_y, start_yaw])
bbox_spec = np.array([_LENGTH, length, r_width, l_width])
return start_wp_spec, bbox, bbox_spec
def __capture_agents_within_radius(self, frame_id):
labels = self.__map_reader.get_actor_labels(self.__ego_vehicle)
if self.__should_exclude_dataset_sample(labels):
self.__remove_scene_builder()
player_location = carlautil.to_location_ndarray(self.__ego_vehicle)
player_z = player_location[2]
if len(self.__other_vehicles):
other_ids = list(self.__other_vehicles.keys())
other_vehicles = self.__other_vehicles.values()
others_data = carlautil.actors_to_Lxyz_Vxyz_Axyz_Rpyr_ndarray(
other_vehicles).T
other_locations = others_data[:3].T
distances = np.linalg.norm(other_locations - player_location,
axis=1)
df = pd.DataFrame({
"frame_id":
np.full((len(other_ids), ), frame_id),
"type":
[self.__scene_config.node_type.VEHICLE] * len(other_ids),
"node_id":
util.map_to_list(str, other_ids),
"robot": [False] * len(other_ids),
"distances":
distances,
"x":
others_data[0],
"y":
others_data[1],
"z":
others_data[2],
"v_x":
others_data[3],
"v_y":
others_data[4],
"v_z":
others_data[5],
"a_x":
others_data[6],
"a_y":
others_data[7],
"a_z":
others_data[8],
"length":
others_data[9],
"width":
others_data[10],
"height":
others_data[11],
"heading":
others_data[13],
})
df = df[df["distances"] < self.__radius]
df = df[df["z"].between(
player_z - self.Z_LOWERBOUND,
player_z + self.Z_UPPERBOUND,
inclusive="neither",
)]
del df["distances"]
else:
df = pd.DataFrame(columns=[
"frame_id",
"type",
"node_id",
"robot",
"x",
"y",
"z",
"length",
"width",
"height",
"heading",
])
ego_data = carlautil.actor_to_Lxyz_Vxyz_Axyz_Rpyr_ndarray(
self.__ego_vehicle)
data_point = pd.DataFrame.from_records([{
"frame_id":
frame_id,
"type":
self.__scene_config.node_type.VEHICLE,
"node_id":
"ego",
"robot":
True,
"x":
ego_data[0],
"y":
ego_data[1],
"z":
ego_data[2],
"v_x":
ego_data[3],
"v_y":
ego_data[4],
"v_z":
ego_data[5],
"a_x":
ego_data[6],
"a_y":
ego_data[7],
"a_z":
ego_data[8],
"length":
ego_data[9],
"width":
ego_data[10],
"height":
ego_data[11],
"heading":
ego_data[13],
}])
df = pd.concat((df, data_point), ignore_index=True)
# df = df.append(data_point, ignore_index=True)
if self.__trajectory_data is None:
self.__trajectory_data = df
else:
self.__trajectory_data = pd.concat((self.__trajectory_data, df),
ignore_index=True)
def actors_to_location_ndarray(alist):
"""Converts iterable of carla.Actor to a ndarray of size (len(alist), 3)"""
return np.array(util.map_to_list(actor_to_location_ndarray, alist))
def transforms_to_location_ndarray(ts):
"""Converts an iterable of carla.Transform to a ndarray of size (len(iterable), 3)"""
return np.array(util.map_to_list(transform_to_location_ndarray, ts))
def vehicles_to_xyz_lwh_pyr_ndarray(alist):
"""Converts iterable of carla.Actor transformation
to an ndarray of size (len(alist), 9) where each row is
[x, y, z, length, width, height, pitch, yaw, roll]"""
return np.array(util.map_to_list(vehicle_to_xyz_lwh_pyr_ndarray, alist))
def make_local_params(self, frame, ovehicles):
"""Get the local optimization parameters used for current MPC step."""
"""Get parameters to construct control and state variables."""
params = util.AttrDict()
params.frame = frame
p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
_, psi_0, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle,
flip_y=True)
v_0_mag = self.get_current_velocity()
x_init = np.array([p_0_x, p_0_y, psi_0, v_0_mag])
initial_state = util.AttrDict(world=x_init,
local=np.array([0, 0, 0, v_0_mag]))
params.initial_state = initial_state
# try:
# u_init = self.__U_warmstarting[self.__step_horizon]
# except TypeError:
# u_init = np.array([0., 0.])
# Using previous control doesn't work
u_init = np.array([0.0, 0.0])
x_bar, u_bar, Gamma, nx, nu = self.__vehicle_model.get_optimization_ltv(
x_init, u_init)
params.x_bar, params.u_bar, params.Gamma = x_bar, u_bar, Gamma
params.nx, params.nu = nx, nu
"""Get parameters for other vehicles."""
# O - number of obstacles
params.O = len(ovehicles)
# K - for each o=1,...,O K[o] is the number of outer approximations for vehicle o
params.K = np.zeros(params.O, dtype=int)
for idx, vehicle in enumerate(ovehicles):
params.K[idx] = vehicle.n_states
"""Get parameters for (random) multiple coinciding control."""
# N_traj : int
# Number of planned trajectories possible to compute
params.N_traj = np.prod(params.K)
if self.__random_mcc:
"""How does random multiple coinciding control work?
each item in the product set S_1 X S_2 X S_3 X S_4
represents the particular choices each vehicles make
get the subset of the product set S_1 X S_2 X S_3 X S_4
such that for each i = 1..4, for each j in S_i there is
a tuple in the subset with j in the i-th place.
"""
vehicle_n_states = [ovehicle.n_states for ovehicle in ovehicles]
n_states_max = max(vehicle_n_states)
vehicle_state_ids = [
util.range_to_list(n_states) for n_states in vehicle_n_states
]
def preprocess_state_ids(state_ids):
state_ids = state_ids + random.choices(
state_ids, k=n_states_max - len(state_ids))
random.shuffle(state_ids)
return state_ids
vehicle_state_ids = util.map_to_list(preprocess_state_ids,
vehicle_state_ids)
# sublist_joint_decisions : list of (list of int)
# Subset of S_1 X S_2 X S_3 X S_4 of joint decisions
params.sublist_joint_decisions = np.array(
vehicle_state_ids).T.tolist()
# N_select : int
# Number of planned trajectories to compute
params.N_select = len(params.sublist_joint_decisions)
else:
# sublist_joint_decisions : list of (list of int)
# Entire set of S_1 X S_2 X S_3 X S_4 of joint decision outcomes
params.sublist_joint_decisions = util.product_list_of_list([
util.range_to_list(ovehicle.n_states) for ovehicle in ovehicles
])
# N_select : int
# Number of planned trajectories to compute, equal to N_traj.
params.N_select = len(params.sublist_joint_decisions)
return params
(["is_at_intersection",
"is_significant_car"], ["significant_at_intersection"]),
(["is_at_intersection", "is_stopped_car"], ["stopped_at_intersection"]),
(["is_at_intersection"], ["other_at_intersection"]),
### counts outside of intersection (other)
(["is_major_turn", "is_significant_car"], ["turn_at_other"]),
(["is_minor_turn", "is_significant_car"], ["turn_at_other"]),
(["is_significant_car"], ["significant_at_other"]),
(["is_stopped_car"], ["stopped_at_other"]),
([], ["other_at_other"])
]
# List of scene attributes
SCENEATTRS = util.deduplicate(
util.merge_list_of_list(
util.map_to_list(util.second, NODEATTR_SCENEATTR_MAP)))
class TrajectronSceneData(object):
"""Trajectron++ scene data. Used to inspect data, count samples in the dataset,
and reweight samples when generating a dataset.
Attributes
==========
n_nodes : int
Number of nodes.
nodeid_scene_dict : dict of (str, Scene)
scene+node ID => scene the node is in
sceneid_scene_dict : dict of (str, Scene)
scene ID => scene
sceneid_count_dict : dict of (str, util.AttrDict)
def main(config):
pp.pprint(vars(config))
group_creator = SampleGroupCreator(config)
split_creator = CrossValidationSplitCreator(config)
logger.info("Loading the data.")
envs = []
for file_path in config.data_files:
try:
loadpath = os.path.join(config.data_dir, file_path)
with open(loadpath, "rb") as f:
env = dill.load(f, encoding="latin1")
envs.append(env)
except:
logger.warning(f"Failed to load {file_path} from data directory")
logger.warning(config.data_dir)
env = None
if len(envs) == 0:
logger.warning("No data. Doing nothing.")
return
elif len(envs) > 0:
logger.info(f"Loaded {len(envs)} data payloads.")
env = envs[0]
for _env in envs[1:]:
env.scenes.extend(_env.scenes)
logger.info(f"Collected {len(env.scenes)} samples.")
sample_dict = {scene.name: scene for scene in env.scenes}
sample_ids = list(sample_dict.keys())
if config.weighted_resampling:
logger.info("Inspecting scene data to use for weighted resampling.")
scene_data = TrajectronSceneData(env.scenes)
scene_data.log_node_count(filename="raw_scene_histogram")
else:
scene_data = None
logger.info("Creating group indices of samples.")
groups = group_creator.make_groups(sample_ids)
logger.info("Creating split indices of samples.")
splits = split_creator.make_splits(groups)
augmentations_dict = dict()
for idx, split in enumerate(splits[: config.n_splits]):
split_idx = idx + 1
logger.info(f"Making (train, val, test) split {split_idx}.")
train_env = make_environment(env.name)
val_env = make_environment(env.name)
test_env = make_environment(env.name)
if config.weighted_resampling:
train_split = scene_data.sample_scenes(config.n_train, scene_ids=split[0])
else:
train_split = split[0][slice(config.n_train)]
# create augmented scenes as needed
unaugmented_ids = set(train_split) - set(augmentations_dict.keys())
logger.info(f"Augmenting {len(unaugmented_ids)} scenes for split {split_idx}.")
for sample_id in tqdm(unaugmented_ids):
scene = sample_dict[sample_id]
augmentations_dict[sample_id] = list()
angles = np.arange(0, 360, 15)
for angle in angles:
augmentations_dict[sample_id].append(augment_scene(scene, angle))
# add all existing augmentations to all scenes right before saving train set.
for sample_id, augments in augmentations_dict.items():
sample_dict[sample_id].augmented = augments
train_env.scenes = util.map_to_list(sample_dict.get, train_split)
n_train_samples = len(train_env.scenes)
logger.info(f"Train set has {n_train_samples} scenes.")
logger.info("Saving train set.")
savepath = os.path.join(
config.split_dir,
"{label}_split{split_idx}_train.pkl".format(
split_idx=split_idx, label=config.label
),
)
with open(savepath, "wb") as f:
dill.dump(train_env, f, protocol=dill.HIGHEST_PROTOCOL)
# remove all existing augmentations from all scenes right after saving train set.
for sample_id in augmentations_dict.keys():
del sample_dict[sample_id].augmented
if config.weighted_resampling:
val_split = scene_data.sample_scenes(config.n_val, scene_ids=split[1])
test_split = scene_data.sample_scenes(config.n_test, scene_ids=split[2])
else:
val_split = split[1][slice(config.n_val)]
test_split = split[2][slice(config.n_test)]
val_env.scenes = util.map_to_list(sample_dict.get, val_split)
n_val_samples = len(val_env.scenes)
test_env.scenes = util.map_to_list(sample_dict.get, test_split)
n_test_samples = len(test_env.scenes)
# naive augmenting for each split
# for scene in tqdm(train_env.scenes):
# scene.augmented = list()
# angles = np.arange(0, 360, 15)
# for angle in angles:
# scene.augmented.append(augment_scene(scene, angle))
logger.info(f"Val set has {n_val_samples} scenes.")
logger.info("Saving val set.")
savepath = os.path.join(
config.split_dir,
"{label}_split{split_idx}_val.pkl".format(
split_idx=split_idx, label=config.label
),
)
with open(savepath, "wb") as f:
dill.dump(val_env, f, protocol=dill.HIGHEST_PROTOCOL)
logger.info(f"Test set has {n_test_samples} scenes.")
logger.info("Saving test set.")
savepath = os.path.join(
config.split_dir,
"{label}_split{split_idx}_test.pkl".format(
split_idx=split_idx, label=config.label
),
)
with open(savepath, "wb") as f:
dill.dump(test_env, f, protocol=dill.HIGHEST_PROTOCOL)
logger.info("Done.")