diff --git a/gazebo_link_to_tf.py b/gazebo_link_to_tf.py index 169e988..19ecde4 100644 --- a/gazebo_link_to_tf.py +++ b/gazebo_link_to_tf.py @@ -4,22 +4,29 @@ 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") + 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] + 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") + (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") + (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) + 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