From 92645377739665adcf8c678e138e0065e8fe337b Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Sun, 9 Feb 2020 16:29:28 +0000 Subject: [PATCH] Update --- gazebo_link_to_tf.py | 29 +++++++++++++++++++++-------- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/gazebo_link_to_tf.py b/gazebo_link_to_tf.py index 3d9c6c2..f8b585b 100644 --- a/gazebo_link_to_tf.py +++ b/gazebo_link_to_tf.py @@ -5,21 +5,34 @@ from gazebo_msgs.msg import LinkStates def callback(data): # Get index baxter_base_idx = data.name.index("baxter::base") - brick_idx = data.name.index("t1::Table") - # Get baxter_base pose and Brick pose w.r.t. gazebo world + object_idx = data.name.index("t1::Table") + print('########################################') + print() + print() + print() + print() + print() + print(object_idx) + print() + print() + print() + print() + print() + print('########################################') + # Get baxter_base pose and object 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 + object_pose = data.pose[object_idx] + # Broadcast base and object 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 + # Broadcast base and object 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") + br2.sendTransform((object_pose.position.x, object_pose.position.y, object_pose.position.z), + (object_pose.orientation.x, object_pose.orientation.y, object_pose.orientation.z, object_pose.orientation.w), + rospy.Time.now(), "object","gazebo_world") def gazebo_link_subscriber():