class Robot(): def __init__(self): rospy.init_node("robot") self.config = read_config() #r.seed(self.config['seed']) self.num_particles = self.config['num_particles'] self.laser_sigma_hit = self.config['laser_sigma_hit'] self.move_list = self.config['move_list'] self.move_list_length = len(self.move_list) self.first_move_sigma_x = self.config['first_move_sigma_x'] self.first_move_sigma_y = self.config['first_move_sigma_y'] self.first_move_sigma_angle = self.config['first_move_sigma_angle'] self.laser_z_hit = self.config['laser_z_hit'] self.laser_z_rand = self.config['laser_z_rand'] self.resample_sigma_x = self.config['resample_sigma_x'] self.resample_sigma_y = self.config['resample_sigma_y'] self.resample_sigma_theta = self.config['resample_sigma_angle'] self.handle_map_first_called = 1 self.move_made = 0 self.map_data_sub = rospy.Subscriber( "/map", OccupancyGrid, self.handle_map_data ) self.base_scan_data = rospy.Subscriber( "/base_scan", LaserScan, self.handle_base_scan_data ) self.particle_pose_pub = rospy.Publisher( "/particlecloud", PoseArray, queue_size = 10 ) self.likelihood_pub = rospy.Publisher( "/likelihood_field", OccupancyGrid, queue_size = 1, latch = True ) self.result_update_pub = rospy.Publisher( "/result_update", Bool, queue_size = 10 ) self.sim_complete_pub = rospy.Publisher( "/sim_complete", Bool, queue_size = 10 ) self.scan_data_avai = 0 self.sensor_loop() def sensor_loop(self): while not rospy.is_shutdown(): if ( self.handle_map_first_called == 0): #print "went in while(true) loop" self.create_particles() self.construct_field() #print "finished construct field" # publish OccupancyMap grid_mssg = self.my_map.to_message() rospy.sleep(1) self.likelihood_pub.publish(grid_mssg) self.make_all_moves() break rospy.sleep(1) self.sim_complete_pub.publish(True) rospy.sleep(1) rospy.signal_shutdown(Robot) def handle_base_scan_data (self, data): self.scan_info = data self.scan_data_avai = 1 def process_scan_data (self): while( self.scan_data_avai == 0 ): rospy.sleep(0.1) #print "process data" #self.scan_data_avai = 0 self.scan_data = self.scan_info for i in range (self.num_particles): if( np.isnan(self.particle_array[i].x) or np.isnan(self.particle_array[i].y) or np.isinf(self.particle_array[i].x) or np.isinf(self.particle_array[i].y) ): continue cell_prob1 = self.my_map.get_cell(self.particle_array[i].x, self.particle_array[i].y) cell_prob2 = self.true_map.get_cell(self.particle_array[i].x, self.particle_array[i].y) if (np.isnan(cell_prob1) or cell_prob2 == 1.0): self.particle_array[i].weight = 0.0 continue pz_array = [] for j in range (10): angle = self.particle_array[i].theta + (self.scan_data.angle_min + (self.scan_data.angle_increment * j)) x = self.particle_array[i].x + (self.scan_data.ranges[j] * np.cos(angle)) y = self.particle_array[i].y + (self.scan_data.ranges[j] * np.sin(angle)) if( np.isnan(x) or np.isnan(y) or np.isinf(x) or np.isinf(y) ): #lp = 0 pz = 0 else: lp = self.my_map.get_cell( x, y ) if( np.isnan(lp) ): pz = 0 else: pz = (self.laser_z_hit * lp) + self.laser_z_rand # if( np.isnan(lp) ): # #lp = 0 # pz = 0 #pz = (self.laser_z_hit * lp) + self.laser_z_rand pz_array.append(pz) p_tot = 0 for a in range (len(pz_array)): value_cubed = math.pow(pz_array[a],3) if(np.isnan(value_cubed)): continue else: p_tot = p_tot + value_cubed #sigmoid function for calculating weight self.particle_array[i].weight = self.particle_array[i].weight * (1.0/(1.0+math.pow(math.e, -1*p_tot))) self.normalize_weight() def create_particles(self): self.particle_array = [] 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 range (self.num_particles): # x and y are coordinates col = r.randint(0, self.my_map_width) row = r.randint(0, self.my_map_height) x, y = self.my_map.cell_position (row,col) theta = math.radians(r.random() * 360) pose = get_pose (x,y,theta) particle = Particle() particle.x = x particle.y = y particle.theta = theta particle.pose = pose particle.weight = (1.0/self.num_particles) self.particle_array.append(particle) self.pose_array.poses.append(pose) rospy.sleep(1) self.particle_pose_pub.publish(self.pose_array) def handle_map_data(self, data): if( self.handle_map_first_called == 1 ): self.my_map = Map(data) self.true_map = Map(data) self.my_map_width = self.my_map.width self.my_map_height = self.my_map.height self.handle_map_first_called = 0 #print "handle map is called" def make_all_moves(self): self.num_moves = len(self.move_list) for i in range (self.num_moves): #print "make_move" self.make_move() rospy.sleep(1) self.result_update_pub.publish(True) self.move_made = self.move_made + 1 def make_move(self): i = self.move_made #add noise to x, y, theta when first move move_function(self.move_list[i][0], 0) for j in range(len(self.particle_array)): self.particle_array[j].theta += math.radians(self.move_list[i][0]) self.particle_array[i].theta = self.particle_array[i].theta % (math.radians(360)) for a in range(self.move_list[i][2]): move_function(0, self.move_list[i][1]) self.particle_update(i) if(i == 0): self.particle_add_noise_first_move() self.process_scan_data() self.resampling_particle() def normalize_weight(self): total = 0 for k in range (self.num_particles): total += self.particle_array[k].weight for j in range (self.num_particles): self.particle_array[j].weight /= total total = 0 for m in range (self.num_particles): total += self.particle_array[m].weight #print "total should be 1: ", total def resampling_particle(self): #print "resampling" self.weight_array = [] new_array = [] for i in range (self.num_particles): self.weight_array.append(self.particle_array[i].weight) for j in range (self.num_particles): particle = np.random.choice(self.particle_array, None, True, self.weight_array) new_particle = Particle() new_particle.x = particle.x new_particle.y = particle.y new_particle.theta = particle.theta new_particle.weight = particle.weight new_particle.pose = particle.pose new_x = self.add_resample_noise(new_particle.x, self.resample_sigma_x) new_y = self.add_resample_noise(new_particle.y, self.resample_sigma_y) new_theta = self.add_resample_noise(new_particle.theta, self.resample_sigma_theta) new_pose = get_pose(new_x, new_y, new_theta) new_particle.x = new_x new_particle.y = new_y new_particle.theta = new_theta new_particle.pose = new_pose new_array.append(new_particle) for m in range (self.num_particles): self.particle_array[m] = new_array[m] self.pose_array.poses[m] = new_array[m].pose #self.normalize_weight() # publish the pose array rospy.sleep(1) self.particle_pose_pub.publish(self.pose_array) def particle_update (self, i): for a in range (self.num_particles): update_x = self.particle_array[a].x + self.move_list[i][1] * np.cos(self.particle_array[a].theta) update_y = self.particle_array[a].y + self.move_list[i][1] * np.sin(self.particle_array[a].theta) self.particle_array[a].x = update_x self.particle_array[a].y = update_y self.particle_array[a].pose = get_pose(update_x, update_y, self.particle_array[a].theta) def particle_add_noise_first_move(self): for a in range (self.num_particles): self.particle_array[a].x = self.add_first_move_noise(self.particle_array[a].x, self.first_move_sigma_x) self.particle_array[a].y = self.add_first_move_noise(self.particle_array[a].y, self.first_move_sigma_y) self.particle_array[a].theta = self.add_first_move_noise(self.particle_array[a].theta, self.first_move_sigma_angle) self.particle_array[a].pose = get_pose(self.particle_array[a].x, self.particle_array[a].y, self.particle_array[a].theta) def add_first_move_noise(self, coordinate, sd): # noise = r.gauss(0, sd)*100.)/100. noise = r.gauss(0, sd) added_noise = coordinate + noise return added_noise def add_resample_noise(self, coordinate, sd): # noise = math.ceil(r.gauss(0, sd) * 100.) /100. noise = r.gauss(0, sd) added_noise = coordinate + noise return added_noise def construct_field(self): self.kdtree_array = [] for i in range (self.my_map_height): for j in range (self.my_map_width): x, y = self.my_map.cell_position(i,j) value = self.my_map.get_cell(x, y) if( value == 1.0 ): self.kdtree_array.append([x, y]) self.kdtree = KDTree (self.kdtree_array) self.update_field() def update_field(self): for i in range (self.my_map_height): for j in range (self.my_map_width): coordinate = self.my_map.cell_position(i,j) dist, idx = self.kdtree.query([[coordinate[0],coordinate[1]]], k=1) new_value = self.calculate (dist) coordinate = self.my_map.cell_position(i,j) #if( self.my_map.get_cell(coordinate[0], coordinate[1]) == 0.0): self.my_map.set_cell(coordinate[0], coordinate[1], new_value) def calculate (self, distance): power = (-1.0 * ((distance*distance)/(2.0*self.laser_sigma_hit*self.laser_sigma_hit))) value = math.pow( math.e, power) return value
class Robot(): def __init__(self): """ This will read the config files and set up the different listeners and what not """ self.config = read_config() self.moveList = self.config['move_list'] self.particles = list() self.isMapInit = False random.seed(self.config['seed']) np.random.seed(self.config['seed']) self.baseMap = None self.likelihood_field = None self.laserScanFlag = False self.laserScanData = None self.robotInit() rospy.init_node("robot") self.start() def robotInit(self): """ Init Subscribers and Publishers""" self.mapSub = rospy.Subscriber( "/map", OccupancyGrid, self.mapInit ) self.laserScanSub = rospy.Subscriber( "/base_scan", LaserScan, self.laserScanUpdate ) #Do I need latch? TODO self.particleCloudPub = rospy.Publisher( "/particlecloud", PoseArray, queue_size = 10 ) self.likelihoodFieldPub = rospy.Publisher( "/likelihood_field", OccupancyGrid, queue_size = 10, latch = True ) self.resultUpdatePub = rospy.Publisher( "/result_update", Bool, queue_size = 10 ) self.simCompletePub = rospy.Publisher( "/sim_complete", Bool, queue_size = 10 ) def mapInit(self, mapData): if not self.isMapInit: self.isMapInit = True self.baseMap = Map(mapData) self.likelihood_field = Map(mapData) lsh = self.config['laser_sigma_hit'] width = self.baseMap.width height = self.baseMap.height occPoints = list() points = list() unoccPoints = list() for r in range(height): for c in range(width): x, y = self.baseMap.cell_position(r, c) if self.baseMap.get_cell(x, y) == 1: occPoints.append((x,y)) else: unoccPoints.append((x,y)) points.append((x,y)) self.occPointsSet = set(occPoints) occPoints = np.array(occPoints) points = np.array(points) self.occPoints = occPoints self.points = points self.unoccPoints = unoccPoints self.tree = KDTree(occPoints) distance, index = self.tree.query(points, k = 1) print "Points ", len(points), " should match ", width*height for d, i, p in zip(distance, index, points): x, y = p self.likelihood_field.set_cell(x, y, likeProb(d, lsh)) self.likelihoodFieldPub.publish(self.likelihood_field.to_message()) self.particleInit() def particleInit(self): default_weight = 1.0/(self.config["num_particles"]) numPart = self.config["num_particles"] randPoints = np.random.choice(range(len(self.unoccPoints)), numPart, replace=True) randPoints = [self.unoccPoints[i] for i in randPoints] for x, y in randPoints: p = Particle(x, y, m.pi*random.random(), default_weight) self.particles.append(p) self.publishParticles() self.move() def publishParticles(self): pose_array = self.poseArrayTemplate() for p in self.particles: pose_array.poses.append(p.get_pose()) self.particleCloudPub.publish(pose_array) def laserScanUpdate(self, laserScanMessage): if not self.laserScanFlag: self.laserScanFlag = True self.laserScanData = laserScanMessage def move(self): moves = self.config["move_list"] for i, (a, d, n) in enumerate(moves): print "Move %d, Turn at Angle %.2f, then move %.2f meters %d times." % (i, a, d, n) print "First turn at angle %.2f" % a helper_functions.move_function(a, 0) self.particles = self.updateMove(self.particles, 0.0, a) self.publishParticles() self.particles = self.commit(self.particles) self.publishParticles() for __ in xrange(n): print "Moving forward with distance %d" % d helper_functions.move_function(0, d) self.particles = self.updateMove(self.particles, d, 0.0) self.publishParticles() self.particles = self.commit(self.particles) self.publishParticles() self.resultUpdatePub.publish(True) self.simCompletePub.publish(True) rospy.sleep(10) rospy.signal_shutdown("sim complete") def updateMove(self, parts, d, t): p = parts[0] toret = list() rad = (m.pi * t)/180.0 for p in parts: theta = p.theta + rad x = p.x + d*m.cos(p.theta) y = p.y + d*m.sin(p.theta) particle = Particle(x, y, theta, p.weight) toret.append(particle) return toret def reweight(self, parts): zhit = self.config["laser_z_hit"] zrand = self.config["laser_z_rand"] lsd = self.getLaserScanData() increment = lsd.angle_increment startAngle = lsd.angle_min newParts = list() for p in parts: x, y, theta, q = p.x, p.y, p.theta, p.weight iswall = self.baseMap.get_cell(x, y) if m.isnan(iswall): q = 0.0 elif iswall == 1.0: q = 0.0 currincrement = 0.0 for r in lsd.ranges: if q == 0.0: break currx = x + r * m.cos(theta + startAngle + currincrement) curry = y + r * m.sin(theta + startAngle + currincrement) prob = self.likelihood_field.get_cell(currx, curry) if m.isnan(prob): prob = 0.0 q *= zhit * prob + zrand q += zrand currincrement += increment p.weight = q newParts.append(p) return self.normalize(newParts) def normalize(self, parts): #ALL WEIGHTS GETTING SO SMALL ITS BECOMING 0 weights = [p.weight * 1000 if not m.isnan(p.weight) else 0.0 for p in parts] normalizer = sum(weights) if m.isnan(normalizer): print "GOFFYY!!!!" else: weights = [w/normalizer if not m.isnan(w/normalizer) else 0.0 for w in weights] #why do this?! you don't use it again... print "TTTTTT", weights.count(0), len(weights) normalizer = sum(weights) for i, __ in enumerate(parts): parts[i].weight = weights[i] return parts def resample(self, parts): res = list() weights = [p.weight for p in parts] print "SUMMM", sum(weights) randPoints = np.random.choice(parts, self.config["num_particles"], replace=True, p=weights) for p in randPoints: noise_x = random.gauss(0, self.config["resample_sigma_x"]) noise_y = random.gauss(0, self.config["resample_sigma_y"]) noise_theta = random.gauss(0, self.config["resample_sigma_angle"]) p.x += noise_x p.y += noise_y p.theta += noise_theta res.append(p) return res def commit(self, parts): return self.resample(self.reweight(parts)) def getLaserScanData(self): self.laserScanFlag = False while not self.laserScanFlag: continue return self.laserScanData def poseArrayTemplate(self): pose_array = PoseArray() pose_array.header.stamp = rospy.Time.now() pose_array.header.frame_id = 'map' pose_array.poses =[] return pose_array def start(self): rospy.sleep(5) rospy.spin()
class Robot(): def __init__(self): self.config = read_config() rospy.init_node('robot') self.pose_array = PoseArray() self.pose_array.header.stamp = rospy.Time.now() self.pose_array.header.frame_id = 'map' self.pose_array.poses = [] self.moves = self.config["move_list"] #Particle update values to use self.isFirstMove = True self.first_sigma_x = read.config["first_move_sigma_x"] self.first_sigma_y = read.config["first_move_sigma_y"] self.first_sigma_angle = read.config["first_move_sigma_angle"] self.resample_sigma_x = read.config["resample_sigma_x"] self.resample_sigma_y = read.config["resample_sigma_y"] self.resample_sigma_angle = read.config["resample_sigma_angle"] laser_sub = rospy.Subscriber("/base_scan", LaserScan, self.sensorUpdate) self.likely_pub = rospy.Publisher("/likelihood_field", OccupancyGrid, queue_size=10, latch=True) self.pose_pub = rospy.Publisher("/particlecloud", PoseArray, queue_size=10) map_sub = rospy.Subscriber("/map", OccupancyGrid, self.mapCallBack) rospy.sleep(1) self.width = self.map.width self.height = self.map.height # particle init for i in range(0, self.config["num_particles"]): x = random.randint(0, self.width) y = random.randint(0, self.height) theta = random.uniform(0, 2 * m.pi) obstacle = self.map.get_cell(x, y) if (obstacle < 0.5): pose = get_pose(x, y, theta) self.pose_array.poses.append(pose) else: i -= 1 rospy.sleep(1) self.pose_pub.publish(self.pose_array) self.moveRobot(self.moves) rospy.spin() def mapCallBack(self, message): # 2 self.map = Map(message) self.width = self.map.width self.height = self.map.height sig = self.config['laser_sigma_hit'] coordinates = [] queryPts = [] rospy.sleep(1) # 3 for col in range(0, self.width): for row in range(0, self.height): x, y = self.map.cell_position(row, col) cellVal = self.map.get_cell(x, y) if (cellVal > 0.5): coordinates.append([x, y]) self.kdt = KDTree(coordinates) for col in range(0, self.width): for row in range(0, self.height): dist, ind = self.kdt.query([col, row], k=1) gauss = (1 / (sig * m.sqrt(2 * m.pi))) * ((m.e)**-(((dist)**2) / (2 * (sig**2)))) self.map.set_cell(col, row, gauss) self.likely_pub.publish(self.map.to_message()) def sensorUpdate(self, laser): self.laserMsg = laser def moveRobot(self, motion_cmd): for i in range(0, len(self.moves)): #4 motion = self.moves[i] a = motion[0] d = motion[1] n = motion[2] move_function(a, 0) for t in range(0, n): move_function(0, d) updateParticles(a, d) def updateParticles(a, d): new_pose_array = [] for j in range(0, self.config["num_particles"]): particle = self.pose_array.poses[j] x = particle[0] y = particle[1] theta = particle[2] if (self.isFirstMove): x = x + random.gauss(0, first_move_sigma_x) y = y + random.gauss(0, first_move_sigma_y) theta = theta + random.gauss(0, first_move_sigma_theta) self.isFirstMove = False x = x + d * math.cos(a) y = y + d * math.sin(a) theta = theta + a pose = get_pose(x, y, theta) new_pose_array.append(pose) self.pose_array.poses = new_pose_array #Reweight def motionUpdate(self, motion_cmd, particle): a = motion_cmd[0] d = motion_cmd[1] n = motion_cmd[2]
class Robot(): def __init__(self): self.config = read_config() rospy.init_node('robot') self.pose_array = PoseArray() self.pose_array.header.stamp = rospy.Time.now() self.pose_array.header.frame_id = 'map' self.pose_array.poses = [] self.move_list = self.config["move_list"] #Particle update values to use self.first_sigma_x = self.config["first_move_sigma_x"] self.first_sigma_y = self.config["first_move_sigma_y"] self.first_sigma_angle = self.config["first_move_sigma_angle"] self.resample_sigma_x = self.config["resample_sigma_x"] self.resample_sigma_y = self.config["resample_sigma_y"] self.resample_sigma_angle = self.config["resample_sigma_angle"] self.isFirstMove = True self.particleList = [] self.posePtr = [] self.tally = 0 laser_sub = rospy.Subscriber( "/base_scan", LaserScan, self.senseCallBack ) self.likely_pub = rospy.Publisher( "/likelihood_field", OccupancyGrid, queue_size = 10, latch = True ) self.pose_pub = rospy.Publisher( "/particlecloud", PoseArray, queue_size = 10 ) self.result_pub = rospy.Publisher( "/result_update", Bool, queue_size = 10 ) self.sim_pub = rospy.Publisher( "/sim_complete", Bool, queue_size = 10 ) map_sub = rospy.Subscriber( "/map", OccupancyGrid, self.mapCallBack ) rospy.sleep(1) self.width = self.map.width self.height = self.map.height # particle init i = 0 self.numParticles = self.config["num_particles"] while(i < self.numParticles): i += 1 x = random.randint(0, self.width) y = random.randint(0, self.height) theta = random.uniform(0, 2*m.pi) obstacle = self.map.get_cell(x,y) if(obstacle < 0.5): pose = get_pose(x,y,theta) self.pose_array.poses.append(pose) p = Particle(x,y,theta, 1) self.particleList.append(p) else: i -= 1 self.pose_pub.publish(self.pose_array) rospy.wait_for_message("/likelihood_field", OccupancyGrid) self.moveRobot() self.sim_pub.publish(True) rospy.is_shutdown() rospy.spin() def sigmoid(self, x): y = (1.0 / (1.0 + m.exp(-x))) return y def mapCallBack(self, message): self.map = Map(message) self.whocares = Map(message) self.width = self.map.width self.height = self.map.height self.sig = self.config['laser_sigma_hit'] coordinates = [] rospy.sleep(1) for col in range(0, self.width): for row in range(0, self.height): x, y = self.map.cell_position(row, col) self.cellVal = self.map.get_cell(x,y) if (self.cellVal > 0.5): coordinates.append([x,y]) self.kdt = KDTree(coordinates) for col in range(0, self.width): for row in range(0, self.height): dist, ind = self.kdt.query([col,row], k=1) gauss = (1/(self.sig*m.sqrt(2*m.pi)))*((m.e)**-(((dist)**2)/(2*(self.sig**2)))) self.map.set_cell(col, row, gauss) self.likely_pub.publish(self.map.to_message()) def senseCallBack(self, laser): self.lsr = laser def moveRobot(self): for t in range(0, len(self.move_list)): move = self.move_list[t] #move robot a = move[0] d = move[1] n = move[2] move_function(a,0) self.first = True for s in range(0, n): move_function(0, d) self.motion_update(move) self.first = False self.isFirstMove = False self.pose_array.poses = self.posePtr self.pose_pub.publish(self.pose_array) self.posePtr = [] self.sensor_update() self.resample() self.result_pub.publish(True) #For one particle def motion_update(self, mv): for i in range(0, len(self.particleList)): part_in = self.particleList[i] x = part_in.getX() y = part_in.getY() theta = part_in.getTheta() weight = part_in.getWeight() if (self.isFirstMove): x = x + random.gauss(0, self.first_sigma_x) y = y + random.gauss(0, self.first_sigma_y) theta = theta + random.gauss(0, self.first_sigma_angle) if (self.first): theta = theta + m.radians(mv[0]) x = x + mv[1] * m.cos(theta) y = y + mv[1] * m.sin(theta) new_pose = get_pose(x,y,theta) self.posePtr.append(new_pose) part_in.setX(x) part_in.setY(y) part_in.setTheta(theta) if (m.isnan(self.whocares.get_cell(x,y)) or self.whocares.get_cell(x,y) == 1.0): part_in.setWeight(0.0) def sensor_update(self): z_rand = self.config["laser_z_rand"] z_hit = self.config["laser_z_hit"] normalizer = 0 #Laser Scan for p in range(0, len(self.particleList)): particle = self.particleList[p] if(self.map.get_cell(particle.getX(),particle.getY()) == 1): particle.setWeight(0.0) ptot = 0 noNAN = 0 for l in range(0,len(self.lsr.ranges)): scan = self.lsr.ranges[l] if(scan != self.lsr.range_min and scan != self.lsr.range_max): angle = self.lsr.angle_min + (self.lsr.angle_increment * l) + particle.getTheta() x_pos = scan * m.cos(angle) + particle.getX() y_pos = scan * m.sin(angle) + particle.getY() lp = self.map.get_cell(x_pos,y_pos) if (m.isnan(lp)): noNAN += 1 else: if (noNAN > (len(self.lsr.ranges)*.75)): particle.setWeight(0.0) pz = (z_hit * lp) + z_rand ptot += (pz**2) old_weight = particle.getWeight() new_weight = old_weight * self.sigmoid(ptot) normalizer += new_weight new_particle = Particle(particle.getX(),particle.getY(),particle.getTheta(),new_weight) self.particleList[p] = new_particle for q in range(0, len(self.particleList)): particle = self.particleList[q] weight = particle.getWeight() / normalizer particle.setWeight(weight) def resample(self): good_weights = [] good_particles = [] for w in range(0, len(self.particleList)): particle = self.particleList[w] value = self.whocares.get_cell(particle.getX(),particle.getY()) if (not (value == 1.0 or m.isnan(value))): good_particles.append(self.particleList[w]) good_weights.append(self.particleList[w].getWeight()) # new_particles = np.random.choice(self.particleList, self.numParticles, True, weights) for p in range(0,len(self.particleList)): particle = self.particleList[p] value = self.whocares.get_cell(particle.getX(),particle.getY()) if (value == 1.0 or m.isnan(value)): dingledangle = np.random.choice(good_particles, 1, True, good_weights) particle = dingledangle[0] x = particle.getX() + random.gauss(0,self.resample_sigma_x) y = particle.getY() + random.gauss(0,self.resample_sigma_y) theta = particle.getTheta() + random.gauss(0,self.resample_sigma_angle) new_particle = Particle(x,y,theta,particle.getWeight()) self.particleList[p] = new_particle