configs / launch files

config/config_scene_dineros-left.json

@@ -227,11 +227,11 @@
       "id": "front_panel",
       "pos": {
         "z": 0.885,
-        "x": 0.6525
+        "x": 0.80525
       },
       "size": {
-        "length": 0.5,
-        "width": 0.7,
+        "length": 0.8,
+        "width": 1.8,
         "height": 0.01
       },
       "orientation": {
@@ -243,6 +243,26 @@
         "b": 0.15
       }
     },
+    {
+      "id": "blocker",
+      "pos": {
+        "z": 1.385,
+        "x": 1.00525
+      },
+      "size": {
+        "length": 0.1,
+        "width": 0.8,
+        "height": 1
+      },
+      "orientation": {
+        "w": 1
+      },
+      "color": {
+        "r": 0.15,
+        "g": 0.15,
+        "b": 0.15
+      }
+    },
     {
       "id": "binBlue",
       "type": "BIN",
@@ -287,8 +307,8 @@
       "id": "bigRed",
       "type": "BOX",
       "pos": {
-        "x": 0.3,
-        "y": 0.3,
+        "x": 0.55,
+        "y": 0.1,
         "z": 0.9355
       },
       "size": {
@@ -300,15 +320,15 @@
         "w": 1
       },
       "color": {
-        "r": 1
+        "r": "1"
       }
     },
     {
       "id": "bigBlue",
       "type": "BOX",
       "pos": {
-        "x": 0.1,
-        "y": -0.25,
+        "x": 0.5,
+        "y": -0.2,
         "z": 0.9355
       },
       "size": {
@@ -323,6 +343,47 @@
         "b": 1
       }
     },
+    {
+      "id": "bigGreen",
+      "type": "BOX",
+      "pos": {
+        "x": 0.5,
+        "y": 0.3,
+        "z": 0.9355
+      },
+      "size": {
+        "length": 0.0318,
+        "width": 0.0636,
+        "height": 0.091
+      },
+      "orientation": {
+        "w": 1
+      },
+      "color": {
+        "g": 1
+      }
+    },
+    {
+      "id": "bigYellow",
+      "type": "BOX",
+      "pos": {
+        "x": 0.3,
+        "y": 0,
+        "z": 0.9355
+      },
+      "size": {
+        "length": 0.0318,
+        "width": 0.0636,
+        "height": 0.091
+      },
+      "orientation": {
+        "w": 1
+      },
+      "color": {
+        "r": "1",
+        "g": "1"
+      }
+    },
     {
       "id": "smallBlue",
       "type": "BOX",
@@ -363,6 +424,47 @@
         "r": 1
       }
     },
