diff --git a/config/config_scene_dineros-left.json b/config/config_scene_dineros-left.json index 67da4f29ff8486f13d9365ed527ddbffe321e978..0c239f43206058c678bb236c4c56bfe89387a7bf 100644 --- a/config/config_scene_dineros-left.json +++ b/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", diff --git a/config/config_scene_dineros-left.yaml b/config/config_scene_dineros-left.yaml index 5bc921d40d9376955620354fa856fb64cbfa30d8..c4731a312d35e104313726dac61cd143719204ac 100644 --- a/config/config_scene_dineros-left.yaml +++ b/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 } } ] } diff --git a/config/config_scene_dineros-right.json b/config/config_scene_dineros-right.json index 8a9c69c828b2b80f3fdd1f30ad2dff8479573815..8add43011d2ba6b2230f9988a15cb2306ac64ee4 100644 --- a/config/config_scene_dineros-right.json +++ b/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" } }, { diff --git a/config/config_scene_dineros-right.yaml b/config/config_scene_dineros-right.yaml index 7d09da0a4422d1a94bff9145d3d58e5f47d3eb84..943627a303fbc451ce262439cab26b028a50efd7 100644 --- a/config/config_scene_dineros-right.yaml +++ b/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 } } ] } diff --git a/launch/cell-controllers/noop-sim-cell_dineros-right.launch b/launch/cell-controllers/noop-sim-cell_dineros-right.launch new file mode 100644 index 0000000000000000000000000000000000000000..b968eed498c50756cc050fd57cec220d23d3233a --- /dev/null +++ b/launch/cell-controllers/noop-sim-cell_dineros-right.launch @@ -0,0 +1,19 @@ +<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>