def initializeParticle(self): self.rate.sleep() for i in range(self.numPart): x = random.uniform(0, self.width) y = random.uniform(0, self.height) while (self.mapOri.get_cell(x, y) != 0): x = random.uniform(0, self.width) y = random.uniform(0, self.height) self.particleX.append(x) self.particleY.append(y) self.particleTheta.append(random.uniform(0, 2 * np.pi)) self.particleWeight.append(1 / self.numPart) self.pose_array.poses.append( get_pose(self.particleX[i], self.particleY[i], self.particleTheta[i])) self.poseArray_publisher.publish(self.pose_array) obstalces = filter(lambda x: self.mapOri.get_cell(x[0], x[1]) != 0.0, [(x, y) for x in range(self.width) for y in range(self.height)]) self.obstacle_tree = KDTree(obstalces, leaf_size=20) self.mapLH.grid = np.array([[ GaussianPDF( self.get_nearest_obstacle(x, y)[0][0][0], 0, self.laser_sigma_hit) for x in range(self.width) ] for y in range(self.height)]) self.likelihood_publisher.publish(self.mapLH.to_message())
def first_move(self): move_list = self.config["move_list"][0] a = move_list[0] * m.pi / 180.0 d = move_list[1] n = move_list[2] hf.move_function(a, 0) p = self.particles for i in xrange(self.size): p[i].theta += a + r.gauss(0, self.config["first_move_sigma_angle"]) if (p[i].theta >= 2 * m.pi): p[i].theta -= 2 * m.pi elif (p[i].theta < 0): p[i].theta += 2 * m.pi for _ in xrange(n): hf.move_function(0, d) for i in xrange(self.size): dx = d * m.cos(p[i].theta) + r.gauss( 0, self.config["first_move_sigma_x"]) dy = d * m.sin(p[i].theta) + r.gauss( 0, self.config["first_move_sigma_y"]) p[i].x += dx p[i].y += dy self.pose_array.poses[i] = hf.get_pose(p[i].x, p[i].y, p[i].theta) self.particles = p self.particle_pub.publish(self.pose_array) self.moved = True self.num_moves += 1
def first_move(self): move_list = self.config["move_list"][0] a = move_list[0] * m.pi / 180.0 d = move_list[1] n = move_list[2] hf.move_function(a, 0) p = self.particles for i in xrange(self.size): p[i].theta += a + r.gauss(0, self.config["first_move_sigma_angle"]) if(p[i].theta >= 2*m.pi): p[i].theta -= 2*m.pi elif(p[i].theta < 0): p[i].theta += 2*m.pi for _ in xrange(n): hf.move_function(0,d) for i in xrange(self.size): dx = d*m.cos(p[i].theta)+r.gauss(0,self.config["first_move_sigma_x"]) dy = d*m.sin(p[i].theta)+r.gauss(0,self.config["first_move_sigma_y"]) p[i].x += dx p[i].y += dy self.pose_array.poses[i] = hf.get_pose(p[i].x, p[i].y, p[i].theta) self.particles = p self.particle_pub.publish(self.pose_array) self.moved = True self.num_moves += 1
def resample(self, particles, wheel): poses = [] sdx = self.config['resample_sigma_x'] sdy = self.config['resample_sigma_y'] sdt = self.config['resample_sigma_angle'] new_p = np.random.choice(particles, self.size, p = wheel) for i in xrange(self.size): self.particles[i].x = new_p[i].x + r.gauss(0,sdx) self.particles[i].y = new_p[i].y + r.gauss(0,sdy) self.particles[i].theta = new_p[i].theta + r.gauss(0,sdt) self.particles[i].weight = new_p[i].weight poses.append(hf.get_pose(self.particles[i].x,self.particles[i].y,self.particles[i].theta)) self.publish_pose(poses) self.moved = False
def initialize_particles(self): self.size = self.config["num_particles"] self.particles = [] self.pose_array = PoseArray() self.pose_array.header.stamp = rospy.Time.now() self.pose_array.header.frame_id = 'map' self.pose_array.poses = [] for i in xrange(self.size): x = r.randint(0, self.width) y = r.randint(0, self.height) t = r.random() * m.pi * 2 w = 1. / self.size pose = hf.get_pose(x, y, t) self.pose_array.poses.append(pose) self.particles.append(Particle(x, y, t, w)) self.particle_pub.publish(self.pose_array)
def initialize_particles(self): self.size = self.config["num_particles"] self.particles = [] self.pose_array = PoseArray() self.pose_array.header.stamp = rospy.Time.now() self.pose_array.header.frame_id = 'map' self.pose_array.poses = [] for i in xrange(self.size): x = r.randint(0,self.width) y = r.randint(0,self.height) t = r.random()*m.pi*2 w = 1./self.size pose = hf.get_pose(x,y,t) self.pose_array.poses.append(pose) self.particles.append(Particle(x,y,t,w)) self.particle_pub.publish(self.pose_array)
def moveParticles(self): self.moveList = self.config["move_list"] for i in range (len (self.moveList)): self.first = i self.mAngle = self.moveList[i][0] self.mDist = self.moveList[i][1] self.mSteps = self.moveList[i][2] with open ("moveparticleslog.txt", 'a') as infile: infile.write("mDist, mAngle, mSteps") infile.write(str(self.mDist)) infile.write(str(self.mAngle)) infile.write(str(self.mSteps)) infile.write("\n") #move robot by the angle hf.move_function( m.radians( self.mAngle ), float(0.0) ) #turn by angle, the particles and add noise only for the first move for p in range(len (self.particleArray)): self.particleArray[p][2] += m.radians(self.mAngle) if i == 0: self.particleArray[p][2] += random.gauss(0, self.config["first_move_sigma_angle"]) #move particles by m.Dist mStep times for j in range (self.mSteps): hf.move_function(0.0, self.mDist) #if self.first == 0: self.stepUpdate() with open ("moveparticleslog.txt", 'a') as infile: infile.write("before entering the weigh particles fucntion") infile.write("\n") #if self.first == 0: #weigh and resample particles here self.weighAndResampleParticles() self.poseArray.poses = [] for p in range(len (self.particleArray)): self.pose = hf.get_pose(self.particleArray[p][0], self.particleArray[p][1], self.particleArray[p][2]) self.poseArray.poses.append(self.pose) self.particleArray[p][4] = self.pose #if self.first == 0: #publishing particles to particle cloud self.particlePublisher.publish(self.poseArray)
def map_callback(self, data): self.map = Map(data) print "map called" #Initialize particles self.pose_array = PoseArray() self.pose_array.header.stamp = rospy.Time.now() self.pose_array.header.frame_id = 'map' self.pose_array.poses = [] for n in range(PARTICLE_NUM): randX = random.randint(0, self.map.width) randY = random.randint(0, self.map.height) randTheta = random.uniform(0, 2 * math.pi) self.pose_array.poses.append(helper_functions.get_pose(randX, randY, randTheta)) #Publish initial particles self.cloud_topic.publish(self.pose_array) #Construct likelihood field kdt = KDTree()
def initialize_particles(self): self.size = self.config["num_particles"] self.particles = [] poses = [] valid_spaces = [] prob_list = np.empty(self.size) prob_list.fill(1.0/self.size) for i in xrange(self.width): for j in xrange(self.height): if self.map.get_cell(i,j) < 1.0: valid_spaces.append(i + j*self.width) for i in xrange(self.size): ind = np.random.choice(valid_spaces, 1) y = ind[0] / self.width x = ind[0] - y*self.width t = r.uniform(0, 2*m.pi) w = 1./self.size poses.append(hf.get_pose(x,y,t)) self.particles.append(Particle(x,y,t,w)) self.publish_pose(poses)
def initializeParticle(self): self.rate.sleep() for i in range(self.numPart): x = random.uniform(0,self.width) y = random.uniform(0,self.height) while(self.mapOri.get_cell(x,y) != 0): x = random.uniform(0,self.width) y = random.uniform(0,self.height) self.particleX.append(x) self.particleY.append(y) self.particleTheta.append(random.uniform(0,2*np.pi)) self.particleWeight.append(1/self.numPart) self.pose_array.poses.append(get_pose(self.particleX[i],self.particleY[i],self.particleTheta[i])) self.poseArray_publisher.publish(self.pose_array) obstalces = filter(lambda x: self.mapOri.get_cell(x[0], x[1]) != 0.0, [(x, y) for x in range(self.width) for y in range(self.height)]) self.obstacle_tree = KDTree(obstalces, leaf_size=20) self.mapLH.grid = np.array([[GaussianPDF(self.get_nearest_obstacle(x,y)[0][0][0], 0, self.laser_sigma_hit) for x in range(self.width)] for y in range(self.height)]) self.likelihood_publisher.publish(self.mapLH.to_message())
def initializeParticles(self): #initialize all my particles #self.particleArray = [] self.particle = [] self.numP = self.config["num_particles"] for i in range(self.numP ): #self.particle is an array self.particle = np.random.uniform(0, self.map.width, 1) self.particle = np.append(self.particle, np.random.uniform(0, self.map.height, 1) ) self.particle = np.append(self.particle, np.random.uniform(0, 6.28, 1) ) self.particle = np.append(self.particle, np.array([1.0/800.0] ) ) self.pose = hf.get_pose(self.particle[0], self.particle[1], self.particle[2]) self.poseArray.poses.append(self.pose) self.particle = np.append(self.particle, np.array(self.pose)) self.particleArray.append(self.particle) print ("particle publishing") self.particlePublisher.publish(self.poseArray)
def move(self): first_move = True for moveStep in self.move_list: angle = moveStep[0] dist = moveStep[1] num = moveStep[2] print(angle) print(dist) print(num) if(first_move == True): move_function(angle,0) wTot = 0 wSoFar = 0 resampleProb = [] resampleX = [] resampleY = [] resampleAngle = [] resampleWeight = [] for i in range(self.numPart): self.particleTheta[i] = self.particleTheta[i]+(angle / 180.0 * self.angle_max)+random.gauss(0,self.first_move_sigma_angle) self.particleX[i] = (self.particleX[i]+random.gauss(0,self.first_move_sigma_x)) self.particleY[i] = (self.particleY[i]+random.gauss(0,self.first_move_sigma_y)) particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i]) if math.isnan(particle_poss) or particle_poss == 1.0: self.particleWeight[i] = 0.0 wTot += self.particleWeight[i] for i in range(self.numPart): self.particleWeight[i] = self.particleWeight[i]/wTot wSoFar += self.particleWeight[i] resampleProb.append(wSoFar) self.pose_array.header.stamp = rospy.Time.now() self.pose_array.poses = [] for i in range(self.numPart): re = random.random() index = -1 for j in range(self.numPart): if(re<=resampleProb[j]): index = j break resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x)) resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y)) resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle)) resampleWeight.append(self.particleWeight[index]) self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i])) self.particleX = resampleX self.particleY = resampleY self.particleTheta = resampleAngle self.particleWeight = resampleWeight self.poseArray_publisher.publish(self.pose_array) for manyTimes in range(num): move_function(0,dist) wTot = 0 wSoFar = 0 resampleProb = [] resampleX = [] resampleY = [] resampleAngle = [] resampleWeight = [] for i in range(self.numPart): self.particleX[i] = self.particleX[i]+random.gauss(0,self.first_move_sigma_x) self.particleY[i] = self.particleY[i]+random.gauss(0,self.first_move_sigma_y) self.particleTheta[i] = self.particleTheta[i]+random.gauss(0,self.first_move_sigma_angle) particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i]) if math.isnan(particle_poss) or particle_poss == 1.0: self.particleWeight[i] = 0.0 else: self.particleX[i] = self.particleX[i]+math.cos(self.particleTheta[i])*dist self.particleY[i] = self.particleY[i]+math.sin(self.particleTheta[i])*dist index = 0 pTot = 1 for z in self.ranges: laserAngle = self.angle_min + self.angle_incr*index index+=1 if(z>=self.range_min and z<=self.range_max): xz = self.particleX[i]+math.cos(self.particleTheta[i]+laserAngle)*z yz = self.particleY[i]+math.sin(self.particleTheta[i]+laserAngle)*z prob = self.mapLH.get_cell(xz,yz) if(math.isnan(prob)): prob = self.laser_z_rand else: prob = self.laser_z_hit * prob + self.laser_z_rand pTot *= prob self.particleWeight[i] = self.particleWeight[i]*(pTot+0.001) wTot += self.particleWeight[i] for i in range(self.numPart): self.particleWeight[i] = self.particleWeight[i]/wTot wSoFar += self.particleWeight[i] resampleProb.append(wSoFar) self.pose_array.header.stamp = rospy.Time.now() self.pose_array.poses = [] for i in range(self.numPart): re = random.random() index = -1 for j in range(self.numPart): if(re<=resampleProb[j]): index = j break resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x)) resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y)) resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle)) resampleWeight.append(self.particleWeight[index]) self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i])) self.particleX = resampleX self.particleY = resampleY self.particleTheta = resampleAngle self.particleWeight = resampleWeight self.poseArray_publisher.publish(self.pose_array) first_move = False else: move_function(angle,0) wTot = 0 wSoFar = 0 resampleProb = [] resampleX = [] resampleY = [] resampleAngle = [] resampleWeight = [] for i in range(self.numPart): self.particleTheta[i] = self.particleTheta[i]+(angle / 180.0 * self.angle_max) particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i]) if math.isnan(particle_poss) or particle_poss == 1.0: self.particleWeight[i] = 0.0 wTot += self.particleWeight[i] for i in range(self.numPart): self.particleWeight[i] = self.particleWeight[i]/wTot wSoFar += self.particleWeight[i] resampleProb.append(wSoFar) self.pose_array.header.stamp = rospy.Time.now() self.pose_array.poses = [] for i in range(self.numPart): re = random.random() index = -1 for j in range(self.numPart): if(re<=resampleProb[j]): index = j break resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x)) resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y)) resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle)) resampleWeight.append(self.particleWeight[index]) self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i])) self.particleX = resampleX self.particleY = resampleY self.particleTheta = resampleAngle self.particleWeight = resampleWeight self.poseArray_publisher.publish(self.pose_array) for manyTimes in range(num): move_function(0,dist) wTot = 0 wSoFar = 0 resampleProb = [] resampleX = [] resampleY = [] resampleAngle = [] resampleWeight = [] for i in range(self.numPart): particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i]) if math.isnan(particle_poss) or particle_poss == 1.0: self.particleWeight[i] = 0.0 else: self.particleX[i] = (self.particleX[i]+math.cos(self.particleTheta[i])*dist) self.particleY[i] = (self.particleY[i]+math.sin(self.particleTheta[i])*dist) index = 0 pTot = 1 for z in self.ranges: laserAngle = self.angle_min + self.angle_incr*index index+=1 if(z>=self.range_min and z<=self.range_max): xz = self.particleX[i]+math.cos(self.particleTheta[i]+laserAngle)*z yz = self.particleY[i]+math.sin(self.particleTheta[i]+laserAngle)*z prob = self.mapLH.get_cell(xz,yz) if(math.isnan(prob)): prob = self.laser_z_rand else: prob = self.laser_z_hit * prob + self.laser_z_rand pTot *= prob self.particleWeight[i] = self.particleWeight[i]*(-pTot+0.001) wTot += self.particleWeight[i] for i in range(self.numPart): self.particleWeight[i] = self.particleWeight[i]/wTot wSoFar += self.particleWeight[i] resampleProb.append(wSoFar) self.pose_array.header.stamp = rospy.Time.now() self.pose_array.poses = [] for i in range(self.numPart): re = random.random() index = -1 for j in range(self.numPart): if(re<=resampleProb[j]): index = j break resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x)) resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y)) resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle)) resampleWeight.append(self.particleWeight[index]) self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i])) self.particleX = resampleX self.particleY = resampleY self.particleTheta = resampleAngle self.particleWeight = resampleWeight self.poseArray_publisher.publish(self.pose_array) self.result_publisher.publish(self.mess) self.complete_publisher.publish(self.mess)
def get_pose(self): return helper_functions.get_pose(self.x, self.y, self.theta)
def update_pose(self): self.pose = helper_functions.get_pose(self.x, self.y, self.theta)
def __init__(self, x, y, theta, weight): self.x = x self.y = y self.theta = theta self.weight = weight self.pose = helper_functions.get_pose(x, y, theta)
def move(self): first_move = True for moveStep in self.move_list: angle = moveStep[0] dist = moveStep[1] num = moveStep[2] print(angle) print(dist) print(num) if (first_move == True): move_function(angle, 0) wTot = 0 wSoFar = 0 resampleProb = [] resampleX = [] resampleY = [] resampleAngle = [] resampleWeight = [] for i in range(self.numPart): self.particleTheta[i] = self.particleTheta[i] + ( angle / 180.0 * self.angle_max) + random.gauss( 0, self.first_move_sigma_angle) self.particleX[i] = ( self.particleX[i] + random.gauss(0, self.first_move_sigma_x)) self.particleY[i] = ( self.particleY[i] + random.gauss(0, self.first_move_sigma_y)) particle_poss = self.mapLH.get_cell( self.particleX[i], self.particleY[i]) if math.isnan(particle_poss) or particle_poss == 1.0: self.particleWeight[i] = 0.0 wTot += self.particleWeight[i] for i in range(self.numPart): self.particleWeight[i] = self.particleWeight[i] / wTot wSoFar += self.particleWeight[i] resampleProb.append(wSoFar) self.pose_array.header.stamp = rospy.Time.now() self.pose_array.poses = [] for i in range(self.numPart): re = random.random() index = -1 for j in range(self.numPart): if (re <= resampleProb[j]): index = j break resampleX.append(self.particleX[index] + random.gauss(0, self.resample_sigma_x)) resampleY.append(self.particleY[index] + random.gauss(0, self.resample_sigma_y)) resampleAngle.append( self.particleTheta[index] + random.gauss(0, self.resample_sigma_angle)) resampleWeight.append(self.particleWeight[index]) self.pose_array.poses.append( get_pose(resampleX[i], resampleY[i], resampleAngle[i])) self.particleX = resampleX self.particleY = resampleY self.particleTheta = resampleAngle self.particleWeight = resampleWeight self.poseArray_publisher.publish(self.pose_array) for manyTimes in range(num): move_function(0, dist) wTot = 0 wSoFar = 0 resampleProb = [] resampleX = [] resampleY = [] resampleAngle = [] resampleWeight = [] for i in range(self.numPart): self.particleX[i] = self.particleX[i] + random.gauss( 0, self.first_move_sigma_x) self.particleY[i] = self.particleY[i] + random.gauss( 0, self.first_move_sigma_y) self.particleTheta[ i] = self.particleTheta[i] + random.gauss( 0, self.first_move_sigma_angle) particle_poss = self.mapLH.get_cell( self.particleX[i], self.particleY[i]) if math.isnan(particle_poss) or particle_poss == 1.0: self.particleWeight[i] = 0.0 else: self.particleX[i] = self.particleX[i] + math.cos( self.particleTheta[i]) * dist self.particleY[i] = self.particleY[i] + math.sin( self.particleTheta[i]) * dist index = 0 pTot = 1 for z in self.ranges: laserAngle = self.angle_min + self.angle_incr * index index += 1 if (z >= self.range_min and z <= self.range_max): xz = self.particleX[i] + math.cos( self.particleTheta[i] + laserAngle) * z yz = self.particleY[i] + math.sin( self.particleTheta[i] + laserAngle) * z prob = self.mapLH.get_cell(xz, yz) if (math.isnan(prob)): prob = self.laser_z_rand else: prob = self.laser_z_hit * prob + self.laser_z_rand pTot *= prob self.particleWeight[i] = self.particleWeight[i] * ( pTot + 0.001) wTot += self.particleWeight[i] for i in range(self.numPart): self.particleWeight[i] = self.particleWeight[i] / wTot wSoFar += self.particleWeight[i] resampleProb.append(wSoFar) self.pose_array.header.stamp = rospy.Time.now() self.pose_array.poses = [] for i in range(self.numPart): re = random.random() index = -1 for j in range(self.numPart): if (re <= resampleProb[j]): index = j break resampleX.append( self.particleX[index] + random.gauss(0, self.resample_sigma_x)) resampleY.append( self.particleY[index] + random.gauss(0, self.resample_sigma_y)) resampleAngle.append( self.particleTheta[index] + random.gauss(0, self.resample_sigma_angle)) resampleWeight.append(self.particleWeight[index]) self.pose_array.poses.append( get_pose(resampleX[i], resampleY[i], resampleAngle[i])) self.particleX = resampleX self.particleY = resampleY self.particleTheta = resampleAngle self.particleWeight = resampleWeight self.poseArray_publisher.publish(self.pose_array) first_move = False else: move_function(angle, 0) wTot = 0 wSoFar = 0 resampleProb = [] resampleX = [] resampleY = [] resampleAngle = [] resampleWeight = [] for i in range(self.numPart): self.particleTheta[i] = self.particleTheta[i] + ( angle / 180.0 * self.angle_max) particle_poss = self.mapLH.get_cell( self.particleX[i], self.particleY[i]) if math.isnan(particle_poss) or particle_poss == 1.0: self.particleWeight[i] = 0.0 wTot += self.particleWeight[i] for i in range(self.numPart): self.particleWeight[i] = self.particleWeight[i] / wTot wSoFar += self.particleWeight[i] resampleProb.append(wSoFar) self.pose_array.header.stamp = rospy.Time.now() self.pose_array.poses = [] for i in range(self.numPart): re = random.random() index = -1 for j in range(self.numPart): if (re <= resampleProb[j]): index = j break resampleX.append(self.particleX[index] + random.gauss(0, self.resample_sigma_x)) resampleY.append(self.particleY[index] + random.gauss(0, self.resample_sigma_y)) resampleAngle.append( self.particleTheta[index] + random.gauss(0, self.resample_sigma_angle)) resampleWeight.append(self.particleWeight[index]) self.pose_array.poses.append( get_pose(resampleX[i], resampleY[i], resampleAngle[i])) self.particleX = resampleX self.particleY = resampleY self.particleTheta = resampleAngle self.particleWeight = resampleWeight self.poseArray_publisher.publish(self.pose_array) for manyTimes in range(num): move_function(0, dist) wTot = 0 wSoFar = 0 resampleProb = [] resampleX = [] resampleY = [] resampleAngle = [] resampleWeight = [] for i in range(self.numPart): particle_poss = self.mapLH.get_cell( self.particleX[i], self.particleY[i]) if math.isnan(particle_poss) or particle_poss == 1.0: self.particleWeight[i] = 0.0 else: self.particleX[i] = ( self.particleX[i] + math.cos(self.particleTheta[i]) * dist) self.particleY[i] = ( self.particleY[i] + math.sin(self.particleTheta[i]) * dist) index = 0 pTot = 1 for z in self.ranges: laserAngle = self.angle_min + self.angle_incr * index index += 1 if (z >= self.range_min and z <= self.range_max): xz = self.particleX[i] + math.cos( self.particleTheta[i] + laserAngle) * z yz = self.particleY[i] + math.sin( self.particleTheta[i] + laserAngle) * z prob = self.mapLH.get_cell(xz, yz) if (math.isnan(prob)): prob = self.laser_z_rand else: prob = self.laser_z_hit * prob + self.laser_z_rand pTot *= prob self.particleWeight[i] = self.particleWeight[i] * ( -pTot + 0.001) wTot += self.particleWeight[i] for i in range(self.numPart): self.particleWeight[i] = self.particleWeight[i] / wTot wSoFar += self.particleWeight[i] resampleProb.append(wSoFar) self.pose_array.header.stamp = rospy.Time.now() self.pose_array.poses = [] for i in range(self.numPart): re = random.random() index = -1 for j in range(self.numPart): if (re <= resampleProb[j]): index = j break resampleX.append( self.particleX[index] + random.gauss(0, self.resample_sigma_x)) resampleY.append( self.particleY[index] + random.gauss(0, self.resample_sigma_y)) resampleAngle.append( self.particleTheta[index] + random.gauss(0, self.resample_sigma_angle)) resampleWeight.append(self.particleWeight[index]) self.pose_array.poses.append( get_pose(resampleX[i], resampleY[i], resampleAngle[i])) self.particleX = resampleX self.particleY = resampleY self.particleTheta = resampleAngle self.particleWeight = resampleWeight self.poseArray_publisher.publish(self.pose_array) self.result_publisher.publish(self.mess) self.complete_publisher.publish(self.mess)