def __get_nearby_agent_ids(self, radius=None):
if radius is None:
radius = self.radius
player_location = carlautil.transform_to_location_ndarray(
self.player_transform)
"""When there are road above / below ego vehicle,
then we don't want to include them."""
upperbound_z = 4
lowerbound_z = -4
other_ids = util.map_to_ndarray(lambda v: v.id, self.other_vehicles)
other_locations = util.map_to_ndarray(
lambda v: carlautil.transform_to_location_ndarray(v.get_transform(
)), self.other_vehicles)
distances = np.linalg.norm(other_locations - player_location, axis=1)
z_displacements = other_locations[:, -1] - player_location[-1]
df = pd.DataFrame({
'ids': other_ids,
'distances': distances,
'z_displacements': z_displacements
})
df = df[df['distances'] < radius]
df = df[df['z_displacements'].between(lowerbound_z,
upperbound_z,
inclusive=False)]
df.sort_values('distances', inplace=True)
return df['ids'].to_numpy()
def extract_junction_points(self, tlight_find_radius=25.):
carla_topology = self.carla_map.get_topology()
junctions = carlautil.get_junctions_from_topology_graph(carla_topology)
tlights = util.filter_to_list(lambda a: 'traffic_light' in a.type_id,
self.carla_world.get_actors())
tlight_distances = np.zeros((
len(tlights),
len(junctions),
))
f = lambda j: carlautil.location_to_ndarray(j.bounding_box.location)
junction_locations = util.map_to_ndarray(f, junctions)
g = lambda tl: carlautil.transform_to_location_ndarray(tl.
get_transform())
tlight_locations = util.map_to_ndarray(g, tlights)
for idx, junction in enumerate(junctions):
tlight_distances[:, idx] = np.linalg.norm(tlight_locations -
junction_locations[idx],
axis=1)
is_controlled_junction = (tlight_distances < tlight_find_radius).any(
axis=0)
is_uncontrolled_junction = np.logical_not(is_controlled_junction)
controlled_junction_locations \
= junction_locations[is_controlled_junction]
uncontrolled_junction_locations \
= junction_locations[is_uncontrolled_junction]
return util.AttrDict(controlled=controlled_junction_locations,
uncontrolled=uncontrolled_junction_locations)
def __init__(self, carla_world, carla_map, debug=False):
"""
Parameters
----------
carla_world : carla.World
carla_map : carla.Map
debug : bool
"""
super().__init__(carla_world, carla_map, debug=debug)
"""Generate slope topology of the map"""
# get waypoints
wps = self.carla_map.generate_waypoints(4.0)
def h(wp):
l = wp.transform.location
pitch = wp.transform.rotation.pitch
return np.array([l.x, l.y, l.z, pitch])
loc_and_pitch_of_wps = util.map_to_ndarray(h, wps)
self.wp_locations = loc_and_pitch_of_wps[:, :-1]
self.wp_pitches = loc_and_pitch_of_wps[:, -1]
self.wp_pitches = self.wp_pitches % 360.
self.wp_is_sloped = np.logical_and(
self.SLOPE_DEGREES < self.wp_pitches,
self.wp_pitches < self.SLOPE_UDEGREES)
"""Generate intersection topology of the map"""
# get nodes in graph
topology = self.carla_map.get_topology()
G = nx.Graph()
G.add_edges_from(topology)
tlights = util.filter_to_list(lambda a: 'traffic_light' in a.type_id,
self.carla_world.get_actors())
junctions = carlautil.get_junctions_from_topology_graph(G)
tlight_distances = np.zeros((
len(tlights),
len(junctions),
))
f = lambda j: carlautil.location_to_ndarray(j.bounding_box.location)
junction_locations = util.map_to_ndarray(f, junctions)
g = lambda tl: carlautil.transform_to_location_ndarray(tl.
get_transform())
tlight_locations = util.map_to_ndarray(g, tlights)
for idx, junction in enumerate(junctions):
tlight_distances[:, idx] = np.linalg.norm(tlight_locations -
junction_locations[idx],
axis=1)
is_controlled_junction = (tlight_distances <
self.TLIGHT_FIND_RADIUS).any(axis=0)
is_uncontrolled_junction = np.logical_not(is_controlled_junction)
self.controlled_junction_locations \
= junction_locations[is_controlled_junction]
self.uncontrolled_junction_locations \
= junction_locations[is_uncontrolled_junction]
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 extract_waypoint_points(self,
sampling_precision=4.0,
slope_degrees=5.0):
"""Extract slope topology of the map"""
slope_udegrees = (360. - slope_degrees)
wps = self.carla_map.generate_waypoints(sampling_precision)
def h(wp):
l = wp.transform.location
pitch = wp.transform.rotation.pitch
return np.array([l.x, l.y, l.z, pitch])
loc_and_pitch_of_wps = util.map_to_ndarray(h, wps)
wp_locations = loc_and_pitch_of_wps[:, :-1]
wp_pitches = loc_and_pitch_of_wps[:, -1]
self.wp_pitches = self.wp_pitches % 360.
wp_is_sloped = np.logical_and(slope_degrees < self.wp_pitches,
self.wp_pitches < slope_udegrees)
return pd.DataFrame({
'x': wp_locations[0],
'y': wp_locations[1],
'z': wp_locations[2],
'pitch': wp_pitches,
'is_sloped': wp_is_sloped
})
def __interval_index_ids(wps, distances):
data = util.map_to_ndarray(
lambda wp: [wp.is_junction, wp.road_id, wp.section_id, wp.lane_id],
wps)[1:]
return pd.DataFrame(
data,
columns=["is_junction", "road_id", "section_id", "lane_id"],
index=pd.IntervalIndex.from_breaks(distances))
def old_get_nearby_agent_ids(self, radius=None):
"""Gets all IDs of other vehicles that are radius away from ego vehicle and
returns IDs sorted by nearest to player first.
TODO: don't include other vehicles below / above th ego vehicle
TODO: delete
"""
if radius is None:
radius = self.radius
player_location = carlautil.transform_to_location_ndarray(
self.player_transform)
other_ids = util.map_to_ndarray(lambda v: v.id, self.other_vehicles)
other_locations = util.map_to_ndarray(
lambda v: carlautil.transform_to_location_ndarray(v.get_transform(
)), self.other_vehicles)
distances = np.linalg.norm(other_locations - player_location, axis=1)
df = pd.DataFrame({'ids': other_ids, 'distances': distances})
df = df[df['distances'] < radius]
df.sort_values('distances', inplace=True)
return df['ids'].to_numpy()
def inner(wp):
wps = wp.next_until_lane_end(PRECISION)
points = np.array(util.map_to_ndarray(to_point, wps))
distances = util.distances_from_line_2d(points, x_start, y_start,
x_end, y_end)
wps_mask = np.nonzero(distances > tol)[0]
if wps_mask.size > 0:
# waypoints veer off a straight line
idx = wps_mask[0]
return points[:idx], list()
else:
# waypoints stay one straight line. Get the next set of waypoints
return points, wps[-1].next(PRECISION)
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 __init__(self,
start_wp,
max_distance,
lane_width,
choices=[],
flip_x=False,
flip_y=False):
"""Constructor.
Parameters
==========
start_wp : carla.Waypoint
Waypoint designating the start of the path.
max_distance : float
Maximum distance of path from start_wp we want to sample from.
lane_width : float
Size of road.
choices : list of int
Indices of turns at each junction along the path from start_wp onwards.
If there are more junctions than indices contained in choices, then
choose default turn.
"""
self.delta = lane_width / 2
(self.waypoints, self.points,
self.distances) = carlautil.collect_points_along_path(
start_wp,
choices,
max_distance,
precision=self.__PRECISION,
flip_x=flip_x,
flip_y=flip_y)
self.distance_to_point = pd.DataFrame(
{
"x": self.points[1:, 0],
"y": self.points[1:, 1],
"distances": self.distances[1:]
},
index=pd.IntervalIndex.from_breaks(self.distances))
self.junction_mask = self.__pad_junction_mask(
util.map_to_ndarray(lambda wp: wp.is_junction, self.waypoints))
self.ids_df = self.__interval_index_ids(self.waypoints, self.distances)
self.points_splits, self.split_mask = self.__split_line_by_mask(
self.points, self.junction_mask)
self.distances_splits, _ = self.__split_line_by_mask(
self.distances, self.junction_mask)
self.splines = []
self.dsplines = []
self.ddsplines = []
for distances, points in zip(self.distances_splits,
self.points_splits):
spline = scipy.interpolate.CubicSpline(distances, points, axis=0)
"""Can make spline with another degree"""
#self.spline = scipy.interpolate.make_interp_spline(
# self.distances, self.points, k=self.__K, axis=0
#)
"""Can be LSQ B-spline, if waypoint points aren't aligned, but not necessary"""
#t = np.linspace(self.distances[1], self.distances[-2], n_points // 2)
#t = np.r_[(self.distances[0],)*(self.__K+1), t, (self.distances[-1],)*(self.__K+1)]
#self.spline = scipy.interpolate.make_lsq_spline(
# self.distances, self.points, t=t, k=self.__K, axis=0
#)
dspline = spline.derivative(1)
ddspline = spline.derivative(2)
self.splines.append(spline)
self.dsplines.append(dspline)
self.ddsplines.append(ddspline)
self.road_segs = self.cover_along_path_varyingsize()
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 plot_oa_simulation_0(map_data,
actual_trajectory,
planned_trajectories,
planned_controls,
goals,
lowlevel,
road_segs,
ego_bbox,
step_horizon,
steptime,
filename="oa_simulation",
road_boundary_constraints=True):
"""Plot to compare between actual trajectory and planned trajectories.
Produces one plot per MPC step. Produces plots for velocity, steering.
Parameters
==========
map_data : util.AttrDict
Container of vertices for road segments and lines produced by MapQuerier.
actual_trajectory : collections.OrderedDict of (int, ndarray)
Indexed by frame ID. Array of EV global (x, y) position, heading and speed.
planned_trajectories : collections.OrderedDict of (int, ndarray)
Indexed by frame ID. The EV's planned state over timesteps T+1 incl. origin
with global (x, y) position, heading and speed as ndarray of shape (4, T + 1).
planned_controls : collections.OrderedDict of (int, ndarray)
Indexed by frame ID. The EV's planned controls over timesteps T with
acceleration, steering as ndarray of shape (2, T).
goals : collections.OrderedDict of (int, util.AttrDict)
Indexed by frame ID. EV's goal.
lowlevel : util.AttrDict
PID statistics.
road_segs : util.AttrDict
Container of road segment properties.
ego_bbox : ndarray
EV's longitudinal and lateral dimensions.
step_horizon : int
Number of predictions steps to execute at each iteration of MPC.
steptime : float
Time in seconds taken to complete one step of MPC.
filename : str
Partial file name to save plots.
road_boundary_constraints : bool
Whether to visualize boundary constrains in plots.
"""
frames, actual_trajectory = util.unzip(
[i for i in actual_trajectory.items()])
frames = np.array(frames)
actual_trajectory = np.stack(actual_trajectory)
planned_frames, planned_controls = util.unzip(
[i for i in planned_controls.items()])
_, planned_trajectories = util.unzip(
[i for i in planned_trajectories.items()])
_, goals = util.unzip([i for i in goals.items()])
for planned_frame, planned_trajectory, planned_control, goal \
in zip(planned_frames, planned_trajectories, planned_controls, goals):
frame_idx = np.argwhere(frames == planned_frame)[0, 0]
plot_oa_simulation_timestep_0(
map_data,
actual_trajectory,
frame_idx,
planned_frame,
planned_trajectory,
planned_control,
goal,
road_segs,
ego_bbox,
step_horizon,
steptime,
frames.size,
filename=filename,
road_boundary_constraints=road_boundary_constraints)
# plot PID controller / actuation
_, lowlevel = util.unzip([i for i in lowlevel.items()])
m_speed = util.map_to_ndarray(lambda x: x.measurement.speed, lowlevel)
r_speed = util.map_to_ndarray(lambda x: x.reference.speed, lowlevel)
m_angle = util.map_to_ndarray(lambda x: x.measurement.angle, lowlevel)
r_angle = util.map_to_ndarray(lambda x: x.reference.angle, lowlevel)
m_angle = util.npu.warp_radians_neg_pi_to_pi(m_angle)
r_angle = util.npu.warp_radians_neg_pi_to_pi(r_angle)
c_throttle = util.map_to_ndarray(lambda x: x.control.throttle, lowlevel)
c_brake = util.map_to_ndarray(lambda x: x.control.brake, lowlevel)
c_steer = util.map_to_ndarray(lambda x: x.control.steer, lowlevel)
pe_speed = util.map_to_ndarray(lambda x: x.error.speed.pe, lowlevel)
ie_speed = util.map_to_ndarray(lambda x: x.error.speed.ie, lowlevel)
de_speed = util.map_to_ndarray(lambda x: x.error.speed.de, lowlevel)
pe_angle = util.map_to_ndarray(lambda x: x.error.angle.pe, lowlevel)
ie_angle = util.map_to_ndarray(lambda x: x.error.angle.ie, lowlevel)
de_angle = util.map_to_ndarray(lambda x: x.error.angle.de, lowlevel)
fig, axes = plt.subplots(3, 2, figsize=(20, 20))
axes = axes.T.ravel()
steptimes = np.arange(len(lowlevel)) * 0.05
ax = axes[0]
ax.plot(steptimes, m_speed, "-g.", label="measured speed")
ax.plot(steptimes,
r_speed,
"-",
marker='.',
color="orange",
label="reference speed")
ax.set_title("speed")
ax.set_ylabel("m/s")
ax = axes[1]
ax.plot(steptimes, pe_speed, "-r.", label="prop. error")
ax.plot(steptimes, ie_speed, "-b.", label="integral error")
ax.plot(steptimes, de_speed, "-g.", label="derivative error")
ax.set_title("speed error")
# ax.set_ylim([-10, 10])
ax = axes[2]
ax.plot(steptimes, c_throttle, "-b.", label="applied throttle")
ax.plot(steptimes, c_brake, "-r.", label="applied throttle")
ax.set_title("longitudinal control")
ax = axes[3]
ax.plot(steptimes, m_angle, "-g.", label="measured angle")
ax.plot(steptimes,
r_angle,
"-",
marker='.',
color="orange",
label="reference angle")
ax.set_title("angle")
ax.set_ylabel("rad")
ax = axes[4]
ax.plot(steptimes, pe_angle, "-r.", label="prop. error")
ax.plot(steptimes, ie_angle, "-b.", label="integral error")
ax.plot(steptimes, de_angle, "-g.", label="derivative error")
# ax.set_ylim([-10, 10])
ax.set_title("angle error")
ax = axes[5]
ax.plot(steptimes, c_steer, "-b.", label="applied steer")
ax.set_title("lateral control")
for ax in axes:
ax.grid()
ax.legend()
ax.set_xlabel("seconds s")
fig.tight_layout()
fig.savefig(os.path.join('out', f"{filename}_pid.png"))
fig.clf()
def plot_oa_simulation(map_data,
actual_trajectory,
planned_trajectories,
planned_controls,
goals,
road_segs,
ego_bbox,
step_horizon,
filename="oa_simulation",
road_boundary_constraints=True):
"""Plot to compare between actual trajectory and planned trajectories"""
frames, actual_trajectory = util.unzip(
[i for i in actual_trajectory.items()])
frames = np.array(frames)
actual_trajectory = np.stack(actual_trajectory)
actual_xy = actual_trajectory[:, :2]
actual_psi = actual_trajectory[:, 2]
actual_delta = actual_trajectory[:, 4]
planned_frames, planned_trajectories = util.unzip(
[i for i in planned_trajectories.items()])
goals = util.map_to_ndarray(util.identity, goals.values())
N = len(planned_trajectories)
fig, axes = plt.subplots(N, 3, figsize=(20, (10 / 2) * N))
if N == 1:
axes = axes[None]
for _axes, planned_frame, planned_trajectory, goal \
in zip(axes, planned_frames, planned_trajectories, goals):
ax = _axes[0]
planned_xy = np.concatenate((
planned_trajectory[:, :2],
goal[None],
))
min_x, min_y = np.min(planned_xy, axis=0) - 20
max_x, max_y = np.max(planned_xy, axis=0) + 20
extent = (min_x, max_x, min_y, max_y)
planned_xy = planned_trajectory[:, :2]
"""Map overlay"""
render_map_crop(ax, map_data, extent)
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)
ax.plot(goal[0], goal[1], marker='*', markersize=8, color="green")
"""Plot planned trajectories on separate plots"""
ax.plot(planned_xy.T[0], planned_xy.T[1], "--b.", zorder=20)
# plans are made in current frame, and carried out next frame
idx = np.argwhere(frames == planned_frame)[0, 0] + 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')
ax.set_xlim([min_x, max_x])
ax.set_ylim([min_y, max_y])
"""Plot psi, which is the vehicle longitudinal angle in global coordinates"""
ax = _axes[1]
planned_psi = planned_trajectory[:, 2]
ax.plot(range(1, frames.size + 1), actual_psi, "-k.")
ax.plot(range(idx, idx + planned_psi.size), planned_psi, "-b.")
"""Plot delta, which is the turning angle"""
ax = _axes[2]
planned_delta = planned_trajectory[:, 4]
ax.plot(range(1, frames.size + 1), actual_delta, "-k.")
ax.plot(range(idx, idx + planned_delta.size), planned_delta, "-b.")
for ax in axes[:, 1:].ravel():
ax.grid()
axes[0, 0].set_title("Trajectories on map")
axes[0, 1].set_title("$\Psi$ EV longitudinal angle, radians")
axes[0, 2].set_title("$\delta$ EV turning angle, radians")
fig.tight_layout()
fig.savefig(os.path.join('out', f"{filename}.png"))
fig.clf()
def plot_oa_simulation(map_data,
actual_trajectory,
planned_trajectories,
planned_controls,
goals,
road_segs,
ego_bbox,
step_horizon,
filename="oa_simulation",
road_boundary_constraints=True):
"""Plot to compare between actual trajectory and planned trajectories"""
frames, actual_trajectory = util.unzip(
[i for i in actual_trajectory.items()])
frames = np.array(frames)
actual_trajectory = np.stack(actual_trajectory)
actual_xy = actual_trajectory[:, :2]
planned_frames, planned_trajectories = util.unzip(
[i for i in planned_trajectories.items()])
goals = util.map_to_ndarray(lambda goal: [goal.x, goal.y], goals.values())
N = len(planned_trajectories)
fig, axes = plt.subplots(N // 2 + (N % 2), 2, figsize=(10, (10 / 4) * N))
try:
axes = axes.ravel()
except AttributeError:
axes = np.array([axes])
for ax, planned_frame, planned_trajectory, goal \
in zip(axes, planned_frames, planned_trajectories, goals):
planned_xy = np.concatenate((
planned_trajectory[:, :2],
goal[None],
))
min_x, min_y = np.min(planned_xy, axis=0) - 20
max_x, max_y = np.max(planned_xy, axis=0) + 20
extent = (min_x, max_x, min_y, max_y)
planned_xy = planned_trajectory[:, :2]
"""Map overlay"""
render_map_crop(ax, map_data, extent)
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)
ax.plot(goal[0], goal[1], marker='*', markersize=8, color="green")
"""Plot planned trajectories on separate plots"""
ax.plot(planned_xy.T[0], planned_xy.T[1], "--b.", zorder=20)
# plans are made in current frame, and carried out next frame
idx = np.argwhere(frames == planned_frame)[0, 0] + 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')
ax.set_xlim([min_x, max_x])
ax.set_ylim([min_y, max_y])
fig.tight_layout()
fig.savefig(os.path.join('out', f"{filename}.png"))
fig.clf()
