diff --git a/gazebo_link_to_tf.py b/gazebo_link_to_tf.py new file mode 100644 index 0000000..169e988 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/listener.py b/listener.py new file mode 100644 index 0000000..5acbb1c --- /dev/null +++ b/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() \ No newline at end of file