def plot_oa_simulation(scene,
map_data,
actual_trajectory,
planned_trajectories,
planned_controls,
goals,
lowlevel,
road_segs,
ego_bbox,
step_horizon,
n_coincide,
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
==========
planned_trajectories : collections.OrderedDict of (int, ndarray)
Indexed by frame ID. The EV's planned contingency states over timesteps T+1
incl. origin with global (x, y) position, heading and speed as ndarray of
shape (*, T + 1, 4). The first dimension depends on the OV predicitons.
"""
scene_df = scene_to_df(scene)
scene_df[['position_x',
'position_y']] += np.array([scene.x_min, scene.y_min])
node_ids = scene_df['node_id'].unique()
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(
scene,
scene_df,
node_ids,
map_data,
actual_trajectory,
frame_idx,
planned_frame,
planned_trajectory,
planned_control,
goal,
road_segs,
ego_bbox,
step_horizon,
n_coincide,
steptime,
frames.size,
filename=filename,
road_boundary_constraints=road_boundary_constraints)
def plot_oa_simulation_1(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.
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_1(
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)
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 visualize_wheel_turning_3():
config = parse_arguments()
client = carla.Client(config.host, config.port)
client.set_timeout(10.0)
world = client.get_world()
carla_map = world.get_map()
blueprint = world.get_blueprint_library().find("vehicle.audi.a2")
spawn_point = carla_map.get_spawn_points()[0]
vehicle = None
vwl = carla.VehicleWheelLocation
try:
vehicle = world.spawn_actor(blueprint, spawn_point)
for _ in range(20): world.wait_for_tick()
transform = carlautil.spherical_to_camera_watcher_transform(
3, math.pi, math.pi*(1/6),
location=vehicle.get_location())
world.get_spectator().set_transform(transform)
for _ in range(20): world.wait_for_tick()
fl_angles = dict()
fr_angles = dict()
def plot(snapshot):
fl_angle = vehicle.get_wheel_steer_angle(vwl.FL_Wheel)
fr_angle = vehicle.get_wheel_steer_angle(vwl.FR_Wheel)
fl_angles[snapshot.timestamp.elapsed_seconds] = fl_angle
fr_angles[snapshot.timestamp.elapsed_seconds] = fr_angle
def derivative(x, y):
assert len(x) == len(y)
dy = [None]*(len(x) - 1)
for i in range(len(x) - 1):
dy[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i])
assert len(x[:-1]) == len(dy)
return x[:-1], dy
## Block 1
callback_id = world.on_tick(plot)
"""get_wheel_steer_angle() returns angle in degrees between
45 and 70 deg so the actual turning is more like 57.5 deg.
The wheel on the side the car is turning turns a higher angle.
Wheels reach max turns from 0 in 0.2 seconds so 287.5 deg/s."""
steering_choices = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0]
# steering_choices = [0.6]
for steer in steering_choices:
for _ in range(6): world.wait_for_tick()
time.sleep(0.8)
for _ in range(6): world.wait_for_tick()
vehicle.apply_control(carla.VehicleControl(steer=steer))
for _ in range(6): world.wait_for_tick()
time.sleep(0.8)
for _ in range(6): world.wait_for_tick()
vehicle.apply_control(carla.VehicleControl(steer=0))
for _ in range(6): world.wait_for_tick()
time.sleep(0.8)
world.remove_on_tick(callback_id)
world.wait_for_tick()
fig, axes = plt.subplots(1, 2, figsize=(20,10))
_time, _angles = util.unzip(fl_angles.items())
axes[0].plot(_time, _angles, "-bo", label="FL_Wheel")
_time, _dangles = derivative(_time, _angles)
axes[1].plot(_time, _dangles, "-bo", label="FL_Wheel")
_time, _angles = util.unzip(fr_angles.items())
axes[0].plot(_time, _angles, "-ro", label="FR_Wheel")
_time, _dangles = derivative(_time, _angles)
axes[1].plot(_time, _dangles, "-ro", label="FR_Wheel")
axes[0].legend()
fig.savefig("out/steer")
# plt.show()
finally:
if vehicle:
vehicle.destroy()
vehicle = None
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_multiple_simulation(scene,
actual_trajectory,
planned_trajectories,
planned_controls,
road_segs,
ego_bbox,
control_horizon,
filename="oa_simulation"):
"""Plot to compare between actual trajectory and planned trajectories"""
scene_df = scene_to_df(scene)
scene_df[['position_x',
'position_y']] += np.array([scene.x_min, scene.y_min])
node_ids = scene_df['node_id'].unique()
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()])
N = len(planned_trajectories)
fig, axes = plt.subplots(N // 2 + (N % 2), 2, figsize=(10, (10 / 4) * N))
axes = axes.ravel()
for ax in axes:
"""Render road overlay and plot actual trajectory"""
render_scene(ax, scene, global_coordinates=True)
for A, b in road_segs.polytopes:
util.npu.plot_h_polyhedron(ax, A, b, fc='b', ec='b', alpha=0.2)
for idx, node_id in enumerate(node_ids):
"""Plot OV trajectory"""
if node_id == "ego": continue
node_df = scene_df[scene_df['node_id'] == node_id]
X = node_df[['position_x', 'position_y']].values.T
for ax in axes:
ax.plot(X[0], X[1], ':.', color=AGENT_COLORS[idx % NCOLORS])
for ax, planned_frame, planned_trajectory in zip(axes, planned_frames,
planned_trajectories):
"""Plot planned trajectories on separate plots"""
idx = np.argwhere(frames == planned_frame)[0, 0] + 1
# plans are made in current frame, and carried out next frame
ax.plot(actual_xy[:idx, 0], actual_xy[:idx, 1], '-ko')
ax.plot(actual_xy[idx:(idx + control_horizon), 0],
actual_xy[idx:(idx + control_horizon), 1],
'-o',
color="orange")
ax.plot(actual_xy[(idx + control_horizon):, 0],
actual_xy[(idx + control_horizon):, 1], '-ko')
# planned_trajectory has shape (N_select, T, nx)
for _planned_trajectory in planned_trajectory:
planned_xy = _planned_trajectory[:, :2]
ax.plot(planned_xy.T[0], planned_xy.T[1], "--b.", zorder=20)
_planned_xy = planned_trajectory.reshape(-1,
planned_trajectory.shape[2])
min_x, min_y = np.min(_planned_xy[:, :2], axis=0) - 20
max_x, max_y = np.max(_planned_xy[:, :2], axis=0) + 20
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()
def plot_oa_simulation_0(scene,
actual_trajectory,
planned_trajectories,
planned_controls,
road_segs,
ego_bbox,
step_horizon,
step_size,
filename="oa_simulation",
road_boundary_constraints=True):
"""Plot to compare between actual trajectory and planned trajectories.
This method puts all MPC steps in one plot.
TODO: this needs to be refactored to use improved bicycle model.
"""
scene_df = scene_to_df(scene)
scene_df[['position_x',
'position_y']] += np.array([scene.x_min, scene.y_min])
node_ids = scene_df['node_id'].unique()
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()])
N = len(planned_trajectories)
fig, axes = plt.subplots(N, 3, figsize=(20, (10 / 2) * N))
if axes.ndim == 1:
axes = axes[None]
for ax in axes[:, 0]:
"""Render road overlay and plot actual trajectory"""
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)
for idx, node_id in enumerate(node_ids):
"""Plot OV trajectory"""
if node_id == "ego": continue
node_df = scene_df[scene_df['node_id'] == node_id]
X = node_df[['position_x', 'position_y']].values.T
for ax in axes[:, 0]:
ax.plot(X[0], X[1], ':.', color=AGENT_COLORS[idx % NCOLORS])
for _axes, planned_frame, planned_trajectory in zip(
axes, planned_frames, planned_trajectories):
"""Plot planned trajectories on separate plots"""
ax = _axes[0]
planned_xy = planned_trajectory[:, :2]
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')
min_x, min_y = np.min(planned_xy, axis=0) - 20
max_x, max_y = np.max(planned_xy, axis=0) + 20
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_2(scene,
actual_trajectory,
planned_trajectories,
planned_controls,
road_segs,
ego_bbox,
step_horizon,
step_size,
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.
TODO: this needs to be refactored to use improved bicycle model.
TODO: possibly redundant
Parameters
==========
scene : Scene
A scene produced by SceneBuilder containing environment and OV data.
actual_trajectory : collections.OrderedDict of (int, ndarray)
Indexed by frame ID. The EV's coordinate information produced by
carlautil.actor_to_Lxyz_Vxyz_Axyz_Blwh_Rpyr_ndarray().
planned_trajectories : collections.OrderedDict of (int, ndarray)
Indexed by frame ID. The EV's planned state over timesteps T with
(x position, y position, heading, speed, steering angle) as ndarray
of shape (5, T + 1).
planned_controls :
Indexed by frame ID. The EV's planned controls over timesteps T with
(acceleration, steering rate) as ndarray of shape (2, T).
road_segs : util.AttrDict
Container of road segment properties.
"""
scene_df = scene_to_df(scene)
scene_df[['position_x',
'position_y']] += np.array([scene.x_min, scene.y_min])
scene_df = scene_df.sort_values('node_id')
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_frames, planned_trajectories = util.unzip(
[i for i in planned_trajectories.items()])
for planned_frame, planned_trajectory, planned_control \
in zip(planned_frames, planned_trajectories, planned_controls):
frame_idx = np.argwhere(frames == planned_frame)[0, 0]
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,
frames.size,
filename=filename,
road_boundary_constraints=road_boundary_constraints)
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()