def uncertain_movements(particles, movements, pose):
#Dado uma lista de movimentos para o robo, aplica esse movimento
#num mapa frame-a-frame calculando tambem um desvio padrao probabilistico da
#trajetoria e termina criando um gif representando o trajeto
#Adicionalmente, tambem chamamos a funcao de resample para as particulas
plt.ioff() # Desliga o modo interativo, para nao aparecerem muitas imagens no meio
frames = 1
color_image = cv2.imread("sparse_obstacles.png")
np_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
for delta in movements:
for i in range(len(pose)):
uncertainty= apply_uncertainty(delta[i])
pose[i]+= uncertainty
for particle in particles:
uncertainty= apply_uncertainty(delta[i])
particle[i]=uncertainty+particle[i]
if i!=2:
if particle[i]<0:
particle[i]=0
if particle[i]>800:
particle[i]=800
#pondera as particulas levando em conta o movimento que realizamos
position_speculation(particles, pose)
if greatest_probability(particles)>0.9:
resample(particles)
# Simula a leitura do lidar
leituras, lidar_map = inspercles.nb_simulate_lidar(pose, angles, np_image)
# Desenha as particulas
ax = inspercles.nb_draw_map(color_image, pose=pose, robot=True, particles= particles)
#ax.imshow(occupancy_image, alph.2)
# Desenha o mapa do lidar
ax.imshow(lidar_map, alpha=0.5)
plt.savefig("syteste%04d.png"%frames, bounds="tight")
frames+=1
plt.close('all')
plt.ion()
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
# In[7]:
inspercles.nb_draw_map(color_image,
particles=particulas,
initial_position=initial_pose,
pose=pose,
def draw_laser():
ax = inspercles.nb_draw_map(lidar_map, robot=True, pose=pose)
ax.imshow(color_image, alpha=0.8)
def draw_map_particles(particles, robot):
inspercles.nb_draw_map(color_image,
particles=particles,
initial_position=robot.pose(),
pose=robot.pose(),
robot=True)
def draw_map(robot):
inspercles.nb_draw_map(color_image, pose=robot.pose(), robot=True)
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
# In[7]:
inspercles.nb_draw_map(color_image, particles = particulas, initial_position = initial_pose, pose=pose, robot=True)
# In[6]: leituras # In[7]: # Você não deve se preocupar com o código abaixo - é só para gerar uma imagem que será mostrada mais adiante #leituras, inspercles.lidar_map = inspercles.nb_simulate_lidar_fast(projeto_pf.robot.pose(), projeto_pf.angles, inspercles.np_image) leituras, lidar_map_temp = inspercles.nb_simulate_lidar_desenha(projeto_pf.robot, projeto_pf.angles) ax = inspercles.nb_draw_map(lidar_map_temp, robot=True, pose=projeto_pf.robot.pose()) #ax.imshow(inspercles.color_image, alpha=0.8) # ## Loop principal dos movimentos do robô # Há uma lista de deslocamentos para o robô chamada `movimentos` no arquivo `projeto_pf`. Vamos usá-la para simular o movimento no filtro de partículas # # # # Atenção: quando for gerar os arquivos png da entrega por favor apague todos os arquivos do tipo anim*png antes de gerar a versão final # In[8]:
def draw_laser():
ax = inspercles.nb_draw_map(lidar_map, robot=True, pose=pose)
ax.imshow(color_image, alpha=0.8)
def draw_map_particles(particles, robot):
inspercles.nb_draw_map(color_image, particles = particles, initial_position = robot.pose(), pose=robot.pose(), robot=True)
def draw_map(robot):
inspercles.nb_draw_map(color_image, pose=robot.pose(), robot=True)
