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)