Update

main.py

@@ -18,6 +18,8 @@ import baxter_interface
 
 from tf.transformations import quaternion_from_euler
 
+import threading
+
 table = {
     'id':'t1',
     'rframe':'world',
@@ -341,6 +343,21 @@ class PickAndPlace(object):
         self._retract()
 
 
+def _tf_service_initializer():
+    import object_tf_service as ots
+    tf_service = ots.init()
+
+
+
+def tf_service(init=True):
+    if init:
+        tf_service_thread = threading.Thread(target=_tf_service_initializer)
+        tf_service_thread.start()
+    else:
+        tf_service_thread.kill()
+
+
+
 def cleanup():
     delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
     for group in [[table], bricks_start, bricks_end]:
@@ -363,8 +380,7 @@ def load_objects():
     table_reference_frame="world"
     spawn_sdf(table['id'], table_xml, "/", table_pose, table_reference_frame)
     brick_ents = []
-    for x in [bricks_start, bricks_end]:
+    for brick in bricks_start:
-        for brick in x:
         ber = [brick['roll'], brick['pitch'], brick['yaw']] #brick_euler_rotation
         brick_pose = Pose(position=Point(x=brick['x'], y=brick['y'], z=brick['z']))
         brick_pose.position = Point(x=brick['x'], y=brick['y'], z=brick['z'])
@@ -374,10 +390,10 @@ def load_objects():
         brick_id = brick['id']
         brick_ents.append(spawn_sdf(brick_id, brick_xml, "/", brick_pose, brick_reference_frame))
 
+#TODO: delete this line
 
 
-
+rospy.init_node("ik_pick_and_place_demo") #What is this?
-rospy.init_node("ik_pick_and_place_demo")
 
 cleanup()
 
@@ -411,6 +427,8 @@ right_pnp.move_to_start(right_pnp.ik_request(right_pose))
 
 load_objects()
 
+print("loaded all objects, starting tf service thread")
+tf_service(init=True)
 
 ####################################HACKING BEGIN
 
@@ -437,5 +455,17 @@ load_objects()
 # right_pnp.move_to_start(right_pnp.ik_request(pose1))
 
 
+try:
+    print('Done with task, waiting to kill tf service')
+    while True:
+        time.sleep(0.1)
+except KeyboardInterrupt:
+    print("KeyboardInterrupt has been caught., killing tf service")
+    tf_service(init=False)
+
+print("Done, exiting")
+exit(0)
+
+
 
 

object_tf_service.py

@@ -44,6 +44,6 @@ class tf_service():
 		# spin() simply keeps python from exiting until this node is stopped
 		rospy.spin()
 
-x = tf_service()
+def init():
-
+	x = tf_service()
-x.gazebo_link_subscriber()
+	x.gazebo_link_subscriber()