class RobotLocalizer(object): """ doc """ def __init__(self): print("init RobotLocalizer") rospy.init_node('localizer') self.tfHelper = TFHelper() self.xs = None self.ys = None self.ranges = [] # Lidar scan self.last_odom_msg = None print(self.last_odom_msg) self.diff_transform = { 'translation': None, 'rotation': None, } self.odom_changed = False # Toggles to True when the odom frame has changed enough # subscribers and publisher self.laser_sub = rospy.Subscriber('/scan', LaserScan, self.process_scan) self.odom_sub = rospy.Subscriber("/odom", Odometry, self.update_odom) ### Used for the particle filter # publisher for the particle cloud for visualizing in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # publisher for the top weighted particle self.topparticle_pub = rospy.Publisher("topparticle", PoseArray, queue_size=10) # publisher for rviz markers self.pub_markers = rospy.Publisher('/visualization_markerarray', MarkerArray, queue_size=10) self.particle_filter = ParticleFilter(self.topparticle_pub, self.particle_pub, self.pub_markers) print("ParticleFilter initialized") print("RobotLocalizer initialized") def update_odom(self, msg): MIN_TRAVEL_DISANCE = 0.1 MIN_TRAVEL_ANGLE = math.radians(5) if self.last_odom_msg is None: self.last_odom_msg = msg return last_xyt = self.tfHelper.convert_pose_to_xy_and_theta( self.last_odom_msg.pose.pose) current_xyt = self.tfHelper.convert_pose_to_xy_and_theta(msg.pose.pose) # Get translation in odom translation = [ current_xyt[0] - last_xyt[0], # x current_xyt[1] - last_xyt[1], # y ] # rotate to vehicle frame translation = self.tfHelper.rotate_2d_vector( translation, -1 * last_xyt[2]) # negative angle to counter rotation # get orientation diff theta = self.tfHelper.angle_diff(current_xyt[2], last_xyt[2]) # Schedule to update particle filter if there's enough change distance_travelled = math.sqrt(translation[0]**2 + translation[1]**2) if distance_travelled > MIN_TRAVEL_DISANCE or theta > MIN_TRAVEL_ANGLE: # TODO(matt): consider using actual transform # last_to_current_transform = self.tfHelper.convert_translation_rotation_to_pose( # translation, self.tfHelper.convert_theta_to_quaternion(theta) # ) print("travelled: {}".format(distance_travelled)) last_to_current_transform = { 'translation': translation, 'rotation': theta, } print("transform\n x: {}\n y: {}".format( last_to_current_transform['translation'][0], last_to_current_transform['translation'][1])) self.diff_transform = last_to_current_transform self.last_odom_msg = msg self.odom_changed = True def process_scan(self, m): """Storing lidar data """ #TODO: self.ranges = m.ranges xs = [] ys = [] for i in range(len(self.ranges)): if self.ranges[i] != 0: theta = math.radians(i) r = self.ranges[i] xf = math.cos(theta) * r yf = math.sin(theta) * r xs.append(xf) ys.append(yf) self.xs = xs self.ys = ys def get_x_directions(self, x): interval = 360 / x angle = 0 directions = [] for i in range(x): dist = self.ranges[angle] directions.append((math.radians(angle), dist)) angle = angle + interval return directions def gen_neighbor_particles(self): """Generates particles around given points""" #TODO: pass def find_all_nearest_objects(self): """Determines nearest non-zero point of all point (loops through)""" #TODO: pass def get_encoder_value(self): """Records odom movement, translate to x, y, and theta""" #TODO: pass """ Functions to write or figure out where they are: Order of particle filter: 1. DONE generate initial 500 random particles 2. DONE get ranges from robot -determine 8 values for directions -find lowest distance to obstacle 3. Process particles - project lowest distance from robot onto each particle -DONE for each particle get nearest object -> error distance -DONE 1/error distance = particle.weight 4. DONE publish particle with highest weight 5. DONE resample particles based on weight 6. DONE move robot - get transform 7. DONE transform resampled points with randomness """ def run(self): # save odom position (Odom or TF Module) # self.generate_random_points() NUM_DIRECTIONS = 8 self.particle_filter.gen_init_particles() # # Get lidar readings in x directions # robo_pts = self.get_x_directions(NUM_DIRECTIONS) # # For each particle compare lidar scan with map # self.particle_filter.compare_points(robo_pts) # # # Publish best guessself.particle_filter.gen_init_particles() # self.particle_filter.publish_top_particle(self.topparticle_pub) # # # Resample particles # self.particle_filter.resample_particles() # # # Publish cloud # self.particle_filter.publish_particle_cloud(self.particle_pub) r = rospy.Rate(5) while not (rospy.is_shutdown()): self.particle_filter.gauge_particle_position() if (self.odom_changed): print("Odom changed, let's do some stuff") # Get lidar readings in x directions robo_pts = self.get_x_directions(NUM_DIRECTIONS) # Update particle poses self.particle_filter.update_all_particles(self.diff_transform) # Display new markers self.particle_filter.draw_markerArray() # For each particle compare lidar scan with map self.particle_filter.compare_points(robo_pts) # Publish best guess self.particle_filter.gen_init_particles() top_particle_pose = self.particle_filter.publish_top_particle() self.tfHelper.fix_map_to_odom_transform( top_particle_pose, rospy.Time(0)) # TODO: Move? # Resample particles self.particle_filter.resample_particles() # Publish cloud self.particle_filter.publish_particle_cloud() # Wait until robot moves enough again self.odom_changed = False self.tfHelper.send_last_map_to_odom_transform() r.sleep()
class ParticleFilter(object): """ The class that represents a Particle Filter ROS Node """ def __init__(self, top_particle_pub, particle_cloud_pub, particle_cloud_marker_pub): # # pose_listener responds to selection of a new approximate robot # # location (for instance using rviz) # rospy.Subscriber("initialpose", # PoseWithCovarianceStamped, # self.update_initial_pose) self.top_particle_pub = top_particle_pub self.particle_cloud_pub = particle_cloud_pub self.particle_cloud_marker_pub = particle_cloud_marker_pub # create instances of two helper objects that are provided to you # as part of the project self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.particles = [] self.markerArray = MarkerArray() def gauge_particle_position(self): xs = [particle.x for particle in self.particles] ys = [particle.y for particle in self.particles] print("min x: {} --- average x: {} --- max x: {} \nmin y: {} --- average y: {} --- max y: {}".format( min(xs), sum(xs)/len(xs), max(xs), min(ys), sum(ys)/len(ys), max(ys) )) def get_marker(self, x, y): marker = Marker() marker.header.frame_id = "base_link" marker.type = marker.SPHERE marker.pose.position.x = x marker.pose.position.y = y marker.pose.position.z = 0 marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 return marker def draw_markerArray(self): markerArray = MarkerArray() for p in self.particles: m = self.get_marker(p.x, p.y) markerArray.markers.append(m) self.particle_cloud_marker_pub.publish(markerArray) def publish_particle_cloud(self): msg = PoseArray() msg.header.frame_id = "map" # Make pose from particle for all particles msg.poses = [particle.get_pose() for particle in self.particles] # Publish self.particle_cloud_pub.publish(msg) def publish_top_particle(self): msg = PoseArray() top_particle = self.particles[0] for particle in self.particles: if particle.weight > top_particle.weight: top_particle = particle msg.poses.append(top_particle.get_pose()) # print(msg) self.top_particle_pub.publish(msg) return top_particle.get_pose() def gen_init_particles(self): """Generating random particles with x, y, and t values""" # width = self.occupancy_field.map.info.width # height = self.occupancy_field.map.info.height width = 10 height = 10 print("map width: {}, height: {}".format(width, height)) for i in range(500): # x = r.randrange(0,width) # y = r.randrange(0,height) x = r.uniform(-5, 5) y = r.uniform(-5, 5) t = math.radians(r.randrange(0,360)) p = Particle(x,y,t) self.particles.append(p) 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 = \ self.transform_helper.convert_pose_to_xy_and_theta(msg.pose.pose) # TODO this should be deleted before posting self.transform_helper.fix_map_to_odom_transform(msg.pose.pose, msg.header.stamp) # initialize your particle filter based on the xy_theta tuple def resample_particles(self): """Resample particles with replacement.""" if len(self.particles): weights = [particle.weight if not math.isnan(particle.weight) else 0.0001 for particle in self.particles] total_weight = sum(weights) weights = [weight / total_weight for weight in weights] before = time.time() self.particles = [particle.deep_copy() for particle in list(np.random.choice( # self.particles = [particle for particle in list(np.random.choice( self.particles, size=len(self.particles), replace=True, p=weights, ))] after = time.time() print("************************************timer: {}".format(after-before)) print("number of particles: {}".format(len(self.particles))) else: print("No particles to resample from") return None def update_all_particles(self, transform): for particle in self.particles: # self.update_particle(particle, transform) self.update_particle_with_randomness(particle, transform) def update_particle_with_randomness(self, particle, transform): # TODO(matt): Make this a tunable param DISTANCE_VAR_SCALE = 0.001 ANGLE_VAR_SCALE = math.radians(0.5) # NOTE: We scale the variance instead of the standard deviation because # that makes it independent of the update time (the noise in one update # will be the same as the sum of the noise in two updates) distance = math.sqrt(transform['translation'][0]**2 + transform['translation'][1]**2) translation_mean, translation_var = 0, DISTANCE_VAR_SCALE #* distance # scale with magnitude rotation_mean, rotation_var = 0, ANGLE_VAR_SCALE modified_transform = transform # modified_transform['translation'][0] += float(np.random.normal(translation_mean, math.sqrt(translation_var), 1)) # modified_transform['translation'][1] += float(np.random.normal(translation_mean, math.sqrt(translation_var), 1)) # modified_transform['rotation'] += float(np.random.normal(rotation_mean, math.sqrt(rotation_var))) modified_transform['translation'][0] += float(np.random.uniform(-0.01, 0.01, 1)) modified_transform['translation'][1] += float(np.random.uniform(-0.01, 0.01, 1)) modified_transform['rotation'] += float(np.random.uniform(-math.radians(5), -math.radians(5), 1)) self.update_particle(particle, modified_transform) def update_particle(self, particle, transform): # rotate translation in the particle's direction particle_translation = self.transform_helper.rotate_2d_vector(transform['translation'], particle.theta) particle.translate(particle_translation) particle.rotate(transform['rotation']) # def compare_points(self): # """Compares translated particle to lidar scans, returns weights values""" # distances = [] # errordis = 0 # for a in range(500): # particle.ParticleCloud(self.particle[a]) # for b in range(8): # d[b] = OccupancyField.get_closest_obstacle_distance(particle.ParticleCloud[b][1],particle.ParticleCloud[b][2]) # particle.Particle.weight = 1 / (sum(d) + .01) def compare_points(self, robo_pts): """ This function determines the weights for each particle. Args: robo_pts (list): is a list of lidar readings for n directions. It can be obtained by calling get_x_directions in robot localizerself. """ for p in self.particles: p_cloud = ParticleCloud(p) p_cloud.generate_points(robo_pts) d = [] for pt in p_cloud.pts: if pt[1] != 0: d dist = abs(self.occupancy_field.get_closest_obstacle_distance(pt[0],pt[1])) d.append(math.exp(-d**2/1)) p.weight = 1 / (sum(d) + .01) def run(self): pass