class RunRobot(object): ''' Represents all of the sensor and robot model related operations''' def __init__(self): self.transform_helper = TFHelper() self.odom_sub = rospy.Subscriber('odom', Odometry, self.odom_callback) self.odom_pos = None self.odom_ori = None self.odom_header = None self.robot_xyyaw_pose = None def odom_callback(self, msg): self.odom_header = msg.header self.odom_pos = msg.pose.pose.position self.odom_ori = msg.pose.pose.orientation def laserCallback(self, msg): ''' Represents all of the logic for handling laser messages''' # Set ranges for front, back, left, and right front_left = msg.ranges[0:90] back_left = msg.ranges[90:180] back_right = msg.ranges[180:270] front_right = msg.ranges[270:360] laser_diff = 0 #set difference between quadrants = 0 # Calculate difference between left front and back to determine where wall is on left side for front_left, back_left in zip(front_left, reversed(back_left)): if front_left == 0.0 or back_left == 0.0: continue laser_diff += front_left - back_left # Calculate diff between right front and back to determine where wall is on right side for front_right, back_right in zip(front_right, reversed(back_right)): if front_right == 0.0 or back_right == 0.0: continue laser_diff += back_right - front_right def robot_position(self): '''Represents the position of the robot as a x, y, yaw tuple''' pose = self.transform_helper.convert_translation_rotation_to_pose(self.odom_pos, self.odom_ori) self.robot_xyyaw_pose = self.transform.helper.convert_pose_to_xy_and_theta(pose) return self.robot_xyyaw_pose
def get_pose(self): translation = [self.x, self.y, 0] rotation = TFHelper.convert_theta_to_quaternion(self.theta) return TFHelper.convert_translation_rotation_to_pose( translation, rotation)
class RobotLocalizer(object): ''' The class that represents a Particle Filter ROS Node ''' def __init__(self): print('Initializing') self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('localizer') self.pf = ParticleFilter() self.base_frame = "base_link" # Robot base frame self.map_frame = "map" # Map coord frame self.odom_frame = "odom" # Odom coord frame self.scan_topic = "scan" # Laser scan topic self.linear_threshold = 0.1 # the amount of linear movement before performing an update self.angular_threshold = math.pi / 10 # the amount of angular movement before performing an update self.max_dist = 2.0 # maximum penalty to assess in the likelihood field model self.odom_pose = PoseStamped() self.robot_pose = Pose() self.robot_pose = Pose() self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.process_scan) # init pf # subscribers and publisher self.odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_particle_updater) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.pose_updater) # enable listening for and broadcasting coord transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.current_odom_xy_theta = [] print("initialization complete") self.initialized = True def robot_pose_updater(self): ''' Update the estimate of the robot's pose given the updated particles by computing the mean pose''' self.pf.particle_normalizer() # Calculate avg particle position based on pose mean_particle = Particle(0, 0, 0, 0) mean_particle_theta_x = 0 mean_particle_theta_y = 0 for particle in self.pf.particle_cloud: mean_particle.x += particle.x * particle.w mean_particle.y += particle.y * particle.w # Using trig to calculate angle (@Paul I hate Trigonometry!) distance_vector = np.sqrt( np.square(particle.y) + np.square(particle.x)) mean_particle_theta_x += distance_vector * np.cos( particle.theta) * particle.w mean_particle_theta_y += distance_vector * np.sin( particle.theta) * particle.w mean_particle.theta = np.arctan2(float(mean_particle_theta_y), float(mean_particle_theta_x)) self.robot_pose = mean_particle.particle_to_pose() # Get the laser messages def laserCallback(self, msg): self.laserCallback = msg def odom_particle_updater(self, msg): ''' Updates particles based on new odom pose using a delta value for x,y,theta''' new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # compute the change (delta) in x,y,theta 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 = .25 # noise level ''' updates the particles based on angle1, dist, and angle2. angle1: Angle by which the robot rotates to face new xy position dist: Distance to move forward to xy position angle2: Angle by which the robot rotates to face final direction ''' for particle in self.pf.particle_cloud: # calculates angle1, d, and angle2 angle1 = np.arctan2(float(delta[1]), float( delta[0])) - old_odom_xy_theta[2] dist = np.sqrt(np.square(delta[0]) + np.square(delta[1])) angle2 = delta[2] - angle1 # updates the particles with the above variables, while also adding in some noise #This is the part of class where Paul moved and we all updated based on that movement particle.theta = particle.theta + angle1 * ( random_sample() * odom_noise + (1 - odom_noise / 2.0)) particle.x = particle.x + dist * np.cos( particle.theta) * (random_sample() * odom_noise + (1 - odom_noise / 2.0)) particle.y = particle.y + dist * np.sin( particle.theta) * (random_sample() * odom_noise + (1 - odom_noise / 2.0)) particle.theta = particle.theta + angle2 * ( random_sample() * odom_noise + (1 - odom_noise / 2.0)) def particle_resampler(self): '''Resample the particles according to the new particle weights.''' # make sure the distribution is normalized self.pf.particle_normalizer() particle_list = [] weights = [] num_samples = len(self.pf.particle_cloud) for particle in self.pf.particle_cloud: particle_list.append(particle) weights.append(particle.w) self.pf.particle_cloud = self.draw_random_sample( particle_list, weights, num_samples) def draw_random_sample(self, choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ # sets up an index list for the chosen particles, and makes bins for the probabilities values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize( random_sample(n), bins )] # chooses the new particles based on the probabilities of the old ones samples = [] for i in inds: samples.append( deepcopy(choices[int(i)]) ) # makes the new particle cloud based on the chosen particles return samples def laser_particle_updater(self, msg): '''Updates the particle weights in response to the scan contained in the msg''' # Find the total error for each particle based on 36 laser measurements taken from the Neato's actual position for particle in self.pf.particle_cloud: error = [] for theta in range(0, 360, 10): rad = np.radians(theta) err = self.occupancy_field.get_closest_obstacle_distance( particle.x + msg.ranges[theta] * np.cos(particle.theta + rad), particle.y + msg.ranges[theta] * np.sin(particle.theta + rad)) if ( math.isnan(err) ): # if the get_closest_obstacle_distance method finds that a point is out of bounds, then the particle can't ever be it particle.w = 0 break if ( sum(error) == 0 ): # if the particle is basically a perfect match, then we make the particle almost always enter the next iteration through resampling particle.w = 1.0 else: particle.w = 1.0 / sum( error ) # the errors are inverted such that large errors become small and small errors become large def pose_updater(self, msg): ''' Restart particle filter based on updated pose ''' xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( msg.pose.pose) #self.fix_map_to_odom_transform(msg) self.pf.particle_cloud_init(xy_theta) self.fix_map_to_odom_transform(msg) print("particle cloud initialized") def process_scan(self, msg): '''Handling laser data to update our understanding of the robot based on laser and odometry''' if not (self.initialized): print("first if not") # wait for initialization to complete return if not (self.tf_listener.canTransform('base_link', 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('base_link', 'odom', 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 pose = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, pose) # find out where the robot thinks it is based on its odometry pose = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, pose) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not (self.pf.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.pf.particle_cloud_init() # cache the last odometry pose so we can only update our particle filter if we move more than self.linear_threshold or self.angular_threshold self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized print("Trying to initialize!") self.fix_map_to_odom_transform(msg) print("Initialized finally!") self.pf.particle_publisher(msg) else: # we have moved far enough to do an update! #self.odom_particle_updater(msg) # update based on odometry #print("map!") #self.laser_particle_updater(msg) # update based on laser scan #self.robot_pose_updater() # update robot's pose self.particle_resampler( ) # 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 self.pf.particle_publisher(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = \ self.transform_helper.convert_pose_inverse_transform(self.robot_pose) pose = PoseStamped( pose=self.transform_helper.convert_translation_rotation_to_pose( translation, rotation), header=Header(stamp=msg.header.stamp, frame_id=self.base_frame)) self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, msg.header.stamp, rospy.Duration(1.0)) self.odom_to_map = self.tf_listener.transformPose( self.odom_frame, pose) (self.translation, self.rotation) = self.transform_helper.convert_pose_inverse_transform( self.odom_to_map.pose) def send_last_map_to_odom_transform(self): if not (hasattr(self, 'translation') and hasattr(self, 'rotation')): print("sup dude") return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), 'odom', 'map')