Skip to content

Instantly share code, notes, and snippets.

@imsrgadich
Forked from tags07/berthy.py
Created November 28, 2016 15:06
Show Gist options
  • Save imsrgadich/ec1b59126afd66fa18dcced00074119d to your computer and use it in GitHub Desktop.
Save imsrgadich/ec1b59126afd66fa18dcced00074119d to your computer and use it in GitHub Desktop.
particle filter visualization
# http://www.udacity-forums.com/cs373/questions/15720/display-hw3-6-particle-filter-in-a-gui?page=1&focusedAnswerId=16593#16593
# Creator: http://www.udacity-forums.com/cs373/users/1217/berthy424
# Additions by http://www.udacity-forums.com/cs373/users/149/marcello79
# Ruined by http://www.udacity-forums.com/cs373/users/485/alan
# http://www.udacity-forums.com/cs373/questions/15720/display-hw3-6-particle-filter-in-a-gui/17348
import Tkinter as tk
import particle_filter_hw3_6 as pf
import math
import random
import itertools
class DispParticleFilter(tk.Tk):
def __init__(self, N = 100,
delay = 250, displayRealRobot = True, displayGhost = False):
tk.Tk.__init__(self)
self.title( 'Display Particle Filter CS373-HW03.06')
self.N = N
self.delay = delay
self.displayRealRobot = displayRealRobot
self.displayGhost = displayGhost
self.particle_stream = particle_stream()
self.path = []
self.path_index = 0
self.init_filter()
# Drawing
self.grid()
self.margin = 50
self.zoom_factor = 2.0
self.playing = False
self.canvas = DisplayParticles(self.margin, self.zoom_factor)
self.canvas.configure(bg = 'ivory', bd = 2, relief = tk.SUNKEN)
self.canvas.grid(row = 0, padx = 5, pady = 5)
self.canvas.draw_all(self.particles, self.robot, self.path,
self.displayRealRobot, self.displayGhost)
self.mousex = self.mousey = 0
self.canvas.bind('<Motion>', self.on_motion)
self.canvas.bind('<B1-Motion>', self.on_drag)
self.canvas.bind('<ButtonRelease-1>', self.play_or_pause)
self.canvas.bind('<Button-3>', self.reset_filter)
# Parameter frame
self.parameterFrame = tk.LabelFrame(self, text = 'Parameters:')
self.parameterFrame.grid(row = 2, padx = 5, pady = 5)
self.nslider = tk.Scale(self.parameterFrame,
from_ = 1, to = 1000, orient = tk.HORIZONTAL,
command = self.set_n,
label = 'num particles:')
self.nslider.set(500)
self.nslider.grid(row = 0, column = 0, padx = 5, pady = 5)
self.delay_slider = tk.Scale(
self.parameterFrame, from_ = 1, to = 1000, orient = tk.HORIZONTAL,
command = self.set_delay,
label = 'frame delay:')
self.delay_slider.set(500)
self.delay_slider.grid(row = 0, column = 1, padx = 5, pady = 5)
self.bearing_slider = tk.Scale(
self.parameterFrame, from_ = 0.01, to = 1.0, orient = tk.HORIZONTAL,
command = self.set_bearing_noise,
resolution = 0.01,
label = 'bearing noise:')
self.bearing_slider.set(0.1)
self.bearing_slider.grid(row = 1, column = 0, padx = 5, pady = 5)
self.steering_slider = tk.Scale(
self.parameterFrame, from_ = 0, to = 1.0, orient = tk.HORIZONTAL,
command = self.set_steering_noise,
resolution = 0.01,
label = 'steering noise:')
self.steering_slider.set(0.1)
self.steering_slider.grid(row = 1, column = 1, padx = 5, pady = 5)
self.distance_slider = tk.Scale(
self.parameterFrame, from_ = 0, to = 10.0, orient = tk.HORIZONTAL,
command = self.set_distance_noise,
label = 'distance noise:')
self.distance_slider.set(5.0)
self.distance_slider.grid(row = 1, column = 2, padx = 5, pady = 5)
# Error frame
self.errorFrame = tk.LabelFrame(self, text = 'Error:')
self.errorFrame.grid(row = 1, padx = 5, pady = 5)
self.posErrorStr = tk.StringVar()
self.posErrorStr.set('Pos: N/A')
self.posError = tk.Label(self.errorFrame, textvariable = self.posErrorStr)
self.posError.grid(row = 0, column = 0, padx = 5, pady = 5)
self.angleErrorStr = tk.StringVar()
self.angleErrorStr.set('Angle: N/A')
self.angleError = tk.Label(self.errorFrame, textvariable = self.angleErrorStr)
self.angleError.grid(row = 0, column = 1, padx = 5, pady = 5)
def on_drag(self, event):
self.path.append((self.to_world(event.x), self.to_world(event.y)))
self.canvas.plot_landmarks(self.path[-1:], self.canvas.path_radius,
self.canvas.path_color)
def set_n(self, event):
self.N = int(self.nslider.get())
def set_delay(self, event):
self.delay = int(self.delay_slider.get())
def set_bearing_noise(self, event):
pf.bearing_noise = float(self.bearing_slider.get())
self.reset_noise()
def set_steering_noise(self, event):
pf.steering_noise = float(self.steering_slider.get())
self.reset_noise()
def set_distance_noise(self, event):
pf.distance_noise = float(self.distance_slider.get())
self.reset_noise()
def reset_noise(self):
self.robot.set_noise(pf.bearing_noise, pf.steering_noise, pf.distance_noise)
for particle in self.particles:
particle.set_noise(
pf.bearing_noise, pf.steering_noise, pf.distance_noise)
def reset_filter(self, event = None):
self.path = []
self.path_index = 0
self.init_filter()
self.canvas.draw_all(self.particles, self.robot, self.path,
self.displayRealRobot, self.displayGhost)
def init_filter(self):
# New robot's position
self.robot = pf.robot()
self.robot.set_noise(pf.bearing_noise, pf.steering_noise, pf.distance_noise)
# Make particles
self.particles = list(itertools.islice(self.particle_stream, self.N))
def to_world(self, x):
return float(x)/self.zoom_factor - self.margin
def on_motion(self, event):
self.mousex = self.to_world(event.x)
self.mousey = self.to_world(event.y)
def track_mouse(self):
if self.path:
for i in range(len(self.path)):
x, y = self.path[self.path_index]
dx = x - self.robot.x
dy = y - self.robot.y
dist = math.sqrt(dx**2 + dy**2)
if dist > 25:
break
else:
self.path_index = (self.path_index+1)%len(self.path)
else:
dx = self.mousex - self.robot.x
dy = self.mousey - self.robot.y
alpha = ((math.atan2(dy, dx) - self.robot.orientation + math.pi) % (2*math.pi)
- math.pi)
if not -pf.max_steering_angle <= alpha <= pf.max_steering_angle:
alpha = math.copysign(pf.max_steering_angle, alpha)
dist = min(math.sqrt(dx**2 + dy**2), 15)
motion = (alpha, dist)
return motion
def nparticles(self, n):
'''
Return a list of n particles
If self.nslider has changed the value of self.N, here is our chance to
update the number of particles in our simulation.
'''
if len(self.particles)<n:
# Add new random particles
return list(itertools.islice(itertools.chain(
self.particles, self.particle_stream), n))
else:
# Truncate self.particles
return self.particles[:n]
def display_error(self):
'''
Set the error Labels
'''
ghost = pf.get_position(self.particles)
error_x, error_y, error_orientation = compute_error(self.robot, ghost)
if error_x < pf.tolerance_xy and error_y < pf.tolerance_xy:
fg = 'blue'
else:
fg = 'red'
error_xy = math.sqrt(error_x**2+error_y**2)
self.posErrorStr.set('Pos: {e:.2f}'.format(e = error_xy))
self.posError.configure(fg = fg)
self.angleErrorStr.set('Angle: {e:.2f}'.format(e = error_orientation))
fg = ('blue' if error_orientation < pf.tolerance_orientation else 'red')
self.angleError.configure(fg = fg)
def next_step(self, event = None):
if self.playing:
motion = self.track_mouse()
# motion update (prediction)
self.robot = self.robot.move(motion)
self.particles = [particle.move(motion) for particle in self.particles]
# measurement update
Z = self.robot.sense()
w = [particle.measurement_prob(Z) for particle in self.particles]
# resampling
self.particles = wheelchoice(self.particles, w)
# resize
self.particles = self.nparticles(self.N)
# replot all
self.display_error()
self.canvas.draw_all(
self.particles, self.robot, self.path,
self.displayRealRobot, self.displayGhost)
self.update_idletasks()
self.after(self.delay, self.next_step)
def play_or_pause(self, event = None):
if self.playing ^ True:
self.playing = True
self.after(self.delay, self.next_step)
else:
self.playing = False
class DisplayParticles(tk.Canvas):
def __init__(self, margin, zoom_factor):
tk.Canvas.__init__(self)
self.margin = margin
self.zoom_factor = zoom_factor
self.larg = self.haut = (2*margin + pf.world_size) * zoom_factor
self.configure(width = self.larg, height = self.haut)
# Landmarks
self.landmarks_radius = 2
self.landmarks_color = 'green'
# Particles
self.particle_radius = 1
self.particle_color = 'red'
# Robot
self.robot_radius = 4
self.robot_color = 'blue'
self.ghost_color = None
# Path
self.path_radius = 1
self.path_color = 'orange'
def draw_all(self, p, realRob, path, displayRealRobot, displayGhost):
self.configure(bg = 'ivory', bd = 2, relief = tk.SUNKEN)
self.delete(tk.ALL)
self.particles = p
self.plot_particles()
if displayGhost:
ghost = pf.get_position(self.particles)
self.plot_robot(ghost[0], ghost[1], ghost[2],
self.robot_radius, self.ghost_color)
self.plot_landmarks(reverse_xy(pf.landmarks),
self.landmarks_radius, self.landmarks_color)
if displayRealRobot:
self.plot_robot(realRob.x, realRob.y, realRob.orientation,
self.robot_radius, self.robot_color)
if path:
self.plot_landmarks(path, self.path_radius, self.path_color)
def to_canvas(self, x):
return (self.margin + x) * self.zoom_factor
def plot_landmarks(self, lms, radius, l_color):
for lx, ly in lms:
x0 = self.to_canvas(lx - radius)
y0 = self.to_canvas(ly - radius)
x1 = self.to_canvas(lx + radius)
y1 = self.to_canvas(ly + radius)
self.create_oval(x0, y0, x1, y1, fill = l_color)
def plot_particles(self):
for particle in self.particles:
self.draw_particle(particle, self.particle_radius, self.particle_color)
def draw_particle(self, particle, radius, p_color, cos = math.cos, sin = math.sin):
x2 = self.to_canvas(particle.x)
y2 = self.to_canvas(particle.y)
x3 = self.to_canvas(particle.x + 2*radius*cos(particle.orientation))
y3 = self.to_canvas(particle.y + 2*radius*sin(particle.orientation))
self.create_line(x2, y2, x3, y3, fill = p_color, width = self.zoom_factor,
arrow = tk.LAST, arrowshape = (2*self.zoom_factor,
3*self.zoom_factor,
1*self.zoom_factor))
def plot_robot(self, x, y, orientation, radius, r_color,
cos = math.cos, sin = math.sin):
x0 = self.to_canvas(x - radius)
y0 = self.to_canvas(y - radius)
x1 = self.to_canvas(x + radius)
y1 = self.to_canvas(y + radius)
self.create_oval(x0, y0, x1, y1, fill = r_color)
x2 = self.to_canvas(x)
y2 = self.to_canvas(y)
x3 = self.to_canvas(x + 2*radius*cos(orientation))
y3 = self.to_canvas(y + 2*radius*sin(orientation))
self.create_line(x2, y2, x3, y3,
fill = r_color, width = self.zoom_factor, arrow = tk.LAST)
def compute_error(final_robot, estimated_position):
error_x = abs(final_robot.x - estimated_position[0])
error_y = abs(final_robot.y - estimated_position[1])
error_orientation = abs(final_robot.orientation - estimated_position[2])
error_orientation = (error_orientation + math.pi) % (2.0 * math.pi) - math.pi
return error_x, error_y, error_orientation
def particle_stream():
while True:
r = pf.robot()
r.set_noise(pf.bearing_noise, pf.steering_noise, pf.distance_noise)
yield r
def wheelchoice(p, weights, n = None):
assert len(p) == len(weights)
if n is None:
n = len(p)
result = []
index = random.randrange(n)
beta = 0.0
mw = max(weights)
for i in range(n):
beta += random.uniform(0, 2.0*mw)
while beta > weights[index]:
beta -= weights[index]
index = (index + 1) % n
result.append(p[index])
return result
def reverse_xy(path):
for x, y in path:
yield y, x
if __name__ == "__main__":
wind = DispParticleFilter(N = 500, displayRealRobot = True, displayGhost = True)
wind.mainloop()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment