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-10 13:38:27 +00:00
parent d6843e1178
commit deb9fbf653
1 changed files with 11 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
object_tf_service.py
View File

@ -5,7 +5,8 @@ from gazebo_msgs.msg import LinkStates
class tf_service(): class tf_service():
def __init__(self): def __init__(self):
self._nodes = ['t1::Table', self._nodes = ['baxter::base',
't1::Table',
'ia1::Brick', 'ia1::Brick',
'ia2::Brick', 'ia2::Brick',
'ia3::Brick', 'ia3::Brick',
@ -20,17 +21,15 @@ class tf_service():
def _callback(self, data): def _callback(self, data):
# Get index # Get index
baxter_base_idx = data.name.index("baxter::base") for object_name in self._nodes:
object_idx = data.name.index("t1::Table") object_idx = data.name.index(object_name)
# Get baxter_base pose and object pose w.r.t. gazebo world # Get baxter_base pose and object pose w.r.t. gazebo world
baxter_base_pose = data.pose[baxter_base_idx] object_pose = data.pose[object_idx]
object_pose = data.pose[object_idx] # Broadcast base and object 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((object_pose.position.x, object_pose.position.y, object_pose.position.z),
br.sendTransform((baxter_base_pose.position.x, baxter_base_pose.position.y, baxter_base_pose.position.z), (object_pose.orientation.x, object_pose.orientation.y, object_pose.orientation.z, object_pose.orientation.w),
(baxter_base_pose.orientation.x, baxter_base_pose.orientation.y, baxter_base_pose.orientation.z, rospy.Time.now(), object_name,'gazebo_world')
baxter_base_pose.orientation.w), rospy.Time.now(),
"object", "gazebo_world")
def gazebo_link_subscriber(self): def gazebo_link_subscriber(self):