Skip to content

Instantly share code, notes, and snippets.

@awesomebytes
Last active March 29, 2023 23:02

Revisions

  1. awesomebytes renamed this gist Mar 29, 2023. 1 changed file with 0 additions and 0 deletions.
    File renamed without changes.
  2. awesomebytes created this gist Mar 29, 2023.
    52 changes: 52 additions & 0 deletions tf_examply.py
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,52 @@
    #!/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()