def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) rad = 1 # meters self.particle_cloud = [] self.particle_cloud.append( Particle(xy_theta[0], xy_theta[1], xy_theta[2])) for i in range(self.n_particles - 1): # initial facing of the particle theta = random.random() * 360 # compute params to generate x,y in a circle other_theta = random.random() * 360 radius = random.random() * rad # x => straight ahead x = radius * math.sin(other_theta) + xy_theta[0] y = radius * math.cos(other_theta) + xy_theta[1] particle = Particle(x, y, theta) self.particle_cloud.append(particle) self.normalize_particles() self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ # levels of noise to introduce variability lin_noise = 1 ang_noise = math.pi / 2.0 # if doesn't exist, use odom if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # make a new particle cloud, and then create a bunch of particles at the initial location with some added noise self.particle_cloud = [] for x in range(self.n_particles): x = xy_theta[0] + (random_sample() * lin_noise - (lin_noise / 2.0)) y = xy_theta[1] + (random_sample() * lin_noise - (lin_noise / 2.0)) theta = xy_theta[2] + (random_sample() * ang_noise - (ang_noise / 2.0)) self.particle_cloud.append(Particle(x, y, theta)) # normalize particles because all weights were originall set to 1 on default self.normalize_particles() self.update_robot_pose()
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return for particle in self.particle_cloud: r1 = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2] d = math.sqrt((delta[0]**2) + (delta[1]**2)) particle.theta += r1 % 360 particle.x += d * math.cos(particle.theta) + normal(0, 0.1) particle.y += d * math.sin(particle.theta) + normal(0, 0.1) particle.theta += (delta[2] - r1 + normal(0, 0.1)) % 360
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ print('update_w_odom') new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta for particle in self.particle_cloud: parc = (math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2]) % 360 particle.x += (math.sqrt((delta[0]**2) + (delta[1]**2)))* math.cos(parc) particle.y += (math.sqrt((delta[0]**2) + (delta[1]**2))) * math.sin(parc) particle.theta += delta[2] else: self.current_odom_xy_theta = new_odom_xy_theta return
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # TODO: modify particles using delta # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) d = math.sqrt(delta[0]**2 + delta[1]**2) #distance between robot pose and particle pose r1 = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2] #intial theta to rotate robot by r2 = delta[2] - r1 #theta between robot orientation after traveling d and final particle orientation for i in self.particle_cloud: #for each particle transform position i.theta += r1 i.x += gauss(d * math.cos(i.theta), self.ODOM_ERROR) #add x noise i.y += gauss(d * math.sin(i.theta), self.ODOM_ERROR) #add y noise i.theta += r2
def resample_particles(self, particles): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ print 'Resampling Particles ...' # make sure the distribution is normalized particles = normalize_particles(particles) choices = particles probabilities = np.array([p.w for p in particles]) most_likely_particle = particles[np.argmax(probabilities)] # If there are multiple most likelies, don't add new particles if (np.size(most_likely_particle) == 1): print 'Adding New Particles ...' num_to_resample = int(len(particles) / 2) num_to_reinit = len(particles) - num_to_resample pose_most_likely = convert_pose_to_xy_and_theta( most_likely_particle.as_pose()) reinitialized_particles = self.initialize_particle_cloud( pose_most_likely, num_to_reinit, most_likely_particle.w * 0.9) else: print 'Number of most likely particles: ' + str( np.size(most_likely_particle)) num_to_resample = len(particles) reinitialized_particles = [] new_particles = draw_random_sample(choices, probabilities, num_to_resample) return normalize_particles(new_particles + reinitialized_particles)
def update_initial_pose(self, msg): print("Update Initial Pose") """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg)
def update_particles_with_odom(self,msg): print("Update Particles with Odom") """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta #R eh o raio feito a partir de um pitagoras com o X e Y da variacao Delta r = math.sqrt(delta[0]**2 + delta[1]**2) #Funcao para conseguir um valor entre -pi e pi e subtrair o antigo valor de theta. (Achei um pouco confusa, ) phi = math.atan2(delta[1], delta[0])-old_odom_xy_theta[2] for particle in self.particle_cloud: particle.x += r*math.cos(phi+particle.theta) particle.y += r*math.sin(phi+particle.theta) particle.theta += delta[2]
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] # TODO create particles # Criando particula print("Inicializacao da Cloud de Particulas") #Valor auxiliar para nao dar erro na hora de criacao das particulas. Irrelevante valor_aux = 0.3 for i in range (self.n_particles): self.particle_cloud.append(Particle(0, 0, 0, valor_aux)) # Randomizar particulas em volta do robo. for i in self.particle_cloud: i.x = xy_theta[0]+ random.uniform(-1,1) i.y = xy_theta[1]+ random.uniform(-1,1) i.theta = xy_theta[2]+ random.uniform(-45,45) #Normalizar as particulas e dar update na posicao do robo self.normalize_particles() self.update_robot_pose() print(xy_theta)
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # Modify particles using delta for particle in self.particle_cloud: particle.x -= delta[0] particle.y -= delta[1] particle.theta += delta[2]
def initialize_particle_cloud(self, xy_theta=None): print 'Cria o set inicial de particulas' if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) x, y, theta = xy_theta # Altere este parametro para aumentar a circunferencia do filtro de particulas # Na VM ate 1 e suportado rad = 0.5 self.particle_cloud = [] self.particle_cloud.append( Particle(xy_theta[0], xy_theta[1], xy_theta[2])) # print 'particle_values_W', self.particle_cloud[0].w # print 'particle_values_X', self.particle_cloud[0].x # print 'particle_values_Y', self.particle_cloud[0].y # print 'particle_values_THETA', self.particle_cloud[0].theta for i in range(self.n_particles - 1): # initial facing of the particle theta = random.random() * 360 # compute params to generate x,y in a circle other_theta = random.random() * 360 radius = random.random() * rad # x => straight ahead x = radius * math.sin(other_theta) + xy_theta[0] y = radius * math.cos(other_theta) + xy_theta[1] particle = Particle(x, y, theta) self.particle_cloud.append(particle) self.normalize_particles()
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return print "delta: ", delta x, y = delta[0], delta[1] r = math.sqrt(x**2 + y**2) theta = np.arctan2(float(y), float(x)) phi = theta - old_odom_xy_theta[2] for particle in self.particle_cloud: particle.x += r * math.cos(phi + particle.theta) particle.y += r * math.sin(phi + particle.theta) particle.theta += delta[2]
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = {'x': new_odom_xy_theta[0] - self.current_odom_xy_theta[0], 'y': new_odom_xy_theta[1] - self.current_odom_xy_theta[1], 'theta': new_odom_xy_theta[2] - self.current_odom_xy_theta[2]} delta['r'] = math.sqrt(delta['x']**2 + delta['y']**2) delta['rot'] = angle_diff(math.atan2(delta['y'],delta['x']), old_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return for p in self.particle_cloud: p.x += delta['r']*math.cos(delta['rot'] + p.theta) p.y += delta['r']*math.sin(delta['rot'] + p.theta) p.theta += delta['theta']
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) rad = 1 # meters self.particle_cloud = [] self.particle_cloud.append(Particle(xy_theta[0], xy_theta[1], xy_theta[2])) for i in range(self.n_particles-1): # initial facing of the particle theta = random.random() * 360 # compute params to generate x,y in a circle other_theta = random.random() * 360 radius = random.random() * rad # x => straight ahead x = radius * math.sin(other_theta) + xy_theta[0] y = radius * math.cos(other_theta) + xy_theta[1] particle = Particle(x,y,theta) self.particle_cloud.append(particle) self.normalize_particles() self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None, num_particles=None, weight=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ print 'Initializing Particle Cloud...' if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if num_particles == None: num_particles = self.num_particles if weight == None: weight = 1 xs, ys, thetas = gen_random_particle_positions( xy_theta, (0.25, 0.25, math.pi / 7), num_particles) particle_cloud = [ Particle(x, y, theta, weight) for (x, y, theta) in zip(xs, ys, thetas) ] return particle_cloud
def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density self.fix_map_to_odom_transform( msg ) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg)
def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.particle_cloud = normalize_particles( self.initialize_particle_cloud(xy_theta)) self.update_robot_pose(self.particle_cloud) self.fix_map_to_odom_transform(msg)
def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! ''' É AQUI!!!! ''' self.update_particles_with_odom(msg) # update based on odometry - FAZER self.update_particles_with_laser(msg) # update based on laser scan - FAZER self.update_robot_pose() # update robot's pose """ abaixo """ self.resample_particles() # resample particles to focus on areas of high density - FAZER self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg)
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] # TODO create particles #zcw what the heck is rviz widget for selecting a guest. a guest to the starting pose of the robot self.normalize_particles() self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.get_particles_with_noise(xy_theta) self.normalize_particles() self.robot_pose = self.odom_pose.pose
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] # TODO create particles self.normalize_particles() self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # TODO create particles self.particle_cloud = self.initial_list_builder(xy_theta) self.normalize_particles() self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] for i in range(self.n_particles): self.particle_cloud.append(Particle(x=xy_theta[0]+gauss(0,0.25),y=xy_theta[1]+gauss(0,0.25),theta=xy_theta[2]+gauss(0,0.25))) self.normalize_particles() self.update_robot_pose()
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) print(new_odom_xy_theta) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) for p in self.particle_cloud: p.x += delta[0] p.y += delta[1] p.theta += delta[2] self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # ???? # TODO: modify particles using delta for p in self.particle_cloud: r = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2] d = math.sqrt((delta[0] ** 2) + (delta[1] ** 2)) p.theta += r % 360 p.x += d * math.cos(p.theta) + normal(0, .1) p.y += d * math.sin(p.theta) + normal(0, .1) p.theta += (delta[2] - r + normal(0, .1)) % 360
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] self.particle_cloud.append(Particle(xy_theta[0],xy_theta[1],xy_theta[2])) # Initialize particle cloud with a decent amount of noise for i in range (0,self.number_of_particles): self.particle_cloud.append(Particle(xy_theta[0]+np.random.randn()*.5,xy_theta[1]+np.random.randn()*.5,xy_theta[2]+np.random.randn()*.5)) self.normalize_particles() self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] # TODO create particles self.particle_cloud.append(Particle(x=0,y=0,theta=0)) # for i in range(self.n_particles): # self.particle_cloud.append(Particle(x=normal(0, .1), y=normal(0, .1), theta=normal(0, 1),w=1/float(self.n_particles))) self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # Creating particle cloud self.particle_cloud=[Particle(x=gauss(xy_theta[0], .3), y= gauss(xy_theta[1], .3), theta=gauss(xy_theta[2], .2)) for n in range (0, self.n_particles)] self.normalize_particles() self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # Create random array of particles self.particle_cloud = [ Particle(*arr) for arr in np.random.normal(xy_theta, self.sd_xy_theta, ( self.n_particles, len(xy_theta))) ] self.normalize_particles() self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] #one particle at origin #self.particle_cloud.append(Particle(0,0,0)) # TODO create particles for i in range(self.n_particles): self.particle_cloud.append(Particle(randn(), randn(),randn())) #add robot location guess to each number self.normalize_particles() self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # TODO create particles self.particle_cloud = [] for i in range(self.n_particles): #create particles with noise; within range +/- 0.3 of each aspect of pose x_gauss = gauss(xy_theta[0], 0.3) y_gauss = gauss(xy_theta[1], 0.3) theta_gauss = gauss(xy_theta[2], 0.3) #add particle to cloud self.particle_cloud.append(Particle(x_gauss, y_gauss, theta_gauss)) self.normalize_particles() self.update_robot_pose()
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return odom_noise = .3 # level of noise put into particles after update from odom to introduce variability # updates the particles based on r1, d, and r2. For more information on this, consult the website for particle in self.particle_cloud: # calculates r1, d, and r2 r1 = np.arctan2(float(delta[1]), float( delta[0])) - old_odom_xy_theta[2] d = np.sqrt(np.square(delta[0]) + np.square(delta[1])) r2 = delta[2] - r1 # updates the particles with the above variables, while also adding in some noise particle.theta = particle.theta + r1 * ( random_sample() * odom_noise + (1 - odom_noise / 2.0)) particle.x = particle.x + d * np.cos( particle.theta) * (random_sample() * odom_noise + (1 - odom_noise / 2.0)) particle.y = particle.y + d * np.sin( particle.theta) * (random_sample() * odom_noise + (1 - odom_noise / 2.0)) particle.theta = particle.theta + r2 * ( random_sample() * odom_noise + (1 - odom_noise / 2.0))
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], angle_diff(new_odom_xy_theta[2], self.current_odom_xy_theta[2])) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return for i, particle in enumerate(self.particle_cloud): # TODO: Change odometry uncertainty to be ROS param # Calculate the angle difference between the old odometry position # and the old particle position. Then create a rotation matrix between # the two angles rotationmatrix = self.make_rotation_matrix(particle.theta - old_odom_xy_theta[2]) # rotate the motion vector, add the result to the particle rotated_delta = np.dot(rotationmatrix, delta[:2]) linear_randomness = np.random.normal(1, 0.2) angular_randomness = np.random.uniform(particle.turn_multiplier, 0.3) particle.x += rotated_delta[0] * linear_randomness particle.y += rotated_delta[1] * linear_randomness particle.theta += delta[2] * angular_randomness # Make sure the particle's angle doesn't wrap particle.theta = angle_diff(particle.theta, 0)
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta is None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] linear_variance = 0.5 # meters angular_variance = 4 xs = np.random.normal(xy_theta[0], linear_variance, size=self.n_particles) ys = np.random.normal(xy_theta[1], linear_variance, size=self.n_particles) thetas = np.random.vonmises(xy_theta[2], angular_variance, size=self.n_particles) self.particle_cloud = [Particle(x=xs[i], y=ys[i], theta=thetas[i]) for i in xrange(self.n_particles)] self.normalize_particles() self.update_robot_pose()
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) x, y, theta = xy_theta self.particle_cloud = [ Particle( float(normal(x, self.initial_position_deviation)), float(normal(y, self.initial_position_deviation)), float(normal(theta, self.initial_angle_deviation)), ) for n in xrange(self.n_particles) ] self.normalize_particles() self.update_robot_pose()
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ # print "Odom particle updating" new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = ( new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2], ) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # Determine angular rotation and displacement from odom history r1 = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2] d = math.sqrt((delta[0] ** 2) + (delta[1] ** 2)) r2 = delta[2] - r1 # Update particle cloud for particle in self.particle_cloud: particle.theta = particle.theta + r1 # Initial rotation odom_x = math.cos(particle.theta) * d # Add displacement odom_y = math.sin(particle.theta) * d particle.x = particle.x + odom_x particle.y = particle.y + odom_y particle.theta = particle.theta + r2 # Second rotation # noise it up some: self.update_particles_with_noise()
def update_particles_with_odom(self, msg): new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # print 'new_odom_xy_theta', new_odom_xy_theta # Pega a posicao da odom (x,y,tehta) if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta # print 'delta', delta else: self.current_odom_xy_theta = new_odom_xy_theta return for particle in self.particle_cloud: d = math.sqrt((delta[0]**2) + (delta[1]**2)) # print 'particle_theta_1', particle.theta particle.x += d * (math.cos(particle.theta) + normal(0, 0.01)) particle.y += d * (math.sin(particle.theta) + normal(0, 0.01)) particle.theta = self.current_odom_xy_theta[2] #+ normal(0,0.05)
def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] (x, y, theta) = xy_theta # Create N particles centered around initial guess for i in range(self.n_particles): sample_x = x + randn() * self.std_x sample_y = y + randn() * self.std_y sample_theta = theta + uniform() * (2 * np.pi) self.particle_cloud.append( Particle(sample_x, sample_y, sample_theta)) self.normalize_particles() self.update_robot_pose()
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # TODONE: modify particles using delta destination_distance = math.hypot( delta[0], delta[1]) # distance between old and new odom # Angle to move by in order to change the heading to new theta destination_heading_theta = self.current_odom_xy_theta[2] - math.atan2( delta[1], delta[0]) # TODO: Parallelize this with a library for particle in self.particle_cloud: # Cosine of the particle heading - destination heading gives us the x component particle.x += destination_distance * math.cos( particle.theta - destination_heading_theta) # Sine of the particle heading - destination heading gives us the y component particle.y += destination_distance * math.sin( particle.theta - destination_heading_theta) particle.theta += delta[2]
def update_particles_with_odom(self, noise): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. noise: a number between 0 and 1 which describes how much noise we will add to the movements """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return d_x, d_y, d_theta = delta # move each particle the same distance in their coordinate frame new_particles = [] for p in self.particle_cloud: angle_between_frames = old_odom_xy_theta[2] - p.theta res = rotate(angle_between_frames, d_x, d_y) new_d_x = res[0] new_d_y = res[1] rands = random_sample(3) pos = np.array([p.x + new_d_x, p.y + new_d_y, p.theta + d_theta]) pos_noisy = pos * (1 + noise * (rands - 0.5)) new_particle = Particle.from_numpy(np.append(pos_noisy, p.w)) new_particles.append(new_particle) self.particle_cloud = new_particles
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return dx, dy, dtheta = delta ds = math.hypot(dx, dy) rotationOne = math.atan2(dy, dx) - self.current_odom_xy_theta[2] rotationTwo = dtheta - rotationOne for particle in self.particle_cloud: # Rotate particle to face its destination coordinate particle.rotate(rotationOne) # Advance the particle forward in its coordinate frame particle.x += ds * math.cos(particle.theta) particle.y += ds * math.sin(particle.theta) # Rotate particle to face its final heading particle.rotate(rotationTwo)
def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return temp = [] # Use trigonometry to update particles based on new odometry pose for particle in self.particle_cloud: psy = math.atan2(delta[1],delta[0])-old_odom_xy_theta[2] intermediate_theta = particle.theta + psy # Calculate radius based on change in x and y r = math.sqrt(delta[0]**2 + delta[1]**2) # Update x and y based on radius and new angle new_x = particle.x + r*math.cos(intermediate_theta) + np.random.randn()*0.1 new_y = particle.y + r*math.sin(intermediate_theta) + np.random.randn()*0.1 # Add change in angle to old angle new_theta = delta[2]+particle.theta + np.random.randn()*0.1 temp.append(Particle(new_x,new_y,new_theta)) self.particle_cloud = temp
def update_initial_pose(self, msg): xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta)
def scan_received(self, msg): # print msg """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # print 'msg.header.frame_id', msg.header.frame_id # calculate pose of laser relative ot the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry # listener.getLatestCommonTime("/base_link",object_pose_in.header.frame_id) # p = PoseStamped(header=Header(stamp=msg.header.stamp, p = PoseStamped( header=Header( stamp=self.tf_listener.getLatestCommonTime( self.base_frame, self.map_frame), # p = PoseStamped(header=Header(stamp=rospy.Time.now(), frame_id=self.base_frame), pose=Pose()) # p_aux = PoseStamped(header=Header(stamp=self.tf_listener.getLatestCommonTime("/base_link","/map"), p_aux = PoseStamped( header=Header( stamp=self.tf_listener.getLatestCommonTime( self.odom_frame, self.map_frame), # p_aux = PoseStamped(header=Header(stamp=rospy.Time.now(), frame_id=self.odom_frame), pose=Pose()) odom_aux = self.tf_listener.transformPose(self.map_frame, p_aux) odom_aux_xy_theta = convert_pose_to_xy_and_theta(odom_aux.pose) # print 'odom_aux_xy_theta', odom_aux_xy_theta self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # print 'self.odom_pose', self.odom_pose # (trans, root) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) # self.odom_pose = trans # print trans, root new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # new_odom_xy_theta = convert_pose_to_xy_and_theta(self.laser_pose.pose) xy_theta_aux = (new_odom_xy_theta[0] + odom_aux_xy_theta[0], new_odom_xy_theta[1] + odom_aux_xy_theta[1], new_odom_xy_theta[2]) self.xy_theta_aux = xy_theta_aux if not (self.particle_cloud): self.initialize_particle_cloud(xy_theta_aux) self.current_odom_xy_theta = new_odom_xy_theta elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.linear_mov or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.linear_mov or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.angular_mov): self.update_particles_with_odom(msg) self.update_particles_with_laser(msg) self.resample_particles() self.publish_particles(msg)