Update

gazebo_link_to_tf.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+import rospy
+import tf
+from gazebo_msgs.msg import LinkStates
+def callback(data):
+	# Get index
+	baxter_base_idx = data.name.index("baxter::base") brick_idx = data.name.index("Brick::Brick")
+	# Get baxter_base pose and Brick pose w.r.t. gazebo world
+	baxter_base_pose = data.pose[baxter_base_idx] brick_pose = data.pose[brick_idx]
+	# Broadcast base and brick poses w.r.t. the gazebo world into tf tree
+	br = tf.TransformBroadcaster()
+	br.sendTransform((baxter_base_pose.position.x, baxter_base_pose.position.y, baxter_base_pose.position.z),
+	(baxter_base_pose.orientation.x, baxter_base_pose.orientation.y, baxter_base_pose.orientation.z,
+	baxter_base_pose.orientation.w), rospy.Time.now(),
+	"base", "gazebo_world")
+	# Broadcast base and brick poses w.r.t. the gazebo world into tf tree
+	br2 = tf.TransformBroadcaster()
+	br2.sendTransform((brick_pose.position.x, brick_pose.position.y, brick_pose.position.z),
+	(brick_pose.orientation.x, brick_pose.orientation.y, brick_pose.orientation.z, brick_pose.orientation.w),
+	rospy.Time.now(), "brick","gazebo_world") 
+def gazebo_link_subscriber():
+	rospy.init_node('gazebo_link_subscriber') rospy.Subscriber("/gazebo/link_states", LinkStates, callback)
+	# spin() simply keeps python from exiting until this node is stopped
+	rospy.spin()
+if __name__ == '__main__': gazebo_link_subscriber()

listener.py

@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+import rospy
+import tf
+if __name__ == '__main__':
+	""" A simple example of tf listener """ 
+	rospy.init_node('example_tf_listener')
+
+	listener = tf.TransformListener()
+
+	rate = rospy.Rate(10.0)
+	while not rospy.is_shutdown():
+		try:
+			(trans,rot) = listener.lookupTransform('left_gripper', 'base', rospy.Time(0))
+		except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+			continue
+			
+		Translation = [trans.x , trans.y , trans.z]
+		Quaternion = [rot.x , rot.y , rot.z, rot.w]
+		Angles = tf.transformations.euler_from_quaternion([rot.x , rot.y ,
+		rot.z, rot.w])
+		print("Translation: ", Translation) print("Quaternion: ", Quaternion) print("Angles: ", Angles)
+		print("")
+		rate.sleep()