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>