def __init__(self, on_update=None, background_file=None, robot=None):
'''
Args:
background_file(str): Background filename
robot(Particle): Particle representing actual robot
particles(list[Particle]): Simulation particles
'''
self.on_update = on_update
# Initialize view
pygame.init()
if background_file is None:
background_file = BACKGROUND_FILE
self.background = pygame.image.load(background_file)
self.size = self.background.get_size()
self.screen = pygame.display.set_mode(self.size)
# Initialize simulation components
self.robot_speed = [0, 0] # [LINEAR_x, LINEAR_y, ANGULAR]
self.robot = robot
if self.robot is None:
self.robot = Particle(self.size[0] / 2, self.size[1] / 2)
projeto_pf.particulas = projeto_pf.cria_particulas()
self.leituras_robot = inspercles.nb_lidar(self.robot,
projeto_pf.angles)
self.ofb = pygame.Surface(self.size)
self.ofb.set_colorkey((0, 0, 0))
def cria_particulas(minx=0,
miny=0,
maxx=largura,
maxy=altura,
n_particulas=num_particulas):
"""
Cria uma lista de partículas distribuídas de forma uniforme entre minx, miny, maxx e maxy
"""
grid = int(math.sqrt(n_particulas))
particle_cloud = []
map_area = altura * largura
part = map_area / n_particulas
part_side_x = largura / grid
part_side_y = altura / grid
part_center = (part_side_x / 2, part_side_y / 2)
particles = []
for i in range(0, grid):
x = part_center[0] + (part_side_x * i)
for k in range(0, grid):
y = part_center[1] + (part_side_y * k)
theta = np.random.uniform(0, 2 * math.pi)
p = Particle(x, y, theta, w=1.0)
particles.append(p)
return particles
def __init__(self, on_update=None, background_file=None, robot=None):
'''
Args:
background_file(str): Background filename
robot(Particle): Particle representing actual robot
particles(list[Particle]): Simulation particles
'''
self.on_update = on_update
# Initialize view
pygame.init()
if background_file is None:
background_file = BACKGROUND_FILE
self.background = pygame.image.load(background_file)
self.size = self.background.get_size()
self.screen = pygame.display.set_mode(self.size)
# Initialize simulation components
self.robot_speed = [0, 0] # [LINEAR_x, LINEAR_y, ANGULAR]
self.robot = robot
if self.robot is None:
self.robot = Particle(self.size[0]/2, self.size[1]/2)
projeto_pf.particulas = projeto_pf.cria_particulas()
self.leituras_robot = inspercles.nb_lidar(self.robot, projeto_pf.angles)
self.ofb = pygame.Surface(self.size)
self.ofb.set_colorkey((0,0,0))
def cria_particulas(minx=0, miny=0, maxx=largura, maxy=altura, n_particulas=num_particulas): for i in range(n_particulas): x = np.random.uniform(minx, maxx) y = np.random.uniform(miny, maxy) theta = np.random.uniform(0, 2 * math.pi) p = Particle(x, y, theta, w=1.0) particulas.append(p) return particulas
def randow_particle(n_part,minx, miny, maxx, maxy):
pos = []
S = []
for i in range(n_part):
x = np.random.randint(minx,maxx)
y = np.random.randint(miny,maxy)
theta = np.random.randint(0, 2*math.pi)
p = Particle(x, y, theta)
S.append(p)
pos.append([x,y,theta])
return S,pos
def cria_particulas(minx=0, miny=0, maxx=largura, maxy=altura, n_particulas=num_particulas): """ Cria uma lista de partículas distribuídas de forma uniforme entre minx, miny, maxx e maxy """ particle_cloud = [] for i in range(n_particulas): x = np.random.uniform(minx,maxx) y = np.random.uniform(miny,maxy) theta = np.random.uniform(0,2*math.pi) p = Particle(x, y, theta) # A prob. w vai ser normalizada depois particle_cloud.append(p) return particle_cloud
def all_zeroes(self):
particles = [Particle(w=0), Particle(w=0), Particle(w=0)]
res = self.test(particles, [Particle(w=0), Particle(w=0), Particle(w=0)])
if res:
return 'All Zeroes Test Passed!'
else:
return 'All Zeros Test Failed!'
def desvio_pd(pos_particulas,movimento):
S_2 = []
for i in pos_particulas:
x = i[0]
y = i[1]
theta = i[2]
x += movimento[0] + np.random.normal(0,1.0)
y += movimento[1] + np.random.normal(0,1.0)
theta += movimento[2] + np.random.normal(0,1.0)
p = Particle(x,y,theta)
S_2.append(p)
return S_2
def basic_reweight(self):
particles = [Particle(w=1.), Particle(w=2.), Particle(w=3.)]
expected_output = [Particle(w=1./6.), Particle(w=2./6.), Particle(w=1./2.)]
res = self.test(particles, expected_output)
if res:
return 'All Zeroes Test Passed!'
else:
return 'All Zeros Test Failed!'
def cria_particulas(minx=0,
miny=0,
maxx=largura,
maxy=altura,
n_particulas=num_particulas):
particle_cloud = []
for i in range(n_particulas):
x = np.random.uniform(minx, maxx)
y = np.random.uniform(miny, maxy)
theta = np.random.uniform(0, math.pi)
prob = Particle(x, y, theta,
w=1.0) # A prob. w vai ser normalizada depois
particle_cloud.append(prob)
return particle_cloud
def nb_create_particles(pose, var_x = 50, var_y = 50, var_theta = math.pi/3, num=30):
"""
Cria num particulas
situadas no intervalo x - var_x a x + var_x, y - var_x at'e y + var_y e theta - var_theta a theta + var_theta
"""
particle_cloud = []
s = pose
for i in range(num):
x = random.uniform(s[0] - var_x, s[0] + var_x)
y = random.uniform(s[1] - var_x, s[1] + var_y)
theta = random.uniform(s[2] - var_theta, s[2] + var_theta)
p = Particle(x, y, theta, w=1.0) # A prob. w vai ser normalizada depois
particle_cloud.append(p)
return particle_cloud
def create_particles(minx, miny, maxx, maxy, n_particulas=10):
"""
Cria num particulas
uniformemente situadas no intervalo x - var_x a x + var_x, y - var_x at'e y + var_y e theta - var_theta a theta + var_theta
retorna uma lista de objetos Particle
"""
particle_cloud = []
for i in range(num):
x = np.random.uniform(minx, maxx)
y = np.random.uniform(miny, maxy)
theta = np.random.uniform(0, math.pi)
prob = Particle(x, y, theta,
w=1.0) # A prob. w vai ser normalizada depois
particle_cloud.append(prob)
return particle_cloud
def create_particles(minx, miny, maxx, maxy, n):
#Cria n particulas
#no quadrado de aresta menor minx, miny e maior maxx, maxy
#com posicao aleatoria dentro quadrado
#e um angulo aleatorio entre 0 e 2*pi
particle_cloud = []
for i in range(n):
x = random.uniform(minx, maxx)
y = random.uniform(miny, maxy)
theta = (random.uniform(0, 2*math.pi)+math.pi)%(2*math.pi)
p = Particle(float(x), float(y), float(theta), 1.0/n)
particle_cloud.append(p)
return particle_cloud
def cria_particulas(minx=0,
miny=0,
maxx=largura,
maxy=altura,
n_particulas=num_particulas):
"""
Cria uma lista de partículas distribuídas de forma uniforme entre minx, miny, maxx e maxy
"""
particulas = []
for i in range(num_particulas):
x = np.random.uniform(minx, maxx)
y = np.random.uniform(miny, maxy)
theta = np.random.uniform(0, 2 * math.pi)
p = Particle(x, y, theta, w=1.0)
particulas.append(p)
return particulas
def gerar_particle_cloud(pose,
minx=10,
maxx=60,
miny=10,
maxy=60,
var_theta=math.pi / 3,
n=40):
particle_cloud = []
s = pose
var_x = maxx - minx
var_y = maxy - miny
for i in range(n):
x = random.uniform(s[0] - var_x, s[0] + var_x)
y = random.uniform(s[1] - var_x, s[1] + var_y)
theta = random.uniform(s[2] - var_theta, s[2] + var_theta)
p = Particle(x, y, theta,
w=1.0) # A prob. w vai ser normalizada depois
particle_cloud.append(p)
return particle_cloud
# -*- coding: utf-8 -*-
"""
Esta classe deve conter todas as suas implementações relevantes para seu filtro de partículas
"""
from pf import Particle, create_particles, draw_random_sample
import numpy as np
import inspercles # necessário para o a função nb_lidar que simula o laser
import math
from scipy.stats import norm
largura = 775 # largura do mapa
altura = 748 # altura do mapa
# Robo
robot = Particle(largura / 2, altura / 2, math.pi / 4, 1.0)
# Nuvem de particulas
particulas = []
num_particulas = 300
# Os angulos em que o robo simulado vai ter sensores
angles = np.linspace(0.0, 2 * math.pi, num=8, endpoint=False)
# Lista mais longa
movimentos_longos = [[-10, -10, 0], [-10, 10, 0], [-10, 0, 0], [-10, 0, 0],
[0, 0, math.pi / 12.0], [0, 0, math.pi / 12.0],
[0, 0, math.pi / 12], [0, 0, -math.pi / 4], [-5, 0, 0],
[-5, 0, 0], [-5, 0, 0], [-10, 0, 0], [-10, 0, 0],
[-10, 0, 0], [-10, 0, 0], [-10, 0, 0], [-15, 0, 0],
inspercles.width = width inspercles.height = height initial_pose = [200, 200, math.pi / 8] # Posicao inicial considerada para o pf inspercles.initial_pose = initial_pose pose = [330, 220, math.radians(90)] # posicao "verdadeira" do robo inspercles.pose = pose robot_radius = 10 # Raio do robo inspercles.robot_radius = robot_radius # Os angulos em que o robo simulado vai ter sensores angles = np.linspace(0.0, 2 * math.pi, num=8) particle_cloud = [] fora = Particle(x="2000", y="2000") # ## Mapa com posição inicial # In[5]: inspercles.nb_draw_map(color_image, pose=pose, robot=True) particle_cloud = inspercles.nb_initialize_particle_cloud() particle_cloud = [fora] + particle_cloud # A função *nb_initialize_particle_cloud()* pertence ao módulo <code>inspercles</code> e já faz uma primeira aleatorização das partículas particulas = particle_cloud
class Window:
"""
Main window of the application
"""
def __init__(self, on_update=None, background_file=None, robot=None):
'''
Args:
background_file(str): Background filename
robot(Particle): Particle representing actual robot
particles(list[Particle]): Simulation particles
'''
self.on_update = on_update
# Initialize view
pygame.init()
if background_file is None:
background_file = BACKGROUND_FILE
self.background = pygame.image.load(background_file)
self.size = self.background.get_size()
self.screen = pygame.display.set_mode(self.size)
# Initialize simulation components
self.robot_speed = [0, 0] # [LINEAR_x, LINEAR_y, ANGULAR]
self.robot = robot
if self.robot is None:
self.robot = Particle(self.size[0] / 2, self.size[1] / 2)
projeto_pf.particulas = projeto_pf.cria_particulas()
self.leituras_robot = inspercles.nb_lidar(self.robot,
projeto_pf.angles)
self.ofb = pygame.Surface(self.size)
self.ofb.set_colorkey((0, 0, 0))
def run(self):
'''
Game loop.
'''
while True:
self.on_events(pygame.event.get())
if nonzero(self.robot_speed):
self.robot.move_relative(self.robot_speed)
self.leituras_robot = inspercles.nb_lidar(
self.robot, projeto_pf.angles)
# move particles
projeto_pf.move_particulas(projeto_pf.particulas,
self.robot_speed)
# draw lasers
# Atualiza probabilidade e posicoes
projeto_pf.leituras_laser_evidencias(projeto_pf.robot,
projeto_pf.particulas)
# Reamostra as particulas
projeto_pf.particulas = projeto_pf.reamostrar(
projeto_pf.particulas)
if self.on_update is not None:
self.on_update(self.robot, projeto_pf.particulas,
self.robot_speed)
self.draw()
def on_events(self, events):
'''
Handle user generated events.
'''
for event in events:
print("event: ", event)
print("speed: ", self.robot_speed)
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
elif event.type == pygame.KEYDOWN:
if event.key in SPEED_DELTAS:
delta = SPEED_DELTAS[event.key]
self.robot_speed[delta[0]] += delta[1]
print(event.key)
elif event.type == pygame.KEYUP:
if event.key in SPEED_DELTAS:
delta = SPEED_DELTAS[event.key]
self.robot_speed[delta[0]] -= delta[1]
def draw(self):
'''
Draw everything (background, particles, and robot).
'''
self.screen.fill(BLACK)
self.screen.blit(self.background, self.background.get_rect())
self.ofb.fill(BLACK)
# Draw maze - debugging reasons
# Descomente para ver um efeito psicodélico
#game_utils.draw_maze(self.ofb)
# Draw particles
for particle in projeto_pf.particulas:
self.draw_particle(particle)
if self.leituras_robot:
game_utils.draw_laser_readings(self.ofb, self.robot,
self.leituras_robot)
self.draw_robot()
self.ofb.convert() # Is this really necessary?
# Draws offscreen buffer to front
self.screen.blit(self.ofb, (0, 0))
pygame.display.flip()
def draw_robot(self):
# Position
r = ROBOT_RADIUS
center = (int(self.robot.x), int(self.robot.y))
# Draw
pygame.draw.circle(self.ofb, ROBOT_COLOR, center, r)
self.draw_particle(self.robot, ROBOT_ARROW_COLOR)
def draw_particle(self, particle, color=ARROW_COLOR):
arrow = pygame.Surface((2 * ARROW_WIDTH, 2 * ARROW_WIDTH),
pygame.SRCALPHA, 32)
arrow = arrow.convert_alpha()
sp = (ARROW_WIDTH, ARROW_WIDTH)
ep = (2 * ARROW_WIDTH, ARROW_WIDTH)
ep1 = (2 * ARROW_WIDTH - ARROW_END_LEN, ARROW_WIDTH + ARROW_HEIGHT)
ep2 = (2 * ARROW_WIDTH - ARROW_END_LEN, ARROW_WIDTH - ARROW_HEIGHT)
pygame.draw.line(arrow, color, sp, ep, ARROW_THICK)
pygame.draw.line(arrow, color, ep, ep1, ARROW_THICK)
pygame.draw.line(arrow, color, ep, ep2, ARROW_THICK)
arrow = pygame.transform.rotate(arrow, -math.degrees(particle.theta))
rect = arrow.get_rect()
self.ofb.blit(arrow, (int(particle.x - rect.width / 2),
int(particle.y - rect.height / 2)))
def random_particle():
x = random() * w
y = random() * h
theta = random() * 2 * math.pi
return Particle(x, y, theta)
class Window:
"""
Main window of the application
"""
def __init__(self, on_update=None, background_file=None, robot=None):
'''
Args:
background_file(str): Background filename
robot(Particle): Particle representing actual robot
particles(list[Particle]): Simulation particles
'''
self.on_update = on_update
# Initialize view
pygame.init()
if background_file is None:
background_file = BACKGROUND_FILE
self.background = pygame.image.load(background_file)
self.size = self.background.get_size()
self.screen = pygame.display.set_mode(self.size)
# Initialize simulation components
self.robot_speed = [0, 0] # [LINEAR_x, LINEAR_y, ANGULAR]
self.robot = robot
if self.robot is None:
self.robot = Particle(self.size[0]/2, self.size[1]/2)
projeto_pf.particulas = projeto_pf.cria_particulas()
self.leituras_robot = inspercles.nb_lidar(self.robot, projeto_pf.angles)
self.ofb = pygame.Surface(self.size)
self.ofb.set_colorkey((0,0,0))
def run(self):
'''
Game loop.
'''
while True:
self.on_events(pygame.event.get())
if nonzero(self.robot_speed):
self.robot.move_relative(self.robot_speed)
self.leituras_robot = inspercles.nb_lidar(self.robot, projeto_pf.angles)
# move particles
projeto_pf.move_particulas(projeto_pf.particulas, self.robot_speed)
# draw lasers
# Atualiza probabilidade e posicoes
projeto_pf.leituras_laser_evidencias(projeto_pf.robot, projeto_pf.particulas)
# Reamostra as particulas
projeto_pf.particulas = projeto_pf.reamostrar(projeto_pf.particulas)
if self.on_update is not None:
self.on_update(self.robot, projeto_pf.particulas, self.robot_speed)
self.draw()
def on_events(self, events):
'''
Handle user generated events.
'''
for event in events:
print("event: ", event)
print("speed: ", self.robot_speed)
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
elif event.type == pygame.KEYDOWN:
if event.key in SPEED_DELTAS:
delta = SPEED_DELTAS[event.key]
self.robot_speed[delta[0]] += delta[1]
print(event.key)
elif event.type == pygame.KEYUP:
if event.key in SPEED_DELTAS:
delta = SPEED_DELTAS[event.key]
self.robot_speed[delta[0]] -= delta[1]
def draw(self):
'''
Draw everything (background, particles, and robot).
'''
self.screen.fill(BLACK)
self.screen.blit(self.background, self.background.get_rect())
self.ofb.fill(BLACK)
# Draw maze - debugging reasons
# Descomente para ver um efeito psicodélico
#game_utils.draw_maze(self.ofb)
# Draw particles
for particle in projeto_pf.particulas:
self.draw_particle(particle)
if self.leituras_robot:
game_utils.draw_laser_readings(self.ofb, self.robot, self.leituras_robot)
self.draw_robot()
self.ofb.convert() # Is this really necessary?
# Draws offscreen buffer to front
self.screen.blit(self.ofb, (0,0))
pygame.display.flip()
def draw_robot(self):
# Position
r = ROBOT_RADIUS
center = (int(self.robot.x), int(self.robot.y))
# Draw
pygame.draw.circle(self.ofb, ROBOT_COLOR, center, r)
self.draw_particle(self.robot, ROBOT_ARROW_COLOR)
def draw_particle(self, particle, color=ARROW_COLOR):
arrow = pygame.Surface((2*ARROW_WIDTH, 2*ARROW_WIDTH), pygame.SRCALPHA, 32)
arrow = arrow.convert_alpha()
sp = (ARROW_WIDTH, ARROW_WIDTH)
ep = (2*ARROW_WIDTH, ARROW_WIDTH)
ep1 = (2*ARROW_WIDTH - ARROW_END_LEN, ARROW_WIDTH + ARROW_HEIGHT)
ep2 = (2*ARROW_WIDTH - ARROW_END_LEN, ARROW_WIDTH - ARROW_HEIGHT)
pygame.draw.line(arrow, color, sp, ep, ARROW_THICK)
pygame.draw.line(arrow, color, ep, ep1, ARROW_THICK)
pygame.draw.line(arrow, color, ep, ep2, ARROW_THICK)
arrow = pygame.transform.rotate(arrow, -math.degrees(particle.theta))
rect = arrow.get_rect()
self.ofb.blit(arrow, (int(particle.x - rect.width / 2), int(particle.y - rect.height / 2)))
