max/robotics
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max Hunt 2020-02-09 16:29:28 +00:00
parent 4bb6daa72f
commit 9264537773
1 changed files with 21 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gazebo_link_to_tf.py
View File

@ -5,21 +5,34 @@ from gazebo_msgs.msg import LinkStates
def callback(data): def callback(data):
# Get index # Get index
baxter_base_idx = data.name.index("baxter::base") baxter_base_idx = data.name.index("baxter::base")
brick_idx = data.name.index("t1::Table") object_idx = data.name.index("t1::Table")
# Get baxter_base pose and Brick pose w.r.t. gazebo world 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] baxter_base_pose = data.pose[baxter_base_idx]
brick_pose = data.pose[brick_idx] object_pose = data.pose[object_idx]
# 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
br = tf.TransformBroadcaster() br = tf.TransformBroadcaster()
br.sendTransform((baxter_base_pose.position.x, baxter_base_pose.position.y, baxter_base_pose.position.z), 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.x, baxter_base_pose.orientation.y, baxter_base_pose.orientation.z,
baxter_base_pose.orientation.w), rospy.Time.now(), baxter_base_pose.orientation.w), rospy.Time.now(),
"base", "gazebo_world") "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 = tf.TransformBroadcaster()
br2.sendTransform((brick_pose.position.x, brick_pose.position.y, brick_pose.position.z), br2.sendTransform((object_pose.position.x, object_pose.position.y, object_pose.position.z),
(brick_pose.orientation.x, brick_pose.orientation.y, brick_pose.orientation.z, brick_pose.orientation.w), (object_pose.orientation.x, object_pose.orientation.y, object_pose.orientation.z, object_pose.orientation.w),
rospy.Time.now(), "brick","gazebo_world") rospy.Time.now(), "object","gazebo_world")
def gazebo_link_subscriber(): def gazebo_link_subscriber():