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)))