+    {
+      "id": "smallYellow",
+      "type": "BOX",
+      "pos": {
+        "x": 0.65,
+        "y": -0.1,
+        "z": 0.9307
+      },
+      "size": {
+        "length": 0.0318,
+        "width": 0.0318,
+        "height": 0.0814
+      },
+      "orientation": {
+        "w": 1
+      },
+      "color": {
+        "r": "1",
+        "g": "1"
+      }
+    },
+    {
+      "id": "smallGreen",
+      "type": "BOX",
+      "pos": {
+        "x": 0.3,
+        "y": 0.15,
+        "z": 0.9307
+      },
+      "size": {
+        "length": 0.0318,
+        "width": 0.0318,
+        "height": 0.0814
+      },
+      "orientation": {
+        "w": 1
+      },
+      "color": {
+        "g": "1"
+      }
+    },
     {
       "id": "arm1",
       "type": "ARM",

config/config_scene_dineros-left.yaml

@@ -22,7 +22,8 @@
   { 'id': 'table-top', 'pos': { 'z': 0.885 },'size': { 'length': 0.8,'width': 0.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
   { 'id': 'left_panel',   'pos': { 'z': 0.885, 'y': -0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
   { 'id': 'right_panel',  'pos': { 'z': 0.885, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
-  { 'id': 'front_panel', 'pos': { 'z': 0.885, 'x': 0.6525 },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'front_panel', 'pos': { 'z': 0.885, 'x': 0.80525 },'size': { 'length': 0.8,'width': 1.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'blocker', 'pos': { 'z': 1.385, 'x': 1.00525 },'size': { 'length': 0.1,'width': 0.8,'height': 1.0 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
 
   # bins width/depth/height 28.5*19*15.5
   # their height is 89 + 7.75 = 0.9675 m
@@ -32,16 +33,22 @@
   { 'id': 'binBlue', 'type': 'BIN','pos': { 'x': -0.255,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
   { 'id': 'binRed',  'type': 'BIN','pos': { 'x': -0.055,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
 
-  { 'id': 'bigRed','type': 'BOX','pos': { 'x': 0.30,'y': 0.30,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
-  { 'id': 'bigBlue','type': 'BOX','pos': { 'x': 0.10,'y': -0.25,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+  # big objects
+  #{ 'id': 'bigRed1','type': 'BOX','pos': { 'x': 0.25,'y': 0.30,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+  { 'id': 'bigRed','type': 'BOX','pos': { 'x': 0.55,'y': 0.1,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': '1' } },  
+  { 'id': 'bigBlue','type': 'BOX','pos': { 'x': 0.50,'y': -0.2,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+  { 'id': 'bigGreen','type': 'BOX','pos': { 'x': 0.50,'y': 0.3,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+  { 'id': 'bigYellow','type': 'BOX','pos': { 'x': 0.30,'y': 0,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': '1', 'g': '1' } },
 
   { 'id': 'smallBlue','type': 'BOX','pos': { 'x': 0.30,'y': -0.20,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
   { 'id': 'smallRed','type': 'BOX','pos': { 'x': 0.15,'y': -0.15,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+  { 'id': 'smallYellow','type': 'BOX','pos': { 'x': 0.65,'y': -0.1,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'r': '1', 'g': '1' } },
+  { 'id': 'smallGreen','type': 'BOX','pos': { 'x': 0.30,'y': 0.15,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'g': '1' } },
 
   # heights of objects (including pins):
   # formula: half-height * 9.6 + 4.6
   # formula (python)
   # for i in range(3,10): print("{ 'id': 'object4x2x%.1f','type': 'BOX','pos': { 'x': 0.3,'y': %f,'z': %f },'size': { 'length': .0318, 'width': .0636,'height': %f },'orientation': { 'w': 1 },'color': {} }," % (i/2, (i-6)*.1, (i*48+23+8900)/10000, (i*96+46)/10000));
 
-  { 'id': 'arm','type': 'ARM','pos': { 'z': 0.89 , 'x': -0.22 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+  { 'id': 'arm1','type': 'ARM','pos': { 'z': 0.89 , 'x': -0.22 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
 ] }

config/config_scene_dineros-right.json

@@ -227,11 +227,11 @@
       "id": "front_panel",
       "pos": {
         "z": 0.885,
-        "x": 0.6525
+        "x": 0.80525
       },
       "size": {
-        "length": 0.5,
-        "width": 0.7,
+        "length": 0.8,
+        "width": 1.8,
         "height": 0.01
       },
       "orientation": {
@@ -244,7 +244,27 @@
       }
     },
     {
-      "id": "binYellow",
+      "id": "blocker",
+      "pos": {
+        "z": 1.385,
+        "x": 1.00525
+      },
+      "size": {
+        "length": 0.1,
+        "width": 0.8,
+        "height": 1
+      },
+      "orientation": {
+        "w": 1
+      },
+      "color": {
+        "r": 0.15,
+        "g": 0.15,
+        "b": 0.15
+      }
+    },
+    {
+      "id": "binGreen",
       "type": "BIN",
       "pos": {
         "x": -0.255,
@@ -260,12 +280,11 @@
         "w": 1
       },
       "color": {
-        "r": 1,
         "g": 1
       }
     },
     {
-      "id": "binGreen",
+      "id": "binYellow",
       "type": "BIN",
       "pos": {
         "x": -0.055,
@@ -281,28 +300,48 @@
         "w": 1
       },
       "color": {
-        "g": 1
+        "r": "1",
+        "g": "1"
       }
     },
     {
-      "id": "bigYellow",
+      "id": "bigRed",
       "type": "BOX",
       "pos": {
-        "x": 0.2,
-        "y": 0.05,
-        "z": 0.9403
+        "x": 0.25,
+        "y": -0.1,
+        "z": 0.9355
       },
       "size": {
         "length": 0.0318,
         "width": 0.0636,
-        "height": 0.1006
+        "height": 0.091
       },
       "orientation": {
         "w": 1
       },
       "color": {
-        "r": 1,
-        "g": 1
+        "r": "1"
+      }
+    },
+    {
+      "id": "bigBlue",
+      "type": "BOX",
+      "pos": {
+        "x": 0.3,
+        "y": 0.2,
+        "z": 0.9355
+      },
+      "size": {
+        "length": 0.0318,
+        "width": 0.0636,
+        "height": 0.091
+      },
+      "orientation": {
+        "w": 1
+      },
+      "color": {
+        "b": 1
       }
     },
     {
@@ -310,7 +349,7 @@
       "type": "BOX",
       "pos": {
         "x": 0.3,
-        "y": 0.3,
+        "y": -0.3,
         "z": 0.9355
       },
       "size": {
@@ -325,12 +364,73 @@
         "g": 1
       }
     },
+    {
+      "id": "bigYellow",
+      "type": "BOX",
+      "pos": {
+        "x": 0.5,
+        "y": 0,
+        "z": 0.9355
+      },
+      "size": {
+        "length": 0.0318,
+        "width": 0.0636,
+        "height": 0.091
+      },
+      "orientation": {
+        "w": 1
+      },
+      "color": {
+        "r": "1",
+        "g": "1"
+      }
+    },
+    {
+      "id": "smallBlue",
+      "type": "BOX",
+      "pos": {
+        "x": 0.5,
+        "y": 0.2,
+        "z": 0.9307
+      },
+      "size": {
+        "length": 0.0318,
+        "width": 0.0318,
+        "height": 0.0814
+      },
+      "orientation": {
+        "w": 1
+      },
+      "color": {
+        "b": 1
+      }
+    },
+    {
+      "id": "smallRed",
+      "type": "BOX",
+      "pos": {
+        "x": 0.65,
+        "y": 0.15,
+        "z": 0.9307
+      },
+      "size": {
+        "length": 0.0318,
+        "width": 0.0318,
+        "height": 0.0814
+      },
+      "orientation": {
+        "w": 1
+      },
+      "color": {
+        "r": 1
+      }
+    },
     {
       "id": "smallYellow",
       "type": "BOX",
       "pos": {
-        "x": 0.3,
-        "y": -0.2,
+        "x": 0.15,
+        "y": 0.1,
         "z": 0.9307
       },
       "size": {
@@ -342,15 +442,15 @@
         "w": 1
       },
       "color": {
-        "r": 1,
-        "g": 1
+        "r": "1",
+        "g": "1"
       }
     },
     {
       "id": "smallGreen",
       "type": "BOX",
       "pos": {
-        "x": 0.15,
+        "x": 0.5,
         "y": -0.15,
         "z": 0.9307
       },
@@ -363,7 +463,7 @@
         "w": 1
       },
       "color": {
-        "g": 1
+        "g": "1"
       }
     },
     {

config/config_scene_dineros-right.yaml

@@ -22,26 +22,33 @@
   { 'id': 'table-top', 'pos': { 'z': 0.885 },'size': { 'length': 0.8,'width': 0.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
   { 'id': 'left_panel',   'pos': { 'z': 0.885, 'y': -0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
   { 'id': 'right_panel',  'pos': { 'z': 0.885, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
-  { 'id': 'front_panel', 'pos': { 'z': 0.885, 'x': 0.6525 },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'front_panel', 'pos': { 'z': 0.885, 'x': 0.80525 },'size': { 'length': 0.8,'width': 1.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'blocker', 'pos': { 'z': 1.385, 'x': 1.00525 },'size': { 'length': 0.1,'width': 0.8,'height': 1.0 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
 
   # bins width/depth/height 28.5*19*15.5
   # their height is 89 + 7.75 = 0.9675 m
   # the width is 31.8 or 63.6
   # if they stand on a panel edge, they stand at 40 + 0.5 + 14.25 = 54.75
   # their x location is N*19 + 9.5 - 35 = N*20 - 25.5 = -25.5 and -5.5 and 14.5 if they have 1 cm distance
-  { 'id': 'binYellow', 'type': 'BIN','pos': { 'x': -0.255,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'r': 1, 'g': 1  } },
-  { 'id': 'binGreen',  'type': 'BIN','pos': { 'x': -0.055,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+  { 'id': 'binGreen', 'type': 'BIN','pos': { 'x': -0.255,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+  { 'id': 'binYellow',  'type': 'BIN','pos': { 'x': -0.055,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'r': '1', 'g': '1' } },
 
-  { 'id': 'bigYellow','type': 'BOX','pos': { 'x': 0.20,'y': 0.05,'z': 0.940300 },'size': { 'length': .0318, 'width': .0636,'height': 0.100600 },'orientation': { 'w': 1 },'color': { 'r': 1, 'g': 1 } },
-  { 'id': 'bigGreen','type': 'BOX','pos': { 'x': 0.30,'y': 0.30,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+  # big objects
+  #{ 'id': 'bigRed1','type': 'BOX','pos': { 'x': 0.25,'y': 0.30,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+  { 'id': 'bigRed','type': 'BOX','pos': { 'x': 0.25,'y': -0.1,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': '1' } },  
+  { 'id': 'bigBlue','type': 'BOX','pos': { 'x': 0.30,'y': 0.2,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+  { 'id': 'bigGreen','type': 'BOX','pos': { 'x': 0.30,'y': -0.3,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+  { 'id': 'bigYellow','type': 'BOX','pos': { 'x': 0.50,'y': 0,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': '1', 'g': '1' } },
 
-  { 'id': 'smallYellow','type': 'BOX','pos': { 'x': 0.30,'y': -0.20,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'r': 1, 'g': 1 } },
-  { 'id': 'smallGreen','type': 'BOX','pos': { 'x': 0.15,'y': -0.15,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+  { 'id': 'smallBlue','type': 'BOX','pos': { 'x': 0.50,'y': 0.20,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+  { 'id': 'smallRed','type': 'BOX','pos': { 'x': 0.65,'y': 0.15,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+  { 'id': 'smallYellow','type': 'BOX','pos': { 'x': 0.15,'y': 0.1,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'r': '1', 'g': '1' } },
+  { 'id': 'smallGreen','type': 'BOX','pos': { 'x': 0.50,'y': -0.15,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'g': '1' } },
 
   # heights of objects (including pins):
   # formula: half-height * 9.6 + 4.6
   # formula (python)
   # for i in range(3,10): print("{ 'id': 'object4x2x%.1f','type': 'BOX','pos': { 'x': 0.3,'y': %f,'z': %f },'size': { 'length': .0318, 'width': .0636,'height': %f },'orientation': { 'w': 1 },'color': {} }," % (i/2, (i-6)*.1, (i*48+23+8900)/10000, (i*96+46)/10000));
 
-  { 'id': 'arm','type': 'ARM','pos': { 'z': 0.89 , 'x': -0.22 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+  { 'id': 'arm2','type': 'ARM','pos': { 'z': 0.89 , 'x': -0.22 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
 ] }

launch/cell-controllers/noop-sim-cell_dineros-right.launch

+<launch>
+
+    <!-- MQTT server for communication with client cells -->
+    <arg name="mqtt_server" default="tcp://localhost:1883"
+         doc="address of the mqtt server for the connection to the controller"/>
+
+    <include file="$(find ccf)/launch/simulation_setup.launch"/>
+
+    <param name="/connector_node_ros_ccf/topics/command" value="/ceti_cell_2/command"/>
+
+    <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="ceti_cell_2" output="screen">
+        <param name="arm" type="string" value="arm2"/>
+        <param name="scene_observer" type="string" value="object_locator/scene/update"/>
+        <param name="mqtt_server" type="yaml" value="$(arg mqtt_server)"/>
+        <param name="scene" type="string"
+               value="$(find ccf_immersive_sorting)/config/config_scene_dineros-right.json"/>
+    </node>
+
+</launch>