def _get_pod_artist(pod: PodState, color: Tuple[float, float, float]) -> Wedge:
# Draw the wedge
theta1, theta2, center = _pod_wedge_info(pod)
wedge = Wedge((center.x, center.y),
Constants.pod_radius(),
theta1,
theta2,
color=color)
wedge.set_zorder(10)
return wedge
def update_scan_field(self):
""" Updates the location of the `scan_field` semi-circle displayed to
indicate its location and the direction in which it is facing. If there
was a no previous scan field, then will be added. Should be called
whenever either location or needs to be
called as part of the rover's move and rotate functions.) """
# Remove the old scan field, if there is one:
w = self.scan_field
if w != None:
self.view.artists.remove(w)
# Make a semi-circle with radius 100 at the current rover location:
w = Wedge(self._loc, 100, self._direction - 90, self._direction + 90)
w.set_facecolor('0.90') # grey
w.set_edgecolor('none')
w.set_zorder(zorders['scan_field'])
w.set_fill(True)
self.view.add_artist(w)
self.scan_field = w
def loop_matplotlib_blitting(nav,
bike,
map_model,
blitting=True,
filename=None):
"""This code uses blitting and callbacks to simulate the
bike. Because so much of the code is shared, this function, when
provided with the filename argument, will save video to the
specified filename instead of displaying the animation in a
window."""
figure, axes = plt.figure(), plt.axes(
**find_display_bounds(map_model.waypoints))
# Square aspect ratio for the axes
axes.set_aspect("equal")
paths = new_map_model.paths
# Draw the paths
lc = mc.LineCollection(paths, linewidths=2, color="blue")
axes.add_collection(lc)
# Paths won't change, so capture them
figure.canvas.draw()
background = [figure.canvas.copy_from_bbox(axes.bbox)]
# Create bike polygon
bike_heading = bike.psi * (180 / math.pi) # heading is psi, but in degrees
wedge_angle = 45 # The angle covered by the wedge (degrees)
theta1 = bike_heading - wedge_angle / 2 + 180
theta2 = bike_heading + wedge_angle / 2 + 180
bike_polygon = Wedge((bike.xB, bike.yB), 1, theta1, theta2, fc="black")
bike_polygon.set_zorder(10)
axes.add_artist(bike_polygon)
# Create bike trajectory
bike_trajectory_polygon = axes.plot([0, 0], [0, 0], "g")[0]
# Set up trajectory data
bike_traj_x = [bike.xB] # Just the x-coords
bike_traj_y = [bike.yB] # Just the y-coords
add_traj_x = bike_traj_x.append
add_traj_y = bike_traj_y.append
# Create lookahead point
#lookahead_polygon = Circle((bike.xB, bike.yB), 1)
#axes.add_artist(lookahead_polygon)
# Create dropped point
#dropped_polygon = Circle((bike.xB, bike.yB), 1, fc="red")
#axes.add_artist(dropped_polygon)
# Create current line highlight
#current_line = axes.plot([0, 0], [0, 0], "r")[0]
#axes.add_artist(current_line)
# Set up resizing handlers
listener_id = [None]
def safe_draw():
canvas = figure.canvas
if listener_id[0]: canvas.mpl_disconnect(listener_id[0])
canvas.draw()
listener_id[0] = canvas.mpl_connect("draw_event", grab_background)
def grab_background(event=None):
#transient_polygons = (bike_polygon, lookahead_polygon, current_line, dropped_polygon)
transient_polygons = (bike_polygon, )
for polygon in transient_polygons:
polygon.set_visible(False)
safe_draw()
background[0] = figure.canvas.copy_from_bbox(figure.bbox)
for polygon in transient_polygons:
polygon.set_visible(True)
blit()
def blit():
figure.canvas.restore_region(background[0])
axes.draw_artist(bike_polygon)
figure.canvas.blit(axes.bbox)
listener_id[0] = figure.canvas.mpl_connect("draw_event", grab_background)
# This timer runs simulation steps and draws the results
figure_restore = figure.canvas.restore_region
get_steering_angle = nav.get_steering_angle
simulation_step = lambda angle: bike.update(bikeSim.new_state(bike, angle))
figure_blit = figure.canvas.blit
def full_step(data=None):
figure_restore(background[0])
simulation_step(get_steering_angle())
# Update bike polygon properties and redraw it
wedge_dir = bike.psi * (180 / math.pi) + 180
bike_pos = (bike.xB, bike.yB)
bike_polygon.set(center=bike_pos,
theta1=wedge_dir - wedge_angle / 2,
theta2=wedge_dir + wedge_angle / 2)
axes.draw_artist(bike_polygon)
# Update trajectory and redraw it
add_traj_x(bike.xB)
add_traj_y(bike.yB)
bike_trajectory_polygon.set_xdata(bike_traj_x)
bike_trajectory_polygon.set_ydata(bike_traj_y)
axes.draw_artist(bike_trajectory_polygon)
# Update and redraw lookahead point
#lookahead_polygon.center = nav.lookahead_point
#axes.draw_artist(lookahead_polygon)
# Update and redraw dropped point
#dropped_polygon.center = nav.dropped_point
#axes.draw_artist(dropped_polygon)
# Update and redraw highlight for current closest line
#curr_path_segment = paths[nav.closest_path_index]
#current_line.set_xdata([curr_path_segment[0][0], curr_path_segment[1][0]])
#current_line.set_ydata([curr_path_segment[0][1], curr_path_segment[1][1]])
#axes.draw_artist(current_line)
# Redraw bike
figure_blit(axes.bbox)
# Start the update & refresh timer
if blitting and not filename:
figure.canvas.new_timer(interval=ANIM_INTERVAL,
callbacks=[(full_step, [], {})]).start()
else:
ani = animation.FuncAnimation(figure,
full_step,
frames=xrange(0, 20000))
if filename:
writer = animation.writers['ffmpeg'](fps=30)
ani.save(filename, writer=writer, dpi=100)
# Display the window with the simulation
plt.show()
def loop_matplotlib_blitting(tank, blitting=True):
"""This code uses blitting and callbacks to simulate the
bike. Because so much of the code is shared, this function, when
provided with the filename argument, will save video to the
specified filename instead of displaying the animation in a
window."""
figure, axes = plt.figure(), plt.axes(
**find_display_bounds(tank.waypoints))
# Square aspect ratio for the axes
axes.set_aspect("equal")
paths = tank.paths
# Draw the paths
lc = mc.LineCollection(paths, linewidths=2, color="blue")
axes.add_collection(lc)
# Paths won't change, so capture them
figure.canvas.draw()
background = [figure.canvas.copy_from_bbox(axes.bbox)]
# Create bike polygon
bike_heading = tank.heading * (180 / math.pi
) # heading is psi, but in degrees
wedge_angle = 45 # The angle covered by the wedge (degrees)
theta1 = bike_heading - wedge_angle / 2 + 180
theta2 = bike_heading + wedge_angle / 2 + 180
bike_polygon = Wedge((tank.x, tank.y), 1, theta1, theta2, fc="black")
bike_polygon.set_zorder(10)
axes.add_artist(bike_polygon)
# Create bike trajectory
bike_trajectory_polygon = axes.plot([0, 0], [0, 0], "g")[0]
# Set up trajectory data
bike_traj_x = [tank.x] # Just the x-coords
bike_traj_y = [tank.y] # Just the y-coords
add_traj_x = bike_traj_x.append
add_traj_y = bike_traj_y.append
# Set up resizing handlers
listener_id = [None]
def safe_draw():
canvas = figure.canvas
if listener_id[0]: canvas.mpl_disconnect(listener_id[0])
canvas.draw()
listener_id[0] = canvas.mpl_connect("draw_event", grab_background)
def grab_background(event=None):
#transient_polygons = (bike_polygon, lookahead_polygon, current_line, dropped_polygon)
transient_polygons = (bike_polygon, )
for polygon in transient_polygons:
polygon.set_visible(False)
safe_draw()
background[0] = figure.canvas.copy_from_bbox(figure.bbox)
for polygon in transient_polygons:
polygon.set_visible(True)
blit()
def blit():
figure.canvas.restore_region(background[0])
axes.draw_artist(bike_polygon)
figure.canvas.blit(axes.bbox)
listener_id[0] = figure.canvas.mpl_connect("draw_event", grab_background)
# This timer runs simulation steps and draws the results
figure_restore = figure.canvas.restore_region
figure_blit = figure.canvas.blit
def full_step(data=None):
figure_restore(background[0])
tank.step(*tank.get_nav_command())
# Update bike polygon properties and redraw it
wedge_dir = tank.heading * (180 / math.pi) + 180
bike_pos = (tank.x, tank.y)
bike_polygon.set(center=bike_pos,
theta1=wedge_dir - wedge_angle / 2,
theta2=wedge_dir + wedge_angle / 2)
axes.draw_artist(bike_polygon)
# Update trajectory and redraw it
add_traj_x(tank.x)
add_traj_y(tank.y)
bike_trajectory_polygon.set_xdata(bike_traj_x)
bike_trajectory_polygon.set_ydata(bike_traj_y)
axes.draw_artist(bike_trajectory_polygon)
# Redraw bike
figure_blit(axes.bbox)
# Start the update & refresh timer
if blitting:
figure.canvas.new_timer(interval=ANIM_INTERVAL,
callbacks=[(full_step, [], {})]).start()
else:
ani = animation.FuncAnimation(figure,
full_step,
frames=xrange(0, 20000))
# Display the window with the simulation
plt.show()
def loop_matplotlib_blitting(bike, blitting=True):
"""This code uses blitting and callbacks to simulate the
bike. Because so much of the code is shared, this function, when
provided with the filename argument, will save video to the
specified filename instead of displaying the animation in a
window."""
figure, (simulation_plot, polar_plot) = plt.subplots(1, 2, figsize=(10, 5))
# if hasattr(bike, 'waypoints'):
# simulation_plot = plt.axes(**find_display_bounds(bike.waypoints))
# else:
# right_bound, upper_bound = bike.path_planner.get_histogram_grid().get_real_size()
# padding = 10
# simulation_plot = plt.axes(xlim=[-padding, right_bound + padding], ylim=[-padding, upper_bound + padding])
# Square aspect ratio for the axes
simulation_plot.set_aspect("equal")
polar_plot.set_aspect("equal")
# Plot paths
lc = mc.LineCollection(bike.get_paths(), linewidths=2, color="blue")
simulation_plot.add_collection(lc)
# Plot obstacles
for x, y, prob in bike.get_obstacles():
simulation_plot.add_artist(Circle((x, y), radius=0.5,
alpha=prob / 100))
# Paths and obstacles won't change, so capture them
figure.canvas.draw()
background = [figure.canvas.copy_from_bbox(simulation_plot.bbox)]
# Create bike polygon
bike_heading = bike.heading * (180 / math.pi
) # heading is psi, but in degrees
wedge_angle = 45 # The angle covered by the wedge (degrees)
theta1 = bike_heading - wedge_angle / 2 + 180
theta2 = bike_heading + wedge_angle / 2 + 180
bike_polygon = Wedge(bike.pos, 1, theta1, theta2, fc="black")
bike_polygon.set_zorder(10)
simulation_plot.add_artist(bike_polygon)
# Create pure pursuit lookahead point
pp_lookahead_polygon = Circle((0, 0), radius=0.5, color="r")
pp_lookahead_polygon.set_zorder(9)
simulation_plot.add_artist(pp_lookahead_polygon)
# Create lookahead point from low-level direction tracking
dir_lookahead_polygon = Circle((0, 0), radius=0.5, color="orange")
dir_lookahead_polygon.set_zorder(9)
simulation_plot.add_artist(dir_lookahead_polygon)
# Create bike trajectory
bike_trajectory_polygon = simulation_plot.plot([0, 0], [0, 0], "g")[0]
# Set up trajectory data
bike_traj_x = [bike.pos[0]] # Just the x-coords
bike_traj_y = [bike.pos[1]] # Just the y-coords
add_traj_x = bike_traj_x.append
add_traj_y = bike_traj_y.append
# Set up resizing handlers
listener_id = [None]
def safe_draw():
canvas = figure.canvas
if listener_id[0]:
canvas.mpl_disconnect(listener_id[0])
canvas.draw()
listener_id[0] = canvas.mpl_connect("draw_event", grab_background)
def grab_background(event=None):
transient_polygons = (bike_polygon, pp_lookahead_polygon,
dir_lookahead_polygon)
for polygon in transient_polygons:
polygon.set_visible(False)
safe_draw()
background[0] = figure.canvas.copy_from_bbox(figure.bbox)
for polygon in transient_polygons:
polygon.set_visible(True)
blit()
def blit():
figure.canvas.restore_region(background[0])
simulation_plot.draw_artist(bike_polygon)
simulation_plot.draw_artist(pp_lookahead_polygon)
simulation_plot.draw_artist(dir_lookahead_polygon)
figure.canvas.blit(simulation_plot.bbox)
listener_id[0] = figure.canvas.mpl_connect("draw_event", grab_background)
# This timer runs simulation steps and draws the results
figure_restore = figure.canvas.restore_region
figure_blit = figure.canvas.blit
def full_step(data=None):
figure_restore(background[0])
bike.step(*bike.get_nav_command())
# Plot polar histogram
# plot_polar_histogram(polar_plot)
# Update bike polygon properties and redraw it
wedge_dir = bike.heading * (180 / math.pi) + 180
bike_polygon.set(center=bike.pos,
theta1=wedge_dir - wedge_angle / 2,
theta2=wedge_dir + wedge_angle / 2)
simulation_plot.draw_artist(bike_polygon)
# Update PP lookahead polygon properties and redraw it
pp_lookahead_polygon.set(center=bike.get_nav_lookahead())
simulation_plot.draw_artist(pp_lookahead_polygon)
# Update PP lookahead polygon properties and redraw it
dir_lookahead_polygon.set(center=bike.get_dir_lookahead())
simulation_plot.draw_artist(dir_lookahead_polygon)
# Update trajectory and redraw it
add_traj_x(bike.pos[0])
add_traj_y(bike.pos[1])
bike_trajectory_polygon.set_xdata(bike_traj_x)
bike_trajectory_polygon.set_ydata(bike_traj_y)
simulation_plot.draw_artist(bike_trajectory_polygon)
# Redraw bike
figure_blit(simulation_plot.bbox)
# Start the update & refresh timer
if blitting:
timer = figure.canvas.new_timer(interval=ANIM_INTERVAL,
callbacks=[(full_step, [], {})
]).start()
else:
ani = animation.FuncAnimation(figure,
full_step,
frames=range(0, 20000))
# Display the window with the simulation
plt.show()
