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=rospy.Time(0),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)