Last active
March 29, 2023 23:02
Revisions
-
awesomebytes renamed this gist
Mar 29, 2023 . 1 changed file with 0 additions and 0 deletions.There are no files selected for viewing
File renamed without changes. -
awesomebytes created this gist
Mar 29, 2023 .There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode charactersOriginal 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()