Update

bricks_etc.py

@@ -11,7 +11,7 @@ table = {
     'yaw':radians(90)
     }
 
-bricks_end =[{ #DO NOT USE THIS
+bricks_end =[{
     'id':'ea1',
     'rframe':'t1',
     'x':0.46,

main.py

@@ -28,7 +28,9 @@ running = True
 
 bricks = bet.getBuildable()
 table = bet.getTable()
-brick_targets = bet.getTargets()
+brick_targets = bet.getDirections()
+built_bricks = bet.getTargets()
+
 
 
 class PickAndPlace(object):
@@ -164,7 +166,6 @@ def _tf_service_initializer():
     tf_service = ots.init()
 
 
-
 def tf_service(init=True):
     global tf_service_thread
     if init:
@@ -176,7 +177,6 @@ def tf_service(init=True):
         ots.SIGKILL = True
 
 
-
 def cleanup():
     delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
     for obj in bet.getAll():
@@ -187,6 +187,7 @@ def load_objects():
     # Load Table SDFs
     with open ("models/L3-table/model.sdf", "r") as table_file:table_xml=table_file.read().replace('\n', '')
     with open ("models/brick/model.sdf", "r") as brick_file:brick_xml=brick_file.read().replace('\n', '')
+    with open ("models/empty/model.sdf", "r") as empty_file:empty_xml=empty_file.read().replace('\n', '')
     # Spawn Table SDF
     rospy.wait_for_service('/gazebo/spawn_sdf_model')
     spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
@@ -207,6 +208,16 @@ def load_objects():
         brick_reference_frame=brick['rframe']
         brick_id = brick['id']
         brick_ents.append(spawn_sdf(brick_id, brick_xml, "/", brick_pose, brick_reference_frame))
+    
+    for brick in built_bricks:
+        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'])
+        bqo = quaternion_from_euler(brick['roll'], brick['pitch'], brick['yaw'])
+        brick_pose.orientation = Quaternion(bqo[0], bqo[1], bqo[2], bqo[3])
+        brick_reference_frame=brick['rframe']
+        brick_id = brick['id']
+        brick_ents.append(spawn_sdf(brick_id, empty_xml, "/", brick_pose, brick_reference_frame))
 
 #TODO: delete this line
 

models_NEEDTOMOVE/empty/model.config

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Brick</name>
+  <version>1.0</version>
+  <sdf version="1.2">model-1_2.sdf</sdf>
+  <sdf version="1.3">model-1_3.sdf</sdf>
+  <sdf version="1.4">model-1_4.sdf</sdf>
+  <sdf version="1.5">model.sdf</sdf>
+
+  <author>
+    <name>RON</name>
+    <email>r.saputra@imperial.ac.uk</email>
+  </author>
+
+  <description>
+    Brick model
+  </description>
+</model>

models_NEEDTOMOVE/empty/model.sdf

@@ -0,0 +1,24 @@
+<?xml version="1.0" ?>
+<sdf version='1.6'>
+  <model name='Brick'>
+    <static>true</static>
+    <link name='Brick'>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <visual name='Brick_visual'>
+        <pose frame=''>0 0 0 1.5708 -0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://Brick/meshes/Brick_1.STL</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 1 0 1</ambient>
+          <diffuse>0 1 0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+          <emissive>0 0 0 0</emissive>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>