From 3e24e87e5ef06250d7cafc8ae362ba750976a5e8 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Thu, 16 Dec 2021 10:48:46 +0100
Subject: [PATCH] update config and launch files
---
config/config_scene_ads-table.json | 386 +++++++++++++++++++++++++
config/config_scene_ads-table.yaml | 44 +++
config/config_scene_ads.yaml | 19 --
config/config_scene_ceti-table.json | 2 +-
config/config_scene_ceti-table.yaml | 2 +-
config/config_scene_st-table.json | 430 ++++++++++++++++++++++++++++
config/config_scene_st-table.yaml | 43 +++
config/config_scene_st.yaml | 19 --
launch/ads-table_robot_main.launch | 19 ++
launch/st-table_robot_main.launch | 19 ++
10 files changed, 943 insertions(+), 40 deletions(-)
create mode 100644 config/config_scene_ads-table.json
create mode 100644 config/config_scene_ads-table.yaml
delete mode 100644 config/config_scene_ads.yaml
create mode 100644 config/config_scene_st-table.json
create mode 100644 config/config_scene_st-table.yaml
delete mode 100644 config/config_scene_st.yaml
create mode 100644 launch/ads-table_robot_main.launch
create mode 100644 launch/st-table_robot_main.launch
diff --git a/config/config_scene_ads-table.json b/config/config_scene_ads-table.json
new file mode 100644
index 0000000..52fc26e
--- /dev/null
+++ b/config/config_scene_ads-table.json
@@ -0,0 +1,386 @@
+{
+ "objects": [
+ {
+ "id": "leg_1",
+ "pos": {
+ "x": 0.426,
+ "y": 0.83,
+ "z": 0.327
+ },
+ "size": {
+ "length": 0.04,
+ "width": 0.04,
+ "height": 0.634
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "leg_2",
+ "pos": {
+ "x": -0.426,
+ "y": -0.83,
+ "z": 0.327
+ },
+ "size": {
+ "length": 0.04,
+ "width": 0.04,
+ "height": 0.634
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "leg_3",
+ "pos": {
+ "x": -0.426,
+ "y": 0.83,
+ "z": 0.327
+ },
+ "size": {
+ "length": 0.04,
+ "width": 0.04,
+ "height": 0.634
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "leg_4",
+ "pos": {
+ "x": 0.426,
+ "y": -0.83,
+ "z": 0.327
+ },
+ "size": {
+ "length": 0.04,
+ "width": 0.04,
+ "height": 0.634
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "table-top",
+ "pos": {
+ "z": 0.668
+ },
+ "size": {
+ "length": 0.892,
+ "width": 1.7,
+ "height": 0.068
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "front_bar",
+ "pos": {
+ "z": 0.257,
+ "x": 0.426
+ },
+ "size": {
+ "length": 0.04,
+ "width": 1.62,
+ "height": 0.04
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "back_bar",
+ "pos": {
+ "z": 0.257,
+ "x": -0.426
+ },
+ "size": {
+ "length": 0.04,
+ "width": 1.62,
+ "height": 0.04
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "left_bar",
+ "pos": {
+ "z": 0.257,
+ "y": -0.83
+ },
+ "size": {
+ "length": 0.892,
+ "width": 0.04,
+ "height": 0.04
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "right_bar",
+ "pos": {
+ "z": 0.257,
+ "y": 0.83
+ },
+ "size": {
+ "length": 0.892,
+ "width": 0.04,
+ "height": 0.04
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "binBlue",
+ "type": "BIN",
+ "pos": {
+ "x": 0.19,
+ "y": -0.6,
+ "z": 0.772
+ },
+ "size": {
+ "length": 0.2,
+ "width": 0.28,
+ "height": 0.14
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "b": 1
+ }
+ },
+ {
+ "id": "binRed",
+ "type": "BIN",
+ "pos": {
+ "x": -0.23,
+ "y": -0.4,
+ "z": 0.772
+ },
+ "size": {
+ "length": 0.2,
+ "width": 0.28,
+ "height": 0.14
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 1
+ }
+ },
+ {
+ "id": "binGreen",
+ "type": "BIN",
+ "pos": {
+ "x": -0.02,
+ "y": -0.5,
+ "z": 0.772
+ },
+ "size": {
+ "length": 0.2,
+ "width": 0.28,
+ "height": 0.14
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "g": 1
+ }
+ },
+ {
+ "id": "bigYellow",
+ "type": "BOX",
+ "pos": {
+ "x": 0.15,
+ "y": 0.4,
+ "z": 0.7331
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.0622
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 1,
+ "g": 1
+ }
+ },
+ {
+ "id": "bigRed",
+ "type": "BOX",
+ "pos": {
+ "x": 0.3,
+ "y": 0,
+ "z": 0.7427
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.0814
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 1
+ }
+ },
+ {
+ "id": "bigGreen",
+ "type": "BOX",
+ "pos": {
+ "x": 0.15,
+ "y": 0,
+ "z": 0.7475
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.091
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "g": 1
+ }
+ },
+ {
+ "id": "bigBlue",
+ "type": "BOX",
+ "pos": {
+ "x": -0.05,
+ "y": 0.4,
+ "z": 0.7475
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.091
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "b": 1
+ }
+ },
+ {
+ "id": "smallYellow",
+ "type": "BOX",
+ "pos": {
+ "x": 0.15,
+ "y": -0.2,
+ "z": 0.7427
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0318,
+ "height": 0.0814
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 1,
+ "g": 1
+ }
+ },
+ {
+ "id": "smallBlue",
+ "type": "BOX",
+ "pos": {
+ "x": 0.15,
+ "y": 0.2,
+ "z": 0.7427
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0318,
+ "height": 0.0814
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "b": 1
+ }
+ },
+ {
+ "id": "arm",
+ "type": "ARM",
+ "pos": {
+ "z": 0.702,
+ "x": -0.22
+ },
+ "size": {},
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 1,
+ "g": 1,
+ "b": 1
+ }
+ }
+ ]
+}
diff --git a/config/config_scene_ads-table.yaml b/config/config_scene_ads-table.yaml
new file mode 100644
index 0000000..52bbe26
--- /dev/null
+++ b/config/config_scene_ads-table.yaml
@@ -0,0 +1,44 @@
+# create json file using
+# yq eval -o=json config_scene_ads-table.yaml > config_scene_ads-table.json
+{ 'objects': [
+ # table
+ # height: 63.4 (legs) + 6.8 (top) = 70.2 cm total
+ # legs width/depth/height 4x4x63.4
+ # table-top width/depth/height 170x89.2x6.8 (169.9 too fuzzy to model)
+ # 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': 'leg_1','pos': { 'x': 0.426,'y': 0.83,'z': 0.327 },'size': { 'length': 0.04,'width': 0.04,'height': 0.634 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'leg_2','pos': { 'x': -0.426,'y': -0.83,'z': 0.327 },'size': { 'length': 0.04,'width': 0.04,'height': 0.634 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'leg_3','pos': { 'x': -0.426,'y': 0.83,'z': 0.327 },'size': { 'length': 0.04,'width': 0.04,'height': 0.634 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'leg_4','pos': { 'x': 0.426,'y': -0.83,'z': 0.327 },'size': { 'length': 0.04,'width': 0.04,'height': 0.634 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table-top', 'pos': { 'z': 0.668 },'size': { 'length': 0.892,'width': 1.7,'height': 0.068 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'front_bar', 'pos': { 'z': 0.257, 'x': 0.426 },'size': { 'length': 0.04,'width': 1.62,'height': 0.04 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'back_bar', 'pos': { 'z': 0.257, 'x': -0.426 },'size': { 'length': 0.04,'width': 1.62,'height': 0.04 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'left_bar', 'pos': { 'z': 0.257, 'y': -0.83 },'size': { 'length': 0.892,'width': 0.04,'height': 0.04 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'right_bar', 'pos': { 'z': 0.257, 'y': 0.83 },'size': { 'length': 0.892,'width': 0.04,'height': 0.04 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+
+
+ # 12 cm from side edges of the table:
+ # x: edge -45 + halfsize 10 + distance 12 = -23 // second + distance 1 + fullsize 20 = -2 // third = 19
+ # y: edge -85 + halfsize 14 + distance 12 = -59 // second + distance 1 + fullsize 28 = -30
+ { 'id': 'binBlue', 'type': 'BIN','pos': { 'x': 0.19,'y': -0.60,'z': 0.772 },'size': { 'length': 0.20,'width': 0.28,'height': 0.14 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+ { 'id': 'binRed', 'type': 'BIN','pos': { 'x': -0.23,'y': -0.40,'z': 0.772 },'size': { 'length': 0.20,'width': 0.28,'height': 0.14 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+ { 'id': 'binGreen','type': 'BIN','pos': { 'x': -0.02,'y': -0.50,'z': 0.772 },'size': { 'length': 0.20,'width': 0.28,'height': 0.14 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+
+ # 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+7160)/10000, (i*96+46)/10000));
+
+ { 'id': 'bigYellow','type': 'BOX','pos': { 'x': 0.15,'y': 0.400000,'z': 0.733100 },'size': { 'length': .0318, 'width': .0636,'height': 0.062200 },'orientation': { 'w': 1 },'color': {r: 1, g: 1} },
+ { 'id': 'bigRed', 'type': 'BOX','pos': { 'x': 0.30,'y': 0.000000,'z': 0.742700 },'size': { 'length': .0318, 'width': .0636,'height': 0.081400 },'orientation': { 'w': 1 },'color': {r: 1} },
+ { 'id': 'bigGreen', 'type': 'BOX','pos': { 'x': 0.15,'y': 0.000000,'z': 0.747500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': {g: 1} },
+ { 'id': 'bigBlue', 'type': 'BOX','pos': { 'x': -0.05,'y': 0.400000,'z': 0.747500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': {b: 1} },
+
+
+
+ # for i in range(3,10): print("{ 'id': 'object2x2x%.1f','type': 'BOX','pos': { 'x': 0.5,'y': %f,'z': %f },'size': { 'length': .0318, 'width': .0318,'height': %f },'orientation': { 'w': 1 },'color': {} }," % (i/2, (i-6)*.1, (i*48+23+7020)/10000, (i*96+46)/10000));
+ { 'id': 'smallYellow','type': 'BOX','pos': { 'x': 0.15,'y': -0.200000,'z': 0.742700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': {r: 1, g: 1} },
+ { 'id': 'smallBlue', 'type': 'BOX','pos': { 'x': 0.15,'y': 0.200000,'z': 0.742700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': {b: 1} },
+
+
+ { 'id': 'arm','type': 'ARM','pos': { 'z': 0.702 , 'x': -0.22 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+] }
diff --git a/config/config_scene_ads.yaml b/config/config_scene_ads.yaml
deleted file mode 100644
index fcd2f25..0000000
--- a/config/config_scene_ads.yaml
+++ /dev/null
@@ -1,19 +0,0 @@
-{ '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
index 0153a59..46964b4 100644
--- a/config/config_scene_ceti-table.json
+++ b/config/config_scene_ceti-table.json
@@ -300,7 +300,7 @@
"w": 1
},
"color": {
- "r": 1
+ "g": 1
}
},
{
diff --git a/config/config_scene_ceti-table.yaml b/config/config_scene_ceti-table.yaml
index 3048dc2..44a984c 100644
--- a/config/config_scene_ceti-table.yaml
+++ b/config/config_scene_ceti-table.yaml
@@ -31,7 +31,7 @@
# 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 } },
+ { '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': { 'g': 1 } },
{ 'id': 'object4x2x5.0_Y','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} },
diff --git a/config/config_scene_st-table.json b/config/config_scene_st-table.json
new file mode 100644
index 0000000..a479512
--- /dev/null
+++ b/config/config_scene_st-table.json
@@ -0,0 +1,430 @@
+{
+ "objects": [
+ {
+ "id": "wheel_1",
+ "pos": {
+ "x": -0.148,
+ "y": 0.67,
+ "z": 0.0655
+ },
+ "size": {
+ "length": 0.18,
+ "width": 0.18,
+ "height": 0.131
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "wheel_2",
+ "pos": {
+ "x": -0.148,
+ "y": -0.67,
+ "z": 0.0655
+ },
+ "size": {
+ "length": 0.18,
+ "width": 0.18,
+ "height": 0.131
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "wheel_3",
+ "pos": {
+ "x": 0.554,
+ "y": 0.67,
+ "z": 0.0655
+ },
+ "size": {
+ "length": 0.18,
+ "width": 0.18,
+ "height": 0.131
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "wheel_4",
+ "pos": {
+ "x": 0.554,
+ "y": -0.67,
+ "z": 0.0655
+ },
+ "size": {
+ "length": 0.18,
+ "width": 0.18,
+ "height": 0.131
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "leg_1",
+ "pos": {
+ "x": -0.225,
+ "y": 0.83,
+ "z": 0.39025
+ },
+ "size": {
+ "length": 0.04,
+ "width": 0.04,
+ "height": 0.5185
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "leg_2",
+ "pos": {
+ "x": -0.225,
+ "y": -0.83,
+ "z": 0.39025
+ },
+ "size": {
+ "length": 0.04,
+ "width": 0.04,
+ "height": 0.5185
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "leg_3",
+ "pos": {
+ "x": 0.633,
+ "y": 0.83,
+ "z": 0.39025
+ },
+ "size": {
+ "length": 0.04,
+ "width": 0.04,
+ "height": 0.5185
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "leg_4",
+ "pos": {
+ "x": 0.633,
+ "y": -0.83,
+ "z": 0.39025
+ },
+ "size": {
+ "length": 0.04,
+ "width": 0.04,
+ "height": 0.5185
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "table_lower_shelf",
+ "pos": {
+ "x": 0.203,
+ "z": 0.151
+ },
+ "size": {
+ "length": 0.9,
+ "width": 1.7,
+ "height": 0.04
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "table_mid_shelf",
+ "pos": {
+ "x": 0.203,
+ "z": 0.333
+ },
+ "size": {
+ "length": 0.9,
+ "width": 1.7,
+ "height": 0.04
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "table_top",
+ "pos": {
+ "x": 0.203,
+ "z": 0.68275
+ },
+ "size": {
+ "length": 0.9,
+ "width": 1.7,
+ "height": 0.0665
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.15,
+ "g": 0.15,
+ "b": 0.15
+ }
+ },
+ {
+ "id": "binBlue",
+ "type": "BIN",
+ "pos": {
+ "x": -0,
+ "y": -0.47,
+ "z": 0.7685
+ },
+ "size": {
+ "length": 0.17,
+ "width": 0.24,
+ "height": 0.105
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "b": 0.8
+ }
+ },
+ {
+ "id": "binRed",
+ "type": "BIN",
+ "pos": {
+ "x": 0.18,
+ "y": -0.47,
+ "z": 0.7685
+ },
+ "size": {
+ "length": 0.17,
+ "width": 0.24,
+ "height": 0.105
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 0.8
+ }
+ },
+ {
+ "id": "binGreen",
+ "type": "BIN",
+ "pos": {
+ "x": 0.36,
+ "y": -0.47,
+ "z": 0.7685
+ },
+ "size": {
+ "length": 0.17,
+ "width": 0.24,
+ "height": 0.105
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "g": 0.8
+ }
+ },
+ {
+ "id": "object4x2x4.5_r",
+ "type": "BOX",
+ "pos": {
+ "x": 0.3,
+ "y": 0.3,
+ "z": 0.7615
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.091
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 1
+ }
+ },
+ {
+ "id": "object4x2x5.0_y",
+ "type": "BOX",
+ "pos": {
+ "x": 0.3,
+ "y": 0,
+ "z": 0.7663
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.1006
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 1,
+ "g": 1
+ }
+ },
+ {
+ "id": "object4x2x5.0_b",
+ "type": "BOX",
+ "pos": {
+ "x": 0.3,
+ "y": 0.1,
+ "z": 0.7663
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.1006
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "b": 1
+ }
+ },
+ {
+ "id": "object4x2x5.0_g",
+ "type": "BOX",
+ "pos": {
+ "x": 0.3,
+ "y": 0.2,
+ "z": 0.7663
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0636,
+ "height": 0.1006
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "g": 1
+ }
+ },
+ {
+ "id": "object2x2x4.0_r",
+ "type": "BOX",
+ "pos": {
+ "x": 0.5,
+ "y": -0.1,
+ "z": 0.7567
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0318,
+ "height": 0.0814
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 1
+ }
+ },
+ {
+ "id": "object2x2x4.0_y",
+ "type": "BOX",
+ "pos": {
+ "x": 0.5,
+ "y": 0,
+ "z": 0.7567
+ },
+ "size": {
+ "length": 0.0318,
+ "width": 0.0318,
+ "height": 0.0814
+ },
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 1,
+ "g": 1
+ }
+ },
+ {
+ "id": "arm",
+ "type": "ARM",
+ "pos": {
+ "z": 0.716
+ },
+ "size": {},
+ "orientation": {
+ "w": 1
+ },
+ "color": {
+ "r": 1,
+ "g": 1,
+ "b": 1
+ }
+ }
+ ]
+}
diff --git a/config/config_scene_st-table.yaml b/config/config_scene_st-table.yaml
new file mode 100644
index 0000000..ed16e15
--- /dev/null
+++ b/config/config_scene_st-table.yaml
@@ -0,0 +1,43 @@
+# create json file using
+# yq eval -o=json config_scene_st-table.yaml > config_scene_st-table.json
+{ 'objects': [
+
+ # table height = 13.1 + 58.5 = 71.6
+ { 'id': 'wheel_1','pos': { 'x': -0.148,'y': 0.67,'z': 0.0655 },'size': { 'length': 0.18,'width': 0.18,'height': 0.131 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'wheel_2','pos': { 'x': -0.148,'y': -0.67,'z': 0.0655 },'size': { 'length': 0.18,'width': 0.18,'height': 0.131 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'wheel_3','pos': { 'x': 0.554,'y': 0.67,'z': 0.0655 },'size': { 'length': 0.18,'width': 0.18,'height': 0.131 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'wheel_4','pos': { 'x': 0.554,'y': -0.67,'z': 0.0655 },'size': { 'length': 0.18,'width': 0.18,'height': 0.131 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+
+ # leg z:
+ { 'id': 'leg_1','pos': { 'x': -0.225,'y': 0.83,'z': 0.39025 },'size': { 'length': 0.04,'width': 0.04,'height': 0.5185 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'leg_2','pos': { 'x': -0.225,'y': -0.83,'z': 0.39025 },'size': { 'length': 0.04,'width': 0.04,'height': 0.5185 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'leg_3','pos': { 'x': 0.633,'y': 0.83,'z': 0.39025 },'size': { 'length': 0.04,'width': 0.04,'height': 0.5185 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'leg_4','pos': { 'x': 0.633,'y': -0.83,'z': 0.39025 },'size': { 'length': 0.04,'width': 0.04,'height': 0.5185 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+
+ # top z pos = 71.6 - (6.65)/2 = 71.6 - 3.325 = 68.275
+ { 'id': 'table_lower_shelf','pos': {'x': 0.203, 'z': 0.151 },'size': { 'length': 0.9,'width': 1.7,'height': 0.04 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table_mid_shelf','pos': {'x': 0.203, 'z': 0.333 },'size': { 'length': 0.9,'width': 1.7,'height': 0.04 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table_top','pos': {'x': 0.203, 'z': 0.68275 },'size': { 'length': 0.9,'width': 1.7,'height': 0.0665 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+
+ # zpos = 71.6 + 5.25 = 76.85
+ # most behind outer corner at: 85-47-12 = 20cm
+ { 'id': 'binBlue', 'type': 'BIN','pos': { 'x': -0.00,'y': -0.47,'z': 0.7685 },'size': { 'length': 0.17,'width': 0.24,'height': 0.105 },'orientation': { 'w': 1 },'color': { 'b': 0.8 } },
+ { 'id': 'binRed', 'type': 'BIN','pos': { 'x': 0.18,'y': -0.47,'z': 0.7685 },'size': { 'length': 0.17,'width': 0.24,'height': 0.105 },'orientation': { 'w': 1 },'color': { 'r': 0.8 } },
+ { 'id': 'binGreen', 'type': 'BIN','pos': { 'x': 0.36,'y': -0.47,'z': 0.7685 },'size': { 'length': 0.17,'width': 0.24,'height': 0.105 },'orientation': { 'w': 1 },'color': { 'g': 0.8 } },
+
+ # 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+7160)/10000, (i*96+46)/10000));
+
+{ 'id': 'object4x2x4.5_r','type': 'BOX','pos': { 'x': 0.3,'y': 0.300000,'z': 0.761500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': {r: 1} },
+
+ { 'id': 'object4x2x5.0_y','type': 'BOX','pos': { 'x': 0.3,'y': 0.000000,'z': 0.766300 },'size': { 'length': .0318, 'width': .0636,'height': 0.100600 },'orientation': { 'w': 1 },'color': {r: 1, g: 1} },
+ { 'id': 'object4x2x5.0_b','type': 'BOX','pos': { 'x': 0.3,'y': 0.100000,'z': 0.766300 },'size': { 'length': .0318, 'width': .0636,'height': 0.100600 },'orientation': { 'w': 1 },'color': {b: 1} },
+ { 'id': 'object4x2x5.0_g','type': 'BOX','pos': { 'x': 0.3,'y': 0.200000,'z': 0.766300 },'size': { 'length': .0318, 'width': .0636,'height': 0.100600 },'orientation': { 'w': 1 },'color': {g: 1} },
+
+
+# for i in range(3,10): print("{ 'id': 'object2x2x%.1f','type': 'BOX','pos': { 'x': 0.5,'y': %f,'z': %f },'size': { 'length': .0318, 'width': .0318,'height': %f },'orientation': { 'w': 1 },'color': {} }," % (i/2, (i-6)*.1, (i*48+23+7160)/10000, (i*96+46)/10000));
+ { 'id': 'object2x2x4.0_r','type': 'BOX','pos': { 'x': 0.5,'y': -0.100000,'z': 0.756700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { r: 1} },
+ { 'id': 'object2x2x4.0_y','type': 'BOX','pos': { 'x': 0.5,'y': 0.00000,'z': 0.756700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { r: 1, g: 1} },
+
+
+ { 'id': 'arm','type': 'ARM','pos': { 'z': 0.716 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+] }
\ No newline at end of file
diff --git a/config/config_scene_st.yaml b/config/config_scene_st.yaml
deleted file mode 100644
index b6e5270..0000000
--- a/config/config_scene_st.yaml
+++ /dev/null
@@ -1,19 +0,0 @@
-{ '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/launch/ads-table_robot_main.launch b/launch/ads-table_robot_main.launch
new file mode 100644
index 0000000..af39594
--- /dev/null
+++ b/launch/ads-table_robot_main.launch
@@ -0,0 +1,19 @@
+<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="[]" />
+
+ <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_ads-table.json" />
+ </node>
+
+</launch>
diff --git a/launch/st-table_robot_main.launch b/launch/st-table_robot_main.launch
new file mode 100644
index 0000000..0f2b318
--- /dev/null
+++ b/launch/st-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_st-table.json" />
+ </node>
+
+</launch>
--
GitLab