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 __plot_ev_current_bbox(self, ax):
"""Box of current ego vehicle position."""
vertices = util.vertices_from_bbox(self.params.initial_state.world[:2],
self.params.initial_state.world[2],
self.ego_bbox)
bb = patches.Polygon(vertices.reshape((
-1,
2,
)),
closed=True,
color='k',
fc=util.plu.modify_alpha("black", 0.2),
ls='-')
ax.add_patch(bb)
def __plot_ev_predicted_bbox(self, ax, t):
"""Get vertices of EV and plot its bounding box."""
if t < 2:
vertices = util.vertices_from_bbox(self.ctrl_result.X_star[t, :2],
self.ctrl_result.X_star[t, 2],
self.ego_bbox)
else:
"""Make EV bbox look better on far horizons by fixing the heading angle."""
heading = np.arctan2(
self.ctrl_result.X_star[t, 1] -
self.ctrl_result.X_star[t - 1, 1],
self.ctrl_result.X_star[t, 0] -
self.ctrl_result.X_star[t - 1, 0])
vertices = util.vertices_from_bbox(self.ctrl_result.X_star[t, :2],
heading, self.ego_bbox)
bb = patches.Polygon(vertices.reshape((
-1,
2,
)),
closed=True,
color='k',
fc='none',
ls='-')
ax.add_patch(bb)
def __plot_ov_clusters(self, ax, t):
"""Plot other vehicles"""
ovehicle_colors = get_ovehicle_color_set()
for ov_idx, ovehicle in enumerate(self.ovehicles):
color = ovehicle_colors[ov_idx][0]
ax.plot(ovehicle.past[:, 0],
ovehicle.past[:, 1],
marker='o',
markersize=2,
color=color)
heading = np.arctan2(ovehicle.past[-1, 1] - ovehicle.past[-2, 1],
ovehicle.past[-1, 0] - ovehicle.past[-2, 0])
vertices = util.vertices_from_bbox(ovehicle.past[-1], heading,
np.array([ovehicle.bbox]))
bb = patches.Polygon(vertices.reshape((
-1,
2,
)),
closed=True,
color=color,
fc=util.plu.modify_alpha(color, 0.2),
ls='-')
ax.add_patch(bb)
for latent_idx in range(ovehicle.n_states):
color = ovehicle_colors[ov_idx][latent_idx]
# Plot overapproximation
A = self.ctrl_result.A_union[t][latent_idx][ov_idx]
b = self.ctrl_result.b_union[t][latent_idx][ov_idx]
util.npu.plot_h_polyhedron(ax, A, b, ec=color, ls='-', alpha=1)
# Plot vertices
vertices = self.ctrl_result.vertices[t][latent_idx][ov_idx]
X = vertices[:, 0:2].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[:, 2:4].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[:, 4:6].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[:, 6:8].T
ax.scatter(X[0], X[1], color=color, s=2)
def plot_multiple_coinciding_controls_timestep(ax,
pred_result,
ovehicles,
params,
ctrl_result,
X_star,
t,
traj_idx,
latent_indices,
ego_bbox,
extent=None):
"""Helper function for plot_multiple_coinciding_controls()
Parameters
==========
ax : matplotlib.axes.Axes
The plot to make.
pred_result : util.AttrDict
Prediction payload containing: scene, timestep, nodes,
predictions, z, latent_probs, past_dict, ground_truth_dict.
ovehicles : list of OVehicle
Vehicles present in the scene.
params : util.AttrDict
Parameters of optimization.
ctrl_result : util.AttrDict
Control optimization payload containing: cost, U_star,
X_star, goal, A_unions, b_unions, vertices.
X_star : ndarray
The predicted states including initial state from optimization.
t : int
The timestep of predicted state we want to show for this plot.
The state corresponding to timestep 1 is X_star[t + 1].
traj_idx : int
latent_indices : ndarray of int
ego_bbox : list of int
Ego bounding box (longitudinal, lateral) dimensions.
extent : tuple of number
Extent of plot (x min, x max, y min, y max).
"""
ovehicle_colors = get_ovehicle_color_set()
render_scene(ax, pred_result.scene, global_coordinates=True)
ax.plot(ctrl_result.goal[0],
ctrl_result.goal[1],
marker='*',
markersize=8,
color="yellow")
# Plot ego vehicle past trajectory
past = None
minpos = np.array([pred_result.scene.x_min, pred_result.scene.y_min])
for node in pred_result.nodes:
if node.id == 'ego':
past = pred_result.past_dict[pred_result.timestep][node] + minpos
break
ax.plot(past[:, 0], past[:, 1], '-ko', markersize=2)
# Box of current ego vehicle position
vertices = util.vertices_from_bbox(params.initial_state.world[:2],
params.initial_state.world[2], ego_bbox)
bb = patches.Polygon(vertices.reshape((
-1,
2,
)),
closed=True,
color='k',
fc=util.plu.modify_alpha("black", 0.2),
ls='-')
ax.add_patch(bb)
# Plot ego vehicle planned trajectory
ax.plot(X_star[:(t + 2), 0], X_star[:(t + 2), 1], 'k-o', markersize=2)
# Get vertices of EV and plot its bounding box
if t < 2:
vertices = util.vertices_from_bbox(X_star[t + 1, :2], X_star[t + 1, 2],
ego_bbox)
else:
# Make EV bbox look better on far horizons by fixing the heading angle
heading = np.arctan2(X_star[t + 1, 1] - X_star[t, 1],
X_star[t + 1, 0] - X_star[t, 0])
vertices = util.vertices_from_bbox(X_star[t + 1, :2], heading,
ego_bbox)
bb = patches.Polygon(vertices.reshape((
-1,
2,
)),
closed=True,
color='k',
fc='none',
ls='-')
ax.add_patch(bb)
# Plot other vehicles
for ov_idx, ovehicle in enumerate(ovehicles):
# Plot past trajectory
latent_idx = latent_indices[ov_idx]
color = ovehicle_colors[ov_idx][0]
ax.plot(ovehicle.past[:, 0],
ovehicle.past[:, 1],
marker='o',
markersize=2,
color=color)
# Plot overapproximation
color = ovehicle_colors[ov_idx][latent_idx]
A = ctrl_result.A_unions[traj_idx][t][ov_idx]
b = ctrl_result.b_unions[traj_idx][t][ov_idx]
try:
util.npu.plot_h_polyhedron(ax, A, b, ec=color, alpha=1)
except scipy.spatial.qhull.QhullError as e:
print(f"Failed to plot polyhedron at timestep t={t}")
# Plot vertices
vertices = ctrl_result.vertices[t][latent_idx][ov_idx]
X = vertices[:, 0:2].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[:, 2:4].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[:, 4:6].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[:, 6:8].T
ax.scatter(X[0], X[1], color=color, s=2)
if extent is not None:
ax.set_xlim([extent[0], extent[1]])
ax.set_ylim([extent[2], extent[3]])
ax.set_xticks([])
ax.set_yticks([])
ax.set_title(f"t = {t + 1}")
ax.set_aspect('equal')
def plot_oa_simulation_2_timestep(scene,
scene_df,
actual_trajectory,
frame_idx,
planned_frame,
planned_trajectory,
planned_control,
road_segs,
ego_bbox,
step_horizon,
step_size,
n_frames,
filename="oa_simulation",
road_boundary_constraints=True):
"""Helper function of plot_oa_simulation_2()
"""
# Generate plots for map, velocity, heading and steering
fig, axes = plt.subplots(3, 2, figsize=(12, 12))
axes = axes.ravel()
ax = axes[0]
render_scene(ax, scene, global_coordinates=True)
if road_boundary_constraints:
for A, b in road_segs.polytopes:
util.npu.plot_h_polyhedron(ax, A, b, fc='b', ec='b', alpha=0.2)
ovehicle_colors = get_ovehicle_color_set()
frame_df = scene_df.loc[scene_df['frame_id'] == frame_idx]
frame_df = frame_df.loc[frame_df['node_id'] != 'ego']
for idx, (_, node_s) in enumerate(frame_df.iterrows()):
"""Plot OV positions"""
position = node_s[['position_x', 'position_y']].values
heading = node_s['heading_°']
# lw = node_s[['length', 'width']].values # no bounding box in dataset
lw = np.array([3.70, 1.79])
vertices = util.vertices_from_bbox(position, heading, lw)
color = ovehicle_colors[idx][0]
bb = patches.Polygon(vertices.reshape((
-1,
2,
)),
closed=True,
color=color,
fc=util.plu.modify_alpha(color, 0.2))
ax.add_patch(bb)
"EV bounding box at current position"
position = actual_trajectory[frame_idx, :2]
heading = actual_trajectory[frame_idx, 2]
vertices = util.vertices_from_bbox(position, heading, ego_bbox)
bb = patches.Polygon(vertices.reshape((
-1,
2,
)),
closed=True,
color="k",
fc=util.plu.modify_alpha("black", 0.2))
ax.add_patch(bb)
"""Plot EV planned trajectory.
This is computed from CPLEX using LTI vehicle dynamics."""
planned_xy = planned_trajectory[:, :2]
ax.plot(planned_xy.T[0], planned_xy.T[1], "--bo", zorder=20, markersize=2)
"""Plot EV planned trajectory.
This is with original non-linear dyamics and controls from CPLEX."""
gt_planned_trajectory = compute_nonlinear_dynamical_states(
planned_trajectory[0],
planned_trajectory.shape[0] - 1,
step_size,
planned_control,
l_r=0.5 * ego_bbox[0],
L=ego_bbox[0])
gt_planned_xy = gt_planned_trajectory[:, :2]
ax.plot(*gt_planned_xy.T, "--go", zorder=20, markersize=2)
"Plot EV actual trajectory"
# plans are made in current frame, and carried out next frame
actual_xy = actual_trajectory[:, :2]
idx = frame_idx + 1
ax.plot(actual_xy[:idx, 0], actual_xy[:idx, 1], '-ko')
ax.plot(actual_xy[idx:(idx + step_horizon), 0],
actual_xy[idx:(idx + step_horizon), 1],
'-o',
color="orange")
ax.plot(actual_xy[(idx + step_horizon):, 0],
actual_xy[(idx + step_horizon):, 1], '-ko')
"Configure plot"
min_x, min_y = np.min(planned_xy, axis=0) - 20
max_x, max_y = np.max(planned_xy, axis=0) + 20
x_mid, y_mid = (max_x + min_x) / 2, (max_y + min_y) / 2
extent = (x_mid - 30, x_mid + 30, y_mid - 30, y_mid + 30)
ax.set_xlim([extent[0], extent[1]])
ax.set_ylim([extent[2], extent[3]])
ax.set_xticks([])
ax.set_yticks([])
ax.set_title(f"frame {frame_idx + 1}")
ax.set_aspect('equal')
"""Plot v, which is the vehicle speed"""
ax = axes[1]
planned_v = planned_trajectory[:, 3]
gt_planned_v = gt_planned_trajectory[:, 3]
actual_v = actual_trajectory[:, 3]
ax.plot(range(1, n_frames + 1), actual_v, "-k.", label="ground truth")
ax.plot(range(idx, idx + planned_v.size),
planned_v,
"-b.",
label="under LTI")
ax.plot(range(idx, idx + gt_planned_v.size),
gt_planned_v,
"-g.",
label="without LTI")
ax.set_title("$v$ speed of c.g., m/s")
ax.set_ylabel("m/s")
"""Plot psi, which is the vehicle longitudinal angle in global coordinates"""
ax = axes[2]
planned_psi = planned_trajectory[:, 2]
gt_planned_psi = gt_planned_trajectory[:, 2]
actual_psi = actual_trajectory[:, 2]
ax.plot(range(1, n_frames + 1), actual_psi, "-k.", label="ground truth")
ax.plot(range(idx, idx + planned_psi.size),
planned_psi,
"-b.",
label="under LTI")
ax.plot(range(idx, idx + gt_planned_psi.size),
gt_planned_psi,
"-g.",
label="without LTI")
ax.set_title("$\psi$ longitudinal angle, radians")
ax.set_ylabel("rad")
"""Plot delta, which is the turning angle"""
ax = axes[3]
planned_delta = planned_trajectory[:, 4]
gt_planned_delta = gt_planned_trajectory[:, 4]
actual_delta = actual_trajectory[:, 4]
ax.plot(range(1, n_frames + 1), actual_delta, "-k.", label="under LTI")
ax.plot(range(idx, idx + planned_delta.size),
planned_delta,
"-b.",
label="under LTI")
ax.plot(range(idx, idx + gt_planned_delta.size),
gt_planned_delta,
"-g.",
label="without LTI")
ax.set_title("$\delta$ turning angle, radians")
ax.set_ylabel("rad")
"""Plot a, which is acceleration control input"""
ax = axes[4]
planned_a = planned_control[:, 0]
ax.plot(range(idx, idx + planned_a.size), planned_a, "-.", color="orange")
ax.set_title("$a$ acceleration input $m/s^2$")
ax = axes[5]
planned_ddelta = planned_control[:, 1]
ax.plot(range(idx, idx + planned_ddelta.size),
planned_ddelta,
"-.",
color="orange")
ax.set_title("$\dot{\delta}$ turning rate input rad/s")
for ax in axes[1:4]:
ax.set_xlabel("time, s")
ax.grid()
ax.legend()
fig.tight_layout()
fig.savefig(os.path.join('out', f"{filename}_idx{frame_idx}.png"))
fig.clf()
def plot_oa_simulation_1_timestep(scene,
scene_df,
actual_trajectory,
frame_idx,
planned_frame,
planned_trajectory,
planned_controls,
road_segs,
ego_bbox,
step_horizon,
filename="oa_simulation",
road_boundary_constraints=True):
"""Helper function of plot_oa_simulation_1()
"""
fig, ax = plt.subplots(figsize=(6, 6))
render_scene(ax, scene, global_coordinates=True)
if road_boundary_constraints:
for A, b in road_segs.polytopes:
util.npu.plot_h_polyhedron(ax, A, b, fc='b', ec='b', alpha=0.2)
ovehicle_colors = get_ovehicle_color_set()
frame_df = scene_df.loc[scene_df['frame_id'] == frame_idx]
frame_df = frame_df.loc[frame_df['node_id'] != 'ego']
for idx, (_, node_s) in enumerate(frame_df.iterrows()):
"""Plot OV positions"""
position = node_s[['position_x', 'position_y']].values
heading = node_s['heading_°']
# lw = node_s[['length', 'width']].values # no bounding box in dataset
lw = np.array([3.70, 1.79])
vertices = util.vertices_from_bbox(position, heading, lw)
color = ovehicle_colors[idx][0]
bb = patches.Polygon(vertices.reshape((
-1,
2,
)),
closed=True,
color=color,
fc=util.plu.modify_alpha(color, 0.2))
ax.add_patch(bb)
"EV bounding box at current position"
position = actual_trajectory[frame_idx, :2]
heading = actual_trajectory[frame_idx, 2]
vertices = util.vertices_from_bbox(position, heading, ego_bbox)
bb = patches.Polygon(vertices.reshape((
-1,
2,
)),
closed=True,
color="k",
fc=util.plu.modify_alpha("black", 0.2))
ax.add_patch(bb)
"Plot EV planned trajectory"
planned_xy = planned_trajectory[:, :2]
ax.plot(planned_xy.T[0], planned_xy.T[1], "--bo", zorder=20, markersize=2)
"Plot EV actual trajectory"
# plans are made in current frame, and carried out next frame
actual_xy = actual_trajectory[:, :2]
idx = frame_idx + 1
ax.plot(actual_xy[:idx, 0], actual_xy[:idx, 1], '-ko')
ax.plot(actual_xy[idx:(idx + step_horizon), 0],
actual_xy[idx:(idx + step_horizon), 1],
'-o',
color="orange")
ax.plot(actual_xy[(idx + step_horizon):, 0],
actual_xy[(idx + step_horizon):, 1], '-ko')
"Configure plot"
min_x, min_y = np.min(planned_xy, axis=0) - 20
max_x, max_y = np.max(planned_xy, axis=0) + 20
x_mid, y_mid = (max_x + min_x) / 2, (max_y + min_y) / 2
extent = (x_mid - 30, x_mid + 30, y_mid - 30, y_mid + 30)
ax.set_xlim([extent[0], extent[1]])
ax.set_ylim([extent[2], extent[3]])
ax.set_xticks([])
ax.set_yticks([])
ax.set_title(f"frame {frame_idx + 1}")
ax.set_aspect('equal')
fig.tight_layout()
fig.savefig(os.path.join('out', f"{filename}_idx{frame_idx}.png"))
fig.clf()
def plot_lcss_prediction_timestep(ax,
pred_result,
ovehicles,
params,
ctrl_result,
X_star,
t,
ego_bbox,
extent=None):
ovehicle_colors = get_ovehicle_color_set()
render_scene(ax, pred_result.scene, global_coordinates=True)
ax.plot(ctrl_result.goal[0],
ctrl_result.goal[1],
marker='*',
markersize=8,
color="yellow")
# Plot ego vehicle past trajectory
past = None
minpos = np.array([pred_result.scene.x_min, pred_result.scene.y_min])
for node in pred_result.nodes:
if node.id == 'ego':
past = pred_result.past_dict[pred_result.timestep][node] + minpos
break
ax.plot(past[:, 0], past[:, 1], '-ko', markersize=2)
# Box of current ego vehicle position
vertices = util.vertices_from_bbox(params.initial_state.world[:2],
params.initial_state.world[2], ego_bbox)
bb = patches.Polygon(vertices.reshape((
-1,
2,
)),
closed=True,
color='k',
fc=util.plu.modify_alpha("black", 0.2),
ls='-')
ax.add_patch(bb)
# Plot ego vehicle planned trajectory
ax.plot(X_star[:(t + 2), 0], X_star[:(t + 2), 1], '--ko', markersize=2)
# Get vertices of EV and plot its bounding box
if t < 2:
vertices = util.vertices_from_bbox(ctrl_result.X_star[t, :2],
ctrl_result.X_star[t, 2], ego_bbox)
else:
# Make EV bbox look better on far horizons by fixing the heading angle
heading = np.arctan2(
ctrl_result.X_star[t, 1] - ctrl_result.X_star[t - 1, 1],
ctrl_result.X_star[t, 0] - ctrl_result.X_star[t - 1, 0])
vertices = util.vertices_from_bbox(ctrl_result.X_star[t, :2], heading,
ego_bbox)
bb = patches.Polygon(vertices.reshape((
-1,
2,
)),
closed=True,
color='k',
fc='none',
ls='-')
ax.add_patch(bb)
# Plot other vehicles
for ov_idx, ovehicle in enumerate(ovehicles):
color = ovehicle_colors[ov_idx][0]
ax.plot(ovehicle.past[:, 0],
ovehicle.past[:, 1],
marker='o',
markersize=2,
color=color)
heading = np.arctan2(ovehicle.past[-1, 1] - ovehicle.past[-2, 1],
ovehicle.past[-1, 0] - ovehicle.past[-2, 0])
vertices = util.vertices_from_bbox(ovehicle.past[-1], heading,
np.array([ovehicle.bbox]))
bb = patches.Polygon(vertices.reshape((
-1,
2,
)),
closed=True,
color=color,
fc=util.plu.modify_alpha(color, 0.2),
ls='-')
ax.add_patch(bb)
for latent_idx in range(ovehicle.n_states):
color = ovehicle_colors[ov_idx][latent_idx]
# Plot overapproximation
A = ctrl_result.A_union[t][latent_idx][ov_idx]
b = ctrl_result.b_union[t][latent_idx][ov_idx]
try:
util.npu.plot_h_polyhedron(ax, A, b, ec=color, ls='-', alpha=1)
except scipy.spatial.qhull.QhullError:
print(f"Failed to plot polyhedron at timestep t={t}")
# Plot vertices
vertices = ctrl_result.vertices[t][latent_idx][ov_idx]
X = vertices[:, 0:2].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[:, 2:4].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[:, 4:6].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[:, 6:8].T
ax.scatter(X[0], X[1], color=color, s=2)
if extent is not None:
ax.set_xlim([extent[0], extent[1]])
ax.set_ylim([extent[2], extent[3]])
ax.set_xticks([])
ax.set_yticks([])
ax.set_title(f"t = {t + 1}")
ax.set_aspect('equal')
