tf_example.py

#!/usr/bin/env python

import rospy
import tf2_ros
from geometry_msgs.msg import PoseStamped

# You want to use "simulated time" so it uses the time from the rosbag
# rosparan set /use_sim_time true
# Play your rosbag with something like:

# rosbag play --clock my_rosbag

class EndEffectorPoseNode(object):
    def __init__(self):
        rospy.init_node("end_effector_pose_node_tf2")
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

        # The frame you want to get the pose relative to
        self.reference_frame = "base_link" # Could be "base_footprint"? "map"?
        self.target_frame = "end_effector_frame"

    def get_latest_pose(self):
        try:
            transform_stamped = self.tf_buffer.lookup_transform(self.reference_frame, 
                                                                self.target_frame, 
                                                                rospy.Time(0), # This means "now"
                                                                rospy.Duration(1.0)) # Some room to not fail, 1.0s is probably not necessary

            pose = PoseStamped()
            pose.header = transform_stamped.header
            pose.pose.position.x = transform_stamped.transform.translation.x
            pose.pose.position.y = transform_stamped.transform.translation.y
            pose.pose.position.z = transform_stamped.transform.translation.z
            pose.pose.orientation = transform_stamped.transform.rotation

            return pose
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
            rospy.logwarn("TF2 Exception: {}".format(e))
            return None

    def run(self):
        rate = rospy.Rate(20)  # 20 times a second
        while not rospy.is_shutdown():
            pose = self.get_latest_pose()
            if pose:
                rospy.loginfo("End-effector pose: {}".format(pose))
            rate.sleep()

if __name__ == "__main__":
    node = EndEffectorPoseNode()
    node.run()