def turn_to_search(): # turn_counter += 1 print 'Turning around. Number of turns:', turn_counter # rotate arlo.go_diff(30, 29, 0, 1) sleep((math.pi * 0.25) / rotationInOneSecond) arlo.stop() for particle in particles: par.move_particle(particle, 0, 0, -(math.pi * 0.25) / rotationInOneSecond) return 1
def move_particles(particles, dist, angle): delta_x = [(np.sin(np.radians(90) - elem.getTheta()) * dist) / np.sin(np.radians(90)) for elem in particles] delta_y = [(np.sin(elem.getTheta()) * dist) / np.sin(np.radians(90)) for elem in particles] delta_theta = np.radians(angle) [ particle.move_particle(elem, p_delta_x, p_delta_y, delta_theta) for elem, p_delta_x, p_delta_y in zip(particles, delta_x, delta_y) ]
LMInSight = False #if driving_dist > 0: # sleep(driving_dist/translationInOneSecond) #arlo.stop() # Move particles # for particle in particles: # dx=delt(paXarticle.getTheta(),actual_driven_dist) # dy=deltaY(particle.getTheta(),actual_driven_dist) # par.move_particle(particle, dx, -dy, lastMeasuredAngle) for particle in particles: dx = np.cos(particle.getTheta()) * actual_driven_dist dy = np.sin(particle.getTheta()) * actual_driven_dist par.move_particle(particle, dx, -dy, 0) elif not LMInSight or (LMInSight and visitedLM[lastSeenLM] ) and not (visitedLM[0] and visitedLM[1]): if turn_counter < 8: turn_counter += 1 # rotate arlo.go_diff(30, 29, 0, 1) sleep((math.pi * 0.25) / rotationInOneSecond) arlo.stop() for particle in particles: par.move_particle(particle, 0, 0, -(math.pi * 0.25) / rotationInOneSecond) elif visitedLM[0] or visitedLM[1]: # Explore
def move_explosion(self): tf = pygame.time.get_ticks() / 1000 for particle in self.particle_group: particle.move_particle(tf)
def updateParticles(particles): for particle in particles: dx = np.cos(particle.getTheta()) * translationInOneSecond dy = np.sin(particle.getTheta()) * translationInOneSecond par.move_particle(particle, dx, dy, -(math.pi * 0.50) / rotationInOneSecond)
# The purpose of the next function is get closer into the middel of the two boxes. So after the robot have droven # half of the distance to the second box, the robot wil drive from its position into the middel of the virtual chart. elif not LMInSight or (LMInSight and visitedLM[lastSeenLM] ) and not (visitedLM[0] and visitedLM[1]): if turn_counter < 8: turn_counter += 1 print 'Turning around. Number of turns:', turn_counter # rotate arlo.go_diff(30, 29, 0, 1) sleep((math.pi * 0.25) / rotationInOneSecond) arlo.stop() for particle in particles: par.move_particle(particle, 0, 0, -(math.pi * 0.25) / rotationInOneSecond) elif visitedLM[0] or visitedLM[1]: # Going around a box. Explore print 'Trying to go around a box in front of me' arlo.go_diff(80, 79, 0, 0) sleep(0.25) arlo.go_diff(30, 29, 1, 0) sleep((math.pi * 0.30) / rotationInOneSecond) driving_dist = 30 t = driving_dist / translationInOneSecond stop_dist = 200 actdrive = driveArlo(dt, t, particles)
def updateParticles(d_dist, d_theta): for particle in particles: dx = np.cos(particle.getTheta()) * d_dist dy = np.sin(particle.getTheta()) * d_dist par.move_particle(particle, dx, -dy, d_theta)
updated_values = con.control_frindo(LANDMARK, former_turn, est_pose, frindo) former_turn += updated_values[2] angular_velocity = updated_values[1] velocity = updated_values[0] num_particles = len(particles) for p in particles: # calculates new orientation if angular_velocity != 0: curr_angle = add_to_angular(p.getTheta(), angular_velocity) if velocity != 0: [x, y] = move_vector(p, velocity) particle.move_particle(p, x, y, curr_angle) if velocity != 0: particle.add_uncertainty(particles, 12, 15) if velocity == 0 and angular_velocity != 0: particle.add_uncertainty(particles, 0, 15) # Fetch next frame colour, distorted = cam.get_colour() # Detect objects objectType, measured_distance, measured_angle, colourProb = cam.get_object( colour) if objectType != 'none': obs_landmark = ret_landmark(colourProb, objectType) print obs_landmark
def update_particles(particles, cam, velocity, angular_velocity, world, WIN_RF1, WIN_World): #print 'update: ' + str(angular_velocity) cv2.waitKey(4) num_particles = len(particles) for p in particles: # calculates new orientation curr_angle = add_to_angular_v2(np.degrees(p.getTheta()), angular_velocity) if velocity > 0.0: [x, y] = move_vector(p, velocity) particle.move_particle(p, x, y, curr_angle) else: particle.move_particle(p, 0.0, 0.0, curr_angle) if velocity != 0.0: particle.add_uncertainty(particles, 12, 5) if velocity == 0.0 and angular_velocity != 0.0: particle.add_uncertainty(particles, 0, 5) # Fetch next frame colour, distorted = cam.get_colour() # Detect objects objectType, measured_distance, measured_angle, colourProb = cam.get_object( colour) if objectType != 'none': obs_landmark = ret_landmark(colourProb, objectType) observed_obj = [ objectType, measured_distance, measured_angle, obs_landmark ] list_of_particles = weight_particles(particles, np.degrees(measured_angle), measured_distance, obs_landmark) particles = resample_particles(list_of_particles[:, [0, 2]]) particle.add_uncertainty(particles, 15, 5) cam.draw_object(colour) else: observed_obj = [None, None, None, None] for p in particles: p.setWeight(1.0 / num_particles) particle.add_uncertainty(particles, 10, 5) est_pose = particle.estimate_pose(particles) draw_world(est_pose, particles, world) #Show frame cv2.imshow(WIN_RF1, colour) #Show world cv2.imshow(WIN_World, world) return { 'est_pos': est_pose, 'obs_obj': observed_obj, 'particles': particles }
def update_particles(particles, cam, velocity, angular_velocity, world, WIN_RF1, WIN_World): raw_input() print 'update: ' + str(angular_velocity) cv2.waitKey(4) num_particles = len(particles) for p in particles: # calculates new orientation curr_angle = add_to_angular_v2(np.degrees(p.getTheta()), angular_velocity) print 'theta_rad: ' + str(p.getTheta()) print 'theta_deg: ' + str(np.degrees(p.getTheta())) print 'cur_ang_deg: ' + str(np.degrees(curr_angle)) if velocity > 0.0: [x, y] = move_vector(p, velocity) particle.move_particle(p, x, y, curr_angle) else: particle.move_particle(p, 0.0, 0.0, curr_angle) print 'cur_ang_rad: ' + str(curr_angle) if velocity != 0.0: particle.add_uncertainty(particles, 12, 15) if velocity == 0.0 and angular_velocity != 0.0: particle.add_uncertainty(particles, 0, 15) # Fetch next frame colour, distorted = cam.get_colour() # Detect objects objectType, measured_distance, measured_angle, colourProb = cam.get_object( colour) if objectType != 'none': obs_landmark = ret_landmark(colourProb, objectType) observed_obj = [objectType, measured_distance, measured_angle, obs_landmark] list_of_particles = weight_particles(particles, np.degrees(measured_angle), measured_distance, obs_landmark) particles = [] for count in range(0, num_particles): rando = np.random.uniform(0.0,1.0) # np.random.normal(0.0, 1.0, 1) # dicto = {'i': 500, # 'n': 2} p = when_in_range(list_of_particles, 0, num_particles, rando) particles.append( particle.Particle(p.getX(), p.getY(), p.getTheta(), 1.0 / num_particles)) print 'list_of_particles: ' + str(list_of_particles) print 'particles: ' + str(particles) particle.add_uncertainty(particles, 5, 2) # new random particles added #for c in range(0, int(math.ceil(num_particles * 0.05))): # p = particle.Particle(500.0 * np.random.ranf() - 100, # 500.0 * np.random.ranf() - 100, # 2.0 * np.pi * np.random.ranf() - np.pi, 0.0) # particles.append(p) # Draw detected pattern cam.draw_object(colour) else: observed_obj = [None, None, None, None] # No observation - reset weights to uniform distribution for p in particles: p.setWeight(1.0 / num_particles) particle.add_uncertainty(particles, 5, 2) # est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose # return [est_pose, observed_obj] est_pose = particle.estimate_pose( particles) # The estimate of the robots current pose print 'Updated pose: ' + str([est_pose.getX(), est_pose.getY()]) draw_world(est_pose, particles, world) # Show frame cv2.imshow(WIN_RF1, colour) # Show world cv2.imshow(WIN_World, world) return {'est_pos': est_pose, 'obs_obj': observed_obj, 'particles': particles}