def redisplay(event):
figure.clf()
axes = figure.add_subplot(1, 1, 1)
if (event is not None) or (gridding[0] is None):
do_gridding(first_x.Value, first_y.Value, second_x.Value, second_y.Value)
self.display_grid(background_image, gridding[0], image_set_number, axes)
canvas.draw()
def redisplay(event):
figure.clf()
axes = figure.add_subplot(1, 1, 1)
if (event is not None) or (gridding[0] is None):
do_gridding(first_x.Value, first_y.Value, second_x.Value,
second_y.Value)
self.display_grid(background_image, gridding[0], image_set_number,
axes)
canvas.draw()
def display(self, workspace, figure):
if self.show_window:
figure.set_subplots((1, 1))
figure.clf()
ax = figure.subplot(0, 0)
gridding = cpg.CPGridInfo()
gridding.deserialize(workspace.display_data.gridding)
self.display_grid(workspace.display_data.background_image,
gridding,
workspace.display_data.image_set_number, ax)
def display(self, workspace, figure):
if self.show_window:
figure.set_subplots((1, 1))
figure.clf()
ax = figure.subplot(0, 0)
gridding = cpg.CPGridInfo()
gridding.deserialize(workspace.display_data.gridding)
self.display_grid(
workspace.display_data.background_image, gridding, workspace.display_data.image_set_number, ax
)
def update(self, robots, motions, short_range_measurements,
long_range_measurements):
"""Perform visualization updates here."""
self.logger.debug("Updating visualization.")
# TODO convert to numpy arrays for scalability
x_robot_gt = []
y_robot_gt = []
x_goal = []
y_goal = []
headings = []
if (self.view_mode == "past_trajectories"):
max_num_poses = 0
for i in range(0, len(robots)):
for j in range(0, len(robots)):
if (j in robots[i].n_poses):
max_num_poses = max(max_num_poses,
robots[i].n_poses[j])
# create square matrix showing all the robots' beliefs about each other
robot_beliefs = np.zeros(
[len(robots), len(robots), max_num_poses,
2]) # 2 bc of x and y
for i, motion in enumerate(motions):
self.logger.debug("Robot {} has pos {}, vel {}".format(
i, motion.pos, motion.vel))
pos = motion.pos
x_robot_gt.append(pos[0])
y_robot_gt.append(pos[1])
robot = robots[i]
for goal_index in range(0, robot.goal_index + 1):
goal = robot.my_goals[goal_index]
x_goal.append(goal[0])
y_goal.append(goal[1])
for other_robot_id in range(0, len(robots)):
if other_robot_id in robot.n_poses:
for pose_index in range(0,
robot.n_poses[other_robot_id]):
start = sum([
robot.n_poses[id] for id in robot.n_poses
if id < other_robot_id
])
j0 = robot.pose_dim * (start + pose_index)
robot_beliefs[i, other_robot_id, pose_index,
0] = robot.x[j0 + 1]
robot_beliefs[i, other_robot_id, pose_index,
1] = robot.x[j0 + 2]
self.robot_errors[i].append(
self.get_euclidean_error(
pos, robot_beliefs[i, i, robot.n_poses[i] - 1, :]))
vel = motion.vel
headings.append((vel[0], vel[1]))
# set the viewport bounds according to all the beliefs and goals
x_min_coord_temp = min(np.min(robot_beliefs[:, :, :, 0]),
min(x_goal))
x_max_coord_temp = max(np.max(robot_beliefs[:, :, :, 0]),
max(x_goal))
y_min_coord_temp = min(np.min(robot_beliefs[:, :, :, 1]),
min(y_goal))
y_max_coord_temp = max(np.max(robot_beliefs[:, :, :, 1]),
max(y_goal))
else:
robot_beliefs = np.zeros([len(robots), len(robots),
2]) # 2 bc of x and y
for i, motion in enumerate(motions):
self.logger.debug("Robot {} has pos {}, vel {}".format(
i, motion.pos, motion.vel))
pos = motion.pos
x_robot_gt.append(pos[0])
y_robot_gt.append(pos[1])
robot = robots[i]
for goal_index in range(0, robot.goal_index + 1):
goal = robot.my_goals[goal_index]
x_goal.append(goal[0])
y_goal.append(goal[1])
for other_robot_id in range(0, len(robots)):
(other_bel_x,
other_bel_y) = robot.robot_pos(other_robot_id)
if other_bel_x is not None:
robot_beliefs[i, other_robot_id, 0] = other_bel_x
robot_beliefs[i, other_robot_id, 1] = other_bel_y
self.robot_errors[i].append(
self.get_euclidean_error(pos, robot_beliefs[i, i, :]))
vel = motion.vel
headings.append((vel[0], vel[1]))
# set the viewport bounds according to all the beliefs and goals
x_min_coord_temp = min(np.min(robot_beliefs[:, :, 0]), min(x_goal))
x_max_coord_temp = max(np.max(robot_beliefs[:, :, 0]), max(x_goal))
y_min_coord_temp = min(np.min(robot_beliefs[:, :, 1]), min(y_goal))
y_max_coord_temp = max(np.max(robot_beliefs[:, :, 1]), max(y_goal))
# get the main update figure, clear it out for our update
# fig = plt.figure(self.main_figure_number)
# fig.clf()
# draw the error graphs
# error_fig = plt.figure(self.error_figure_number)
# error_fig.clf()
# num_subplots = len(robots)
# for i in range(1, num_subplots + 1):
# plt.subplot(num_subplots, 1, i)
# plt.plot(self.robot_errors[i-1])
# get the main update figure, clear it out for our update
fig = plt.figure(self.main_figure_number)
fig.clf()
# calculate viewport bounds
t1 = 2
t2 = 2
self.x_min_coord = min(self.x_min_coord,
(np.floor(x_min_coord_temp / t1) * t1) - t2)
self.x_max_coord = max(self.x_max_coord,
(np.ceil(x_max_coord_temp / t1) * t1) + t2)
self.y_min_coord = min(self.y_min_coord,
(np.floor(y_min_coord_temp / t1) * t1) - t2)
self.y_max_coord = max(self.y_max_coord,
(np.ceil(y_max_coord_temp / t1) * t1) + t2)
x_scale = self.x_max_coord - self.x_min_coord
y_scale = self.y_max_coord - self.y_min_coord
#make the scale the same on each axis
if (x_scale > y_scale):
self.y_max_coord = (
(self.y_max_coord + self.y_min_coord) / 2) + (x_scale / 2)
self.y_min_coord = self.y_max_coord - x_scale
else:
self.x_max_coord = (
(self.x_max_coord + self.x_min_coord) / 2) + (y_scale / 2)
self.x_min_coord = self.x_max_coord - y_scale
plt.xlim([self.x_min_coord, self.x_max_coord])
plt.ylim([self.y_min_coord, self.y_max_coord])
# plt.xlim([self.x_min_coord - margin, self.x_max_coord + margin])
# plt.ylim([self.y_min_coord - margin, self.y_max_coord + margin])
#draw force fields
X, Y = np.meshgrid(np.arange(self.x_min_coord, self.x_max_coord, 1),
np.arange(self.y_min_coord, self.y_max_coord, 1))
U = self.disturbance(0 * X, X, Y)[1]
V = self.disturbance(0 * X, X, Y)[2]
Q = plt.quiver(X, Y, U, V, units='width', color=(0.0, 0.0, 0.0, 0.3))
# draw robots ground truth
plt.scatter(x_robot_gt, y_robot_gt, c='k', marker='o')
if (self.view_mode == "past_trajectories"):
# draw the robot belief as their own individual colors
for i in range(0, len(robots)):
belief_x = np.ndarray([])
belief_y = np.ndarray([])
i_x = 0
i_y = 0
for other_robot_id in range(0, len(robots)):
if (other_robot_id in robots[i].n_poses):
belief_x = np.append(
belief_x,
robot_beliefs[i, other_robot_id,
0:robots[i].n_poses[other_robot_id],
0])
belief_y = np.append(
belief_y,
robot_beliefs[i, other_robot_id,
0:robots[i].n_poses[other_robot_id],
1])
if (other_robot_id == i):
i_x = len(belief_x) - 1
i_y = len(belief_y) - 1
plt.scatter(belief_x, belief_y, marker='o')
plt.annotate(xy=(belief_x[i_x], belief_y[i_y]),
s="{}".format(i))
else:
for i in range(0, len(robots)):
robot_i_beliefs_x = robot_beliefs[i, :, 0]
robot_i_beliefs_y = robot_beliefs[i, :, 1]
plt.scatter(robot_i_beliefs_x, robot_i_beliefs_y, marker='o')
for j in range(len(robot_i_beliefs_x)):
plt.annotate(xy=(robot_i_beliefs_x[j],
robot_i_beliefs_y[j]),
s="{}".format(j))
#draw legends
robot_legend_labels = [
"Robot {} beliefs".format(i) for i in range(0, len(robots))
]
robot_legend_labels = ["Disturbance Field", "Ground Truths"
] + robot_legend_labels
plt.legend(robot_legend_labels,
loc='upper center',
bbox_to_anchor=(0.5, 1.15),
fancybox=True,
ncol=2)
# draw "headings" aka the velocity vectors
ax = plt.axes()
for i, heading in enumerate(headings):
ax.arrow(x_robot_gt[i], y_robot_gt[i], heading[0], heading[1])
# draw goals as green x's
plt.scatter(x_goal, y_goal, c='g', marker='x')
# draw messages being sent between robots
self.draw_messages(ax, motions, short_range_measurements, 'r')
self.draw_messages(ax, motions, long_range_measurements, 'y')
self.logger.debug("Writing frame to file!")
for i in range(self.fps): # Actually just output one frame per second
self.writer.grab_frame()
if not self.headless:
plt.pause(0.01)
plt.show()
def _plot_airmass(self, figure, site, tgt_data, tz):
"""
Plot into `figure` an airmass chart using target data from `info`
with time plotted in timezone `tz` (a tzinfo instance).
"""
# Urk! This seems to be necessary even though we are plotting
# python datetime objects with timezone attached and setting
# date formatters with the timezone
tz_str = tz.tzname(None)
mpl.rcParams['timezone'] = tz_str
# set major ticks to hours
majorTick = mpl_dt.HourLocator(tz=tz)
majorFmt = mpl_dt.DateFormatter('%Hh')
# set minor ticks to 15 min intervals
minorTick = mpl_dt.MinuteLocator(range(0,59,15), tz=tz)
figure.clf()
ax1 = figure.add_subplot(111)
#lstyle = 'o'
lstyle = '-'
lt_data = map(lambda info: info.ut.astimezone(tz),
tgt_data[0].history)
# sanity check on dates in preferred timezone
## for dt in lt_data[:10]:
## print(dt.strftime("%Y-%m-%d %H:%M:%S"))
# plot targets airmass vs. time
for i, info in enumerate(tgt_data):
am_data = numpy.array(map(lambda info: info.airmass, info.history))
am_min = numpy.argmin(am_data)
am_data_dots = am_data
color = self.colors[i % len(self.colors)]
lc = color + lstyle
# ax1.plot_date(lt_data, am_data, lc, linewidth=1.0, alpha=0.3, aa=True, tz=tz)
ax1.plot_date(lt_data, am_data_dots, lc, linewidth=2.0,
aa=True, tz=tz)
#xs, ys = mpl.mlab.poly_between(lt_data, 2.02, am_data)
#ax1.fill(xs, ys, facecolor=self.colors[i], alpha=0.2)
# plot object label
targname = info.target.name
ax1.text(mpl_dt.date2num(lt_data[am_data.argmin()]),
am_data.min() + 0.08, targname.upper(), color=color,
ha='center', va='center')
ax1.set_ylim(2.02, 0.98)
ax1.set_xlim(lt_data[0], lt_data[-1])
ax1.xaxis.set_major_locator(majorTick)
ax1.xaxis.set_minor_locator(minorTick)
ax1.xaxis.set_major_formatter(majorFmt)
labels = ax1.get_xticklabels()
ax1.grid(True, color='#999999')
# plot current hour
lo = datetime.now(tz)
#lo = datetime.now(tz=tz)
hi = lo + timedelta(0, 3600.0)
if lt_data[0] < lo < lt_data[-1]:
ax1.axvspan(lo, hi, facecolor='#7FFFD4', alpha=0.25)
# label axes
localdate = lt_data[0].astimezone(tz).strftime("%Y-%m-%d")
title = 'Airmass for the night of %s' % (localdate)
ax1.set_title(title)
ax1.set_xlabel(tz.tzname(None))
ax1.set_ylabel('Airmass')
# Plot moon altitude and degree scale
ax2 = ax1.twinx()
moon_data = numpy.array(map(lambda info: numpy.degrees(info.moon_alt),
tgt_data[0].history))
#moon_illum = site.moon_phase()
ax2.plot_date(lt_data, moon_data, '#666666', linewidth=2.0,
alpha=0.5, aa=True, tz=tz)
mxs, mys = mpl.mlab.poly_between(lt_data, 0, moon_data)
# ax2.fill(mxs, mys, facecolor='#666666', alpha=moon_illum)
ax2.set_ylabel('Moon Altitude (deg)', color='#666666')
ax2.set_ylim(0, 90)
ax2.set_xlim(lt_data[0], lt_data[-1])
ax2.xaxis.set_major_locator(majorTick)
ax2.xaxis.set_minor_locator(minorTick)
ax2.xaxis.set_major_formatter(majorFmt)
ax2.set_xlabel('')
ax2.yaxis.tick_right()
canvas = self.fig.canvas
if canvas is not None:
canvas.draw()
