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 """ p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
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) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
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 TODO: if you want to learn a lot about tf, reimplement this... I can provide you with some hints as to what is going on here. """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=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, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() mean_x = 0 mean_y = 0 mean_theta = 0 for particle in self.particle_cloud: mean_x += particle.x mean_y += particle.y mean_theta += particle.theta mean_x /= self.n_particles mean_y /= self.n_particles mean_theta /= self.n_particles quat_theta = quaternion_from_euler(0, 0, mean_theta) self.robot_pose = convert_translation_rotation_to_pose(translation=[mean_x, mean_y, 0], rotation=quat_theta)