From c53af97976e6d936ec1ab35db0490470d0a7a56f Mon Sep 17 00:00:00 2001
From: Max Hunt <mh2817@ic.ac.uk>
Date: Mon, 10 Feb 2020 13:12:46 +0000
Subject: [PATCH] Update

---
 gazebo_link_to_tf.py | 13 ------------
 object_tf_service.py | 49 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 49 insertions(+), 13 deletions(-)
 create mode 100644 object_tf_service.py

diff --git a/gazebo_link_to_tf.py b/gazebo_link_to_tf.py
index f8b585b..8cb684a 100644
--- a/gazebo_link_to_tf.py
+++ b/gazebo_link_to_tf.py
@@ -6,19 +6,6 @@ def callback(data):
 	# Get index
 	baxter_base_idx = data.name.index("baxter::base")
 	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]
 	object_pose = data.pose[object_idx]
diff --git a/object_tf_service.py b/object_tf_service.py
new file mode 100644
index 0000000..5a68af6
--- /dev/null
+++ b/object_tf_service.py
@@ -0,0 +1,49 @@
+#!/usr/bin/env python
+import rospy
+import tf
+from gazebo_msgs.msg import LinkStates
+
+class tf_service():
+	def __init__(self):
+		self._nodes = ['t1::Table',
+						'ia1::Brick',
+						'ia2::Brick',
+						'ia3::Brick',
+						'ib1::Brick',
+						'ib2::Brick',
+						'ic1::Brick',
+						'ic2::Brick',
+						'id1::Brick',
+						'ie1::Brick']
+
+
+
+	def _callback(self, data):
+		# Get index
+		baxter_base_idx = data.name.index("baxter::base")
+		object_idx = data.name.index("t1::Table")
+		# 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]
+		# 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 object poses w.r.t. the gazebo world into tf tree
+		br2 = tf.TransformBroadcaster()
+		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(self):
+		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()
+
+x = tf_service()
+
+x.gazebo_link_subscriber()
\ No newline at end of file