# FIND COM
com_pos = np.mean(particle_pos, axis=1)
com_h = np.mean(particle_h)
com_uncertainty = np.std(particle_pos, axis=1)
is_converged = np.all(2 * com_uncertainty < 0.4)
# ---- DRAW OBJECTS
# ------------------------------------------------------------
window.draw()
for i in range(N_PARTICLES):
Particle.draw(window, particle_pos[:, i], particle_h[i])
# Draw the center of mass position and heading
Particle.draw_com(window, com_pos, com_h, com_uncertainty)
# Set the variables on the robot
robot.localized = is_converged
robot.state = state.value
robot.pos = np.reshape(com_pos, (2, 1))
robot.h = com_h
robot.sensor_readings = readings[:]
robot.draw(window)
# Go ahead and update the screen with what we've drawn.
# This MUST happen after all the other drawing commands.
pygame.display.flip()
# ------------------------------------------------------------
[1, 1]), False
while not should_quit:
t += 1
# Limit fps.
update_clock.tick(10)
td = update_clock.get_time()
print("Update took {} ms".format(update_clock.get_time()))
# ---- DRAW OBJECTS
# ------------------------------------------------------------
window.draw()
for i in range(N_PARTICLES):
Particle.draw(window, pf.particle_pos[:, i], pf.particle_h[i])
# Draw the center of mass position and heading
Particle.draw_com(window, com_pos, com_h, com_uncertainty, is_converged)
robot.draw(window)
# Go ahead and update the screen with what we've drawn.
# This MUST happen after all the other drawing commands.
pygame.display.flip()
# ------------------------------------------------------------
# Process events.
mouse_clicked = False
events = pygame.event.get()
for event in events:
if event.type == pygame.QUIT:
should_quit = True
if event.type == pygame.MOUSEBUTTONUP: