diff --git a/config/config_scene.yaml b/config/config_scene.yaml
index 2620d0c0e5242d87d6dfbdc8d2c6e4b39d6e6c18..6522ef9b744be2d11e428eefa59d77c3dd56ac2e 100644
--- a/config/config_scene.yaml
+++ b/config/config_scene.yaml
@@ -3,7 +3,7 @@
{ 'id': 'tablePillar2','pos': { 'x': -0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'tablePillar3','pos': { 'x': -0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'tablePillar4','pos': { 'x': 0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
- { 'id': 'table','pos': { 'z': 0.7 },'size': { 'length': 1.6,'width': 1.6,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'tableCenter','pos': { 'z': 0.7 },'size': { 'length': 1.6,'width': 1.6,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'binBlue','type': 'BIN','pos': { 'x': -0.34,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
{ 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
{ 'id': 'binGreen','type': 'BIN','pos': { 'x': 0.46,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
diff --git a/config/config_scene_ads.yaml b/config/config_scene_ads.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..fcd2f258057fb9738da9ef688c45db4b1fd9220d
--- /dev/null
+++ b/config/config_scene_ads.yaml
@@ -0,0 +1,19 @@
+{ 'objects': [
+ { 'id': 'tablePillar1','pos': { 'x': 0.625,'y': 0.82,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'tablePillar2','pos': { 'x': -0.215,'y': -0.82,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'tablePillar3','pos': { 'x': -0.215,'y': 0.82,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'tablePillar4','pos': { 'x': 0.625,'y': -0.82,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'table','pos': { 'x' : 0.205 ,'z': 0.7 },'size': { 'length': 0.9,'width': 1.7,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'binBlue','type': 'BIN','pos': { 'x': 0.34,'y': -0.65,'z': 0.8325 },'size': { 'length': 0.2,'width': 0.3,'height': 0.15 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+ { 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': -0.65,'z': 0.8325 },'size': { 'length': 0.2,'width': 0.3,'height': 0.15 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+
+ { 'id': 'objectRed1','type': 'BOX','pos': { 'x': 0.5,'y': -0.1,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+ { 'id': 'objectRed2','type': 'BOX','pos': { 'x': 0.0, 'y' : 0.5,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+
+ { 'id': 'objectGreen1','type': 'BOX','pos': { 'x': 0.1,'y': 0.6,'z': 0.8105 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
+ { 'id': 'objectGreen2','type': 'BOX','pos': { 'x': 0.45,'y': 0.2,'z': 0.8105 },'size': {'length':0.031,'width':0.031,'height':0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
+
+ { 'id': 'objectBlue1','type': 'BOX','pos': { 'x': 0.25,'y': -0.4,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+ { 'id': 'objectBlue2','type': 'BOX','pos': { 'x': 0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+ { 'id': 'arm','type': 'ARM','pos': { 'z': 0.75 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+] }
diff --git a/config/config_scene_ceti-table.json b/config/config_scene_ceti-table.json
new file mode 100644
index 0000000000000000000000000000000000000000..597f6fa8724b33d5dc9fc70bcc2b55bbb17b432b
--- /dev/null
+++ b/config/config_scene_ceti-table.json
@@ -0,0 +1,389 @@
+{
+ "objects": [
+ {
+ "id": "wheel_1",
+ "pos": {
+ "x": 0.28,
+ "y": 0.28,
+ "z": 0.06
+ },
+ "size": {
+ "length": 0.12,
+ "width": 0.12,
+ "height": 0.12
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "wheel_2",
+ "pos": {
+ "x": -0.28,
+ "y": -0.28,
+ "z": 0.06
+ },
+ "size": {
+ "length": 0.12,
+ "width": 0.12,
+ "height": 0.12
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "wheel_3",
+ "pos": {
+ "x": -0.28,
+ "y": 0.28,
+ "z": 0.06
+ },
+ "size": {
+ "length": 0.12,
+ "width": 0.12,
+ "height": 0.12
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "wheel_4",
+ "pos": {
+ "x": 0.28,
+ "y": -0.28,
+ "z": 0.06
+ },
+ "size": {
+ "length": 0.12,
+ "width": 0.12,
+ "height": 0.12
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "table_body",
+ "pos": {
+ "z": 0.5
+ },
+ "size": {
+ "length": 0.69,
+ "width": 0.69,
+ "height": 0.76
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "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": "ffront_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": "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": "binGreen",
+ "type": "BIN",
+ "pos": {
+ "x": 0.145,
+ "y": 0.5475,
+ "z": 0.9675
+ },
+ "size": {
+ "length": 0.19,
+ "width": 0.285,
+ "height": 0.155
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 1
+ }
+ },
+ {
+ "id": "object4x2x1.5",
+ "type": "BOX",
+ "pos": {
+ "x": 0.3,
+ "y": -0.3,
+ "z": 0.9067
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.0334
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {}
+ },
+ {
+ "id": "object4x2x2.0",
+ "type": "BOX",
+ "pos": {
+ "x": 0.3,
+ "y": -0.2,
+ "z": 0.9115
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.043
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {}
+ },
+ {
+ "id": "object4x2x2.5",
+ "type": "BOX",
+ "pos": {
+ "x": 0.3,
+ "y": -0.1,
+ "z": 0.9163
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.0526
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {}
+ },
+ {
+ "id": "object4x2x3.0",
+ "type": "BOX",
+ "pos": {
+ "x": 0.3,
+ "y": 0,
+ "z": 0.9211
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.0622
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {}
+ },
+ {
+ "id": "object4x2x3.5",
+ "type": "BOX",
+ "pos": {
+ "x": 0.3,
+ "y": 0.1,
+ "z": 0.9259
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.0718
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {}
+ },
+ {
+ "id": "object4x2x4.0",
+ "type": "BOX",
+ "pos": {
+ "x": 0.3,
+ "y": 0.2,
+ "z": 0.9307
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.0814
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {}
+ },
+ {
+ "id": "object4x2x4.5",
+ "type": "BOX",
+ "pos": {
+ "x": 0.3,
+ "y": 0.3,
+ "z": 0.9355
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.091
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {}
+ },
+ {
+ "id": "arm",
+ "type": "ARM",
+ "pos": {
+ "z": 0.89,
+ "x": -0.22
+ },
+ "size": {},
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 1,
+ "g": 1,
+ "b": 1
+ }
+ }
+ ]
+}
diff --git a/config/config_scene_ceti-table.yaml b/config/config_scene_ceti-table.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..99d579a4abefcfe6452a26292d03eb5aeacb566a
--- /dev/null
+++ b/config/config_scene_ceti-table.yaml
@@ -0,0 +1,44 @@
+# create json file using
+# yq eval -o=json config_scene_ceti-table.yaml > config_scene_ceti-table.json
+{ 'objects': [
+ # table
+ # height: 12 cm wheels, 76 cm body, 1 cm table-top = 89 cm total
+ # wheels width/depth/height 12*12*12
+ # body width/depth/height 69*69*76
+ # table-top width/depth/height 80*80*1
+ # panels are 50*70, with 0.5 cm distance to the table-top
+ # height of both is 12 + 76 + .5 = 88.5
+ # their centre is at 40 + 0.25 + 25 = 65.25
+ { 'id': 'wheel_1','pos': { 'x': 0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'wheel_2','pos': { 'x': -0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'wheel_3','pos': { 'x': -0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'wheel_4','pos': { 'x': 0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table_body','pos': { 'z': 0.50 },'size': { 'length': .69,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { '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': 'ffront_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 } },
+
+ # 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': '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': 'binGreen','type': 'BIN','pos': { 'x': 0.145,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'r': 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': 'object4x2x1.5','type': 'BOX','pos': { 'x': 0.3,'y': -0.300000,'z': 0.906700 },'size': { 'length': .0318, 'width': .0636,'height': 0.033400 },'orientation': { 'w': 1 },'color': {} },
+ { 'id': 'object4x2x2.0','type': 'BOX','pos': { 'x': 0.3,'y': -0.200000,'z': 0.911500 },'size': { 'length': .0318, 'width': .0636,'height': 0.043000 },'orientation': { 'w': 1 },'color': {} },
+ { 'id': 'object4x2x2.5','type': 'BOX','pos': { 'x': 0.3,'y': -0.100000,'z': 0.916300 },'size': { 'length': .0318, 'width': .0636,'height': 0.052600 },'orientation': { 'w': 1 },'color': {} },
+ { 'id': 'object4x2x3.0','type': 'BOX','pos': { 'x': 0.3,'y': 0.000000,'z': 0.921100 },'size': { 'length': .0318, 'width': .0636,'height': 0.062200 },'orientation': { 'w': 1 },'color': {} },
+ { 'id': 'object4x2x3.5','type': 'BOX','pos': { 'x': 0.3,'y': 0.100000,'z': 0.925900 },'size': { 'length': .0318, 'width': .0636,'height': 0.071800 },'orientation': { 'w': 1 },'color': {} },
+ { 'id': 'object4x2x4.0','type': 'BOX','pos': { 'x': 0.3,'y': 0.200000,'z': 0.930700 },'size': { 'length': .0318, 'width': .0636,'height': 0.081400 },'orientation': { 'w': 1 },'color': {} },
+ { 'id': 'object4x2x4.5','type': 'BOX','pos': { 'x': 0.3,'y': 0.300000,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': {} },
+
+ { '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 } }
+] }
diff --git a/config/config_scene_st.yaml b/config/config_scene_st.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b6e5270b1ff9ceedc14116d8e8f93c3255fbac36
--- /dev/null
+++ b/config/config_scene_st.yaml
@@ -0,0 +1,19 @@
+{ 'objects': [
+ { 'id': 'tablePillar1','pos': { 'x': 0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'tablePillar2','pos': { 'x': -0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'tablePillar3','pos': { 'x': -0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'tablePillar4','pos': { 'x': 0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'table','pos': { 'z': 0.7 },'size': { 'length': 1.6,'width': 1.6,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'binBlue','type': 'BIN','pos': { 'x': -0.34,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+ { 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+
+ { 'id': 'objectRed1','type': 'BOX','pos': { 'x': 0.5,'y': -0.1,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+ { 'id': 'objectRed2','type': 'BOX','pos': { 'x': -0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+
+ { 'id': 'objectGreen1','type': 'BOX','pos': { 'x': 0.45,'y': -0.3,'z': 0.8105 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
+ { 'id': 'objectGreen2','type': 'BOX','pos': { 'x': -0.45,'y': -0.2,'z': 0.8105 },'size': {'length':0.031,'width':0.031,'height':0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
+
+ { 'id': 'objectBlue1','type': 'BOX','pos': { 'x': 0.25,'y': -0.4,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+ { 'id': 'objectBlue2','type': 'BOX','pos': { 'x': 0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+ { 'id': 'arm','type': 'ARM','pos': { 'z': 0.75 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+] }
diff --git a/config/config_scene_td.yaml b/config/config_scene_td.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5c43b6324ef8d2daba5e76baa49bfec61efd8a97
--- /dev/null
+++ b/config/config_scene_td.yaml
@@ -0,0 +1,26 @@
+{ 'objects': [
+{ 'id': 'tablePillar1','pos': { 'x': 0.37,'y': 0.37,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+{ 'id': 'tablePillar2','pos': { 'x': -0.37,'y': -0.37,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+{ 'id': 'tablePillar3','pos': { 'x': -0.37,'y': 0.37,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+{ 'id': 'tablePillar4','pos': { 'x': 0.37,'y': -0.37,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+
+{ 'id': 'tableCenter','pos': { 'z': 0.7 },'size': { 'length': 0.8,'width': 0.8,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+{ 'id': 'tableLeft','pos': { 'z': 0.7, 'y' : -0.665 },'size': { 'length': 0.8,'width': 0.51,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+{ 'id': 'tableRight','pos': { 'z': 0.7, 'y' : 0.665 },'size': { 'length': 0.8,'width': 0.51,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+{ 'id': 'tableFront','pos': { 'z': 0.7, 'x' : 0.665 },'size': { 'length': 0.51,'width': 0.8,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+
+{ 'id': 'binBlue','type': 'BIN','pos': { 'x': -0.15,'y': 0.50,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+{ 'id': 'binRed','type': 'BIN','pos': { 'x': 0.20,'y': 0.50,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+
+{ 'id': 'objectRed1','type': 'BOX','pos': { 'x': 0.5,'y': -0.1,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+{ 'id': 'objectRed2','type': 'BOX','pos': { 'x': 0.25,'y': -0.2,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+{ 'id': 'objectRed3','type': 'BOX','pos': { 'x': 0.35, 'y' : 0.1,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+{ 'id': 'objectGreen1','type': 'BOX','pos': { 'x': 0.45,'y': -0.3,'z': 0.8105 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
+{ 'id': 'objectGreen2','type': 'BOX','pos': { 'x': 0.0,'y': -0.5,'z': 0.8105 },'size': {'length':0.031,'width':0.031,'height':0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
+{ 'id': 'objectGreen3','type': 'BOX','pos': { 'x': 0.1,'y': -0.3,'z': 0.819 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
+{ 'id': 'objectBlue1','type': 'BOX','pos': { 'x': 0.25,'y': -0.5,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+{ 'id': 'objectBlue2','type': 'BOX','pos': { 'x': -0.2,'y': -0.3,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+{ 'id': 'objectBlue3','type': 'BOX','pos': { 'x': 0.45,'y' : 0.1, 'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+
+{ 'id': 'arm','type': 'ARM','pos': { 'z': 0.75 , 'x' : -0.22},'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+] }
diff --git a/launch/ceti-table_robot_main.launch b/launch/ceti-table_robot_main.launch
new file mode 100644
index 0000000000000000000000000000000000000000..3c974b4e9e22531fe5c62a0ae691f7050fb25d7f
--- /dev/null
+++ b/launch/ceti-table_robot_main.launch
@@ -0,0 +1,19 @@
+<launch>
+
+ <arg name="robot_ip" default="172.31.1.13" />
+ <arg name="load_gripper" default="true" />
+ <arg name="connection_address" default="tcp://*:6576" />
+ <arg name="client_controllers" default="[]" />
+
+ <include file="$(find ccf)/launch/robot_setup.launch" >
+ <arg name="robot_ip" value="$(arg robot_ip)" />
+ <arg name="load_gripper" value="$(arg load_gripper)"/>
+ </include>
+
+ <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" >
+ <param name="connection_address" type="string" value="$(arg connection_address)" />
+ <param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
+ <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene_ceti-table.json" />
+ </node>
+
+</launch>
diff --git a/launch/ceti-table_robot_main_dummy_selector.launch b/launch/ceti-table_robot_main_dummy_selector.launch
new file mode 100644
index 0000000000000000000000000000000000000000..ae399802695a8b40d4fb8daa1ae7e7c88e65c2da
--- /dev/null
+++ b/launch/ceti-table_robot_main_dummy_selector.launch
@@ -0,0 +1,4 @@
+<launch>
+ <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
+ <include file="$(find ccf_immersive_sorting)/launch/ceti-table_robot_main.launch" />
+</launch>
diff --git a/launch/ceti-table_simulated_main.launch b/launch/ceti-table_simulated_main.launch
new file mode 100644
index 0000000000000000000000000000000000000000..d4607d6d126f0c0fa62d29d3fe4e7a5aabede939
--- /dev/null
+++ b/launch/ceti-table_simulated_main.launch
@@ -0,0 +1,14 @@
+<launch>
+
+ <arg name="connection_address" default="tcp://*:6576" />
+ <arg name="client_controllers" default="[]" />
+
+ <include file="$(find ccf)/launch/simulation_setup.launch"/>
+
+ <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" >
+ <param name="connection_address" type="string" value="$(arg connection_address)" />
+ <param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
+ <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene_ceti-table.json" />
+ </node>
+
+</launch>
diff --git a/launch/ceti-table_simulated_main_dummy_selector.launch b/launch/ceti-table_simulated_main_dummy_selector.launch
new file mode 100644
index 0000000000000000000000000000000000000000..6ce6439b0ecaef20e16328ce9c1c8d8e6a7c861a
--- /dev/null
+++ b/launch/ceti-table_simulated_main_dummy_selector.launch
@@ -0,0 +1,4 @@
+<launch>
+ <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
+ <include file="$(find ccf_immersive_sorting)/launch/ceti-table_simulated_main.launch" />
+</launch>
diff --git a/launch/dummy_main_external_client.launch b/launch/dummy_main_external_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..ba77e8d92c3991baf3641435520742288cfc0f60
--- /dev/null
+++ b/launch/dummy_main_external_client.launch
@@ -0,0 +1,7 @@
+<launch>
+ <node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="main_controller" output="screen">
+ <param name="connection_address" type="string" value="tcp://*:6576" />
+ <param name="client_controllers" type="yaml" value="['tcp://127.0.0.1:6586']" />
+ <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
+ </node>
+</launch>
diff --git a/launch/robot_main.launch b/launch/robot_main.launch
index 6e3d21c290c5f5c2d567c12b8da1cb54247cd7ff..a1fa771ad23771d4a1a12d3ee1a754ec3e3c8c5c 100644
--- a/launch/robot_main.launch
+++ b/launch/robot_main.launch
@@ -1,6 +1,7 @@
<launch>
<arg name="robot_ip" default="172.31.1.13" />
+ <!-- <arg name="robot_ip" default="172.16.0.2" /> -->
<arg name="load_gripper" default="true" />
<arg name="connection_address" default="tcp://*:6576" />
<arg name="client_controllers" default="[]" />
@@ -10,9 +11,10 @@
<arg name="load_gripper" value="$(arg load_gripper)"/>
</include>
- <node pkg="ccf" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" >
+ <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" >
<param name="connection_address" type="string" value="$(arg connection_address)" />
<param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
+ <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
</node>
-</launch>
\ No newline at end of file
+</launch>
diff --git a/launch/robot_main_dummy_selector.launch b/launch/robot_main_dummy_selector.launch
index 24b14d344db918b95ad13edf36e1a402d2cd4ffe..5d3b444e018d9940688aaa3c604e1c0c8680eb4c 100644
--- a/launch/robot_main_dummy_selector.launch
+++ b/launch/robot_main_dummy_selector.launch
@@ -1,4 +1,4 @@
<launch>
<node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
- <include file="robot_main.launch" />
+ <include file="$(find ccf_immersive_sorting)/launch/robot_main.launch" />
</launch>
diff --git a/launch/simulated_main.launch b/launch/simulated_main.launch
index f80a829a5fbea30411102d483807ae0a71997b55..afb6dc2ea73be5d433d28bcc7ee8a251c77d15fb 100644
--- a/launch/simulated_main.launch
+++ b/launch/simulated_main.launch
@@ -5,9 +5,10 @@
<include file="$(find ccf)/launch/simulation_setup.launch"/>
- <node pkg="ccf" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" >
+ <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" >
<param name="connection_address" type="string" value="$(arg connection_address)" />
<param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
+ <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
</node>
-</launch>
\ No newline at end of file
+</launch>
diff --git a/launch/simulated_main_dummy_client.launch b/launch/simulated_main_dummy_client.launch
index f2dc5a207b36b2495ad520fd42ae1304ce2474d8..f319995c1198783f1076baa5f7d4cccbfb16767a 100644
--- a/launch/simulated_main_dummy_client.launch
+++ b/launch/simulated_main_dummy_client.launch
@@ -4,9 +4,11 @@
<node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="main_controller" output="screen">
<param name="connection_address" type="string" value="tcp://*:6576" />
<param name="client_controllers" type="yaml" value="['tcp://127.0.0.1:6586']" />
+ <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
</node>
<node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="client_controller" output="screen">
<param name="connection_address" type="string" value="tcp://*:6586" />
<param name="client_controllers" type="yaml" value="[]" />
+ <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
</node>
</launch>
diff --git a/launch/simulated_main_dummy_selector.launch b/launch/simulated_main_dummy_selector.launch
index b28af5eac0c78e00e2733dcbf4fe5953d1bdcfb8..e845cbb02e0bcc54606007611766c029f9edfc0e 100644
--- a/launch/simulated_main_dummy_selector.launch
+++ b/launch/simulated_main_dummy_selector.launch
@@ -1,4 +1,4 @@
<launch>
<node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
- <include file="simulated_main.launch" />
+ <include file="$(find ccf_immersive_sorting)/launch/simulated_main.launch" />
</launch>
diff --git a/launch/simulated_main_external_client.launch b/launch/simulated_main_external_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..1a22b53a6cfafb74bd1a6213b730bf447bb23df4
--- /dev/null
+++ b/launch/simulated_main_external_client.launch
@@ -0,0 +1,9 @@
+<launch>
+ <include file="$(find ccf)/launch/simulation_setup.launch"/>
+
+ <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="main_controller" output="screen">
+ <param name="connection_address" type="string" value="tcp://*:6576" />
+ <param name="client_controllers" type="yaml" value="['tcp://127.0.0.1:6586']" />
+ <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
+ </node>
+</launch>
diff --git a/src/dummy_selection_provider.cpp b/src/dummy_selection_provider.cpp
index 5be7a623f5c01a8bb7eacf9bbc26bec69cce943a..85efe4b260fc406579deb2456980f701e83d2307 100644
--- a/src/dummy_selection_provider.cpp
+++ b/src/dummy_selection_provider.cpp
@@ -52,21 +52,17 @@ int main(int argc, char **argv) {
ros::NodeHandle n("ccf");
if ((rv = nng_pair1_open(&sock)) != 0) {
- ROS_ERROR_STREAM("[dummy_selection_provider] nng_pair1_open returned: " << nng_strerror(rv));
+ ROS_ERROR_STREAM("nng_pair1_open returned: " << nng_strerror(rv));
}
ros::Rate connection_retry_rate(1);
while ((rv = nng_dial(sock, URL.c_str(), nullptr, 0)) != 0) {
- ROS_WARN_STREAM(
- "[dummy_selection_provider] nng_dial returned: " << nng_strerror(rv)
- << ". Trying to connect again in one second...");
+ ROS_WARN_STREAM("nng_dial returned: " << nng_strerror(rv) << ". Trying to connect again in one second...");
connection_retry_rate.sleep();
}
- ROS_INFO_STREAM(
- "[dummy_selection_provider] nng_dial returned: " << nng_strerror(rv)
- << " (which is the translation of error code " << rv
- << "). Connection established!");
+ ROS_INFO_STREAM("nng_dial returned: " << nng_strerror(rv) << " (which is the translation of error code " << rv
+ << "). Connection established!");
ros::Rate loop_rate(200);
ros::Rate pause_rate(ros::Duration(2)); // seconds
@@ -87,23 +83,20 @@ int main(int argc, char **argv) {
std::string s;
if (google::protobuf::TextFormat::PrintToString(scene, &s)) {
- ROS_INFO_STREAM("[dummy_selection_provider] Received a valid scene with " << scene.objects().size()
- << " objects.");
- ROS_DEBUG_STREAM("[dummy_selection_provider] Content:" << std::endl << s);
+ ROS_INFO_STREAM("Received a valid scene with " << scene.objects().size() << " objects.");
+ ROS_DEBUG_STREAM("Content:" << std::endl << s);
} else {
- ROS_WARN_STREAM("[dummy_selection_provider] Received an invalid scene!" << std::endl
- << "[dummy_selection_provider] Partial content:"
- << std::endl
- << scene.ShortDebugString());
+ ROS_WARN_STREAM("Received an invalid scene!" << std::endl
+ << "[dummy_selection_provider] Partial content:"
+ << std::endl
+ << scene.ShortDebugString());
}
// collect all object names
std::vector<std::string> objects{};
std::vector<std::string> bins{};
- for (const auto &object : scene.objects()) {
- ROS_INFO_STREAM(
- "[dummy_selection_provider] found object " << object.id() << " of type "
- << Object_Type_Name(object.type()));
+ for (const auto &object: scene.objects()) {
+ ROS_DEBUG_STREAM("found object " << object.id() << " of type " << Object_Type_Name(object.type()));
if (object.type() == Object_Type_BOX) {
objects.push_back(object.id());
} else if (object.type() == Object_Type_BIN) {
@@ -112,9 +105,9 @@ int main(int argc, char **argv) {
}
if (objects.empty()) {
- ROS_INFO("[dummy_selection_provider] No objects to select in a scene without boxes!");
+ ROS_INFO("No objects to select in a scene without boxes!");
} else if (bins.empty()) {
- ROS_INFO("[dummy_selection_provider] No bin to put the object in available!");
+ ROS_INFO("No bin to put the object in available!");
} else {
// select a random object
std::uniform_int_distribution<u_long> distribution{0, objects.size() - 1};
@@ -123,7 +116,7 @@ int main(int argc, char **argv) {
pause_rate.sleep();
std::string object{objects[distribution(rng)]};
- ROS_INFO_STREAM("[dummy_selection_provider] Selecting random object: " << object);
+ ROS_INFO_STREAM("Selecting random object: " << object);
sendSelection(object);
// wait again, then send the bin object
@@ -134,14 +127,14 @@ int main(int argc, char **argv) {
std::uniform_int_distribution<u_long> binDistribution{0, bins.size() - 1};
std::string bin{bins[binDistribution(rng)]};
- ROS_INFO_STREAM("[dummy_selection_provider] Selecting random bin: " << bin);
+ ROS_INFO_STREAM("Selecting random bin: " << bin);
sendSelection(bin);
}
} else if (rv == NNG_EAGAIN) {
// no message received in current spin
} else {
- ROS_ERROR_STREAM("[dummy_selection_provider] nng_recv returned: " << nng_strerror(rv));
+ ROS_ERROR_STREAM("nng_recv returned: " << nng_strerror(rv));
}
ros::spinOnce();
diff --git a/src/dummy_sorting_controller.cpp b/src/dummy_sorting_controller.cpp
index c03e7d6d043c09da5bcae1b4a9b5fe3913fa4ac9..d1580aebb81fadc74332dc825eb70909f21f6a86 100644
--- a/src/dummy_sorting_controller.cpp
+++ b/src/dummy_sorting_controller.cpp
@@ -7,8 +7,6 @@
\date 07.07.20
*/
-#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
-
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/Empty.h>
@@ -45,16 +43,17 @@ int main(int argc, char **argv) {
auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
- ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to " << clientControllers.size() << " client controllers.");
- for (const auto &client : clientControllers) {
- ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to client at " << client << ".");
+ ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers.");
+ for (const auto &client: clientControllers) {
+ ROS_INFO_STREAM("Connecting to client at " << client << ".");
std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false);
client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection"));
client_connection->setReceiveTopic("client_scene");
connector.addConnection(std::move(client_connection));
}
- connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene.yaml"));
+ connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
+ "/config/config_scene.yaml"));
Object *robot = nullptr;
Object *selectedBox = nullptr;
@@ -74,38 +73,36 @@ int main(int argc, char **argv) {
selection.SerializeAsString());
if (currentlyPickedBox.has_value()) {
- ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress.");
+ ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
return;
}
- Object *object;
- try {
- object = connector.resolveObject(selection.id());
- } catch (std::out_of_range &e) {
- ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
+ Object *object = connector.resolveObject(selection.id());
+ if (!object) {
+ ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
return;
}
auto type = Object::Type_Name(object->type());
if (object->type() == Object::BOX) {
selectedBox = object;
- ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
} else if (object->type() == Object::BIN) {
selectedBin = object;
- ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
} else {
- ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
+ ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
}
if (selectedBin && selectedBox) {
auto boxId = selectedBox->id();
- ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
+ ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
currentlyPickedBox = boxId;
if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
- ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!");
+ ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
selectedBox = nullptr;
selectedBin = nullptr;
} else {
- ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more.");
+ ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
selectedBox = nullptr;
selectedBin = nullptr;
connector.sendScene();
@@ -116,12 +113,9 @@ int main(int argc, char **argv) {
auto sceneUpdateCallback = [¤tlyPickedBox, &connector]() {
if (currentlyPickedBox.has_value()) {
- try {
- connector.resolveObject(currentlyPickedBox.value());
- } catch (std::out_of_range &e) {
- ROS_INFO_STREAM(
- "[" << NODE_NAME << "] box " << currentlyPickedBox.value()
- << " has been removed from the scene!");
+ auto resolved = connector.resolveObject(currentlyPickedBox.value());
+ if (!resolved) {
+ ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
currentlyPickedBox.reset();
}
}
diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp
index 0d72c9bc2b8b3f9099f4346d0347493816044225..8e5d94886d5f4a13c21626de56d93bf7b9a6b403 100644
--- a/src/moveit_sorting_controller.cpp
+++ b/src/moveit_sorting_controller.cpp
@@ -7,9 +7,8 @@
\date 07.07.20
*/
-#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
-
#include <ros/ros.h>
+#include <ros/package.h>
#include <std_msgs/Empty.h>
#include "connector.pb.h"
@@ -44,16 +43,19 @@ int main(int argc, char **argv) {
auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
- ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to " << clientControllers.size() << " client controllers.");
- for (const auto &client : clientControllers) {
- ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to client at " << client << ".");
+ ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers.");
+ for (const auto &client: clientControllers) {
+ ROS_INFO_STREAM("Connecting to client at " << client << ".");
std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false);
client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection"));
client_connection->setReceiveTopic("client_scene");
connector.addConnection(std::move(client_connection));
}
- // scene loading is not required, the scene is updated by moveit
+ ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
+
+ connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
+ "/config/config_scene.yaml"));
Object *robot = nullptr;
Object *selectedBox = nullptr;
@@ -73,38 +75,36 @@ int main(int argc, char **argv) {
selection.SerializeAsString());
if (currentlyPickedBox.has_value()) {
- ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress.");
+ ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
return;
}
- Object *object;
- try {
- object = connector.resolveObject(selection.id());
- } catch (std::out_of_range &e) {
- ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
+ Object *object = connector.resolveObject(selection.id());
+ if (!object) {
+ ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
return;
}
auto type = Object::Type_Name(object->type());
if (object->type() == Object::BOX) {
selectedBox = object;
- ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
} else if (object->type() == Object::BIN) {
selectedBin = object;
- ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
} else {
- ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
+ ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
}
if (selectedBin && selectedBox) {
auto boxId = selectedBox->id();
- ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
+ ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
currentlyPickedBox = boxId;
if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
- ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!");
+ ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
selectedBox = nullptr;
selectedBin = nullptr;
} else {
- ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more.");
+ ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
selectedBox = nullptr;
selectedBin = nullptr;
connector.sendScene();
@@ -115,12 +115,9 @@ int main(int argc, char **argv) {
auto sceneUpdateCallback = [¤tlyPickedBox, &connector]() {
if (currentlyPickedBox.has_value()) {
- try {
- connector.resolveObject(currentlyPickedBox.value());
- } catch (std::out_of_range &e) {
- ROS_INFO_STREAM(
- "[" << NODE_NAME << "] box " << currentlyPickedBox.value()
- << " has been removed from the scene!");
+ auto resolved = connector.resolveObject(currentlyPickedBox.value());
+ if (!resolved) {
+ ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
currentlyPickedBox.reset();
}
}