From 9e604dcd9fdc332e7501209c6d579e0ab3753bf6 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Fri, 25 Nov 2022 06:35:34 +0100
Subject: [PATCH] some regex

---
 .../profiles/default/devel_collisions.txt     |   2 +-
 src/.vscode/c_cpp_properties.json             |  48 +--
 src/mtc/CMakeLists.txt                        |  11 +
 src/mtc/include/impl/abstract_mediator.h      |   6 +-
 src/mtc/include/impl/mediator.h               |   2 +-
 src/mtc/include/impl/moveit_mediator.h        |   2 +-
 .../include/reader/abstract_param_reader.h    |   7 +-
 src/mtc/include/reader/robot_reader.h         |  23 ++
 src/mtc/include/reader/wing_reader.h          |  23 ++
 src/mtc/launch/cell_routine.launch            |   2 +-
 .../{1543924248.yaml => -1003092792.yaml}     |   0
 .../{1601455593.yaml => -1010460506.yaml}     |   0
 .../{1587832961.yaml => -1024001513.yaml}     |   0
 .../{1575731070.yaml => -1036064907.yaml}     |   0
 .../{-1890202883.yaml => -211114391.yaml}     |   0
 .../{-1901373430.yaml => -222544024.yaml}     |   0
 .../{-1951250192.yaml => -258879761.yaml}     |   0
 .../{-1962818280.yaml => -270419468.yaml}     |   0
 .../{1850935177.yaml => -652201118.yaml}      |   0
 .../{1895656920.yaml => -696648853.yaml}      |   0
 .../{1821310512.yaml => -742232139.yaml}      |   0
 .../{1807829100.yaml => -755774193.yaml}      |   0
 .../{1795913128.yaml => -767674851.yaml}      |   0
 .../{1868843792.yaml => -789859288.yaml}      |   0
 .../{1855511004.yaml => -803422225.yaml}      |   0
 .../{1843511501.yaml => -815321487.yaml}      |   0
 .../{1656942183.yaml => -846087811.yaml}      |   0
 .../{1711393564.yaml => -852264234.yaml}      |   0
 .../{1697794188.yaml => -865767525.yaml}      |   0
 .../{1685895957.yaml => -877673981.yaml}      |   0
 .../{1701273998.yaml => -890048247.yaml}      |   0
 .../{1759072325.yaml => -899976593.yaml}      |   0
 .../{1745566468.yaml => -914020264.yaml}      |   0
 .../{1733626611.yaml => -925946624.yaml}      |   0
 .../{1567654444.yaml => -934521686.yaml}      |   0
 .../{1555239444.yaml => -947074197.yaml}      |   0
 .../{1588704657.yaml => -958226134.yaml}      |   0
 .../{1648958842.yaml => -962936586.yaml}      |   0
 .../{1635476033.yaml => -976487161.yaml}      |   0
 .../{1612469426.yaml => -979308652.yaml}      |   0
 .../{1623703517.yaml => -988375807.yaml}      |   0
 .../{1599871509.yaml => -991922905.yaml}      |   0
 .../{-542667557.yaml => 1183415122.yaml}      |   0
 .../{-587344676.yaml => 1227695304.yaml}      |   0
 .../{-453685788.yaml => 1272105605.yaml}      |   0
 .../{-442405442.yaml => 1283302731.yaml}      |   0
 .../{-429778748.yaml => 1295853078.yaml}      |   0
 .../{-498149190.yaml => 1316801078.yaml}      |   0
 .../{-487005315.yaml => 1328208922.yaml}      |   0
 .../{-474510554.yaml => 1340801804.yaml}      |   0
 .../{-1564063432.yaml => 144778443.yaml}      |   0
 .../{-1553074118.yaml => 155931149.yaml}      |   0
 .../{-1540350554.yaml => 168492950.yaml}      |   0
 .../{-1608866404.yaml => 189371734.yaml}      |   0
 .../{-1597568458.yaml => 203146650.yaml}      |   0
 .../{-1585194105.yaml => 215783882.yaml}      |   0
 .../{-1358218541.yaml => 254319817.yaml}      |   0
 .../{-1405464563.yaml => 301530140.yaml}      |   0
 .../dummy/{-1572377916.yaml => 40651904.yaml} |   0
 .../{-1180235424.yaml => 434875766.yaml}      |   0
 .../{-1227713112.yaml => 482378175.yaml}      |   0
 .../{-1060422961.yaml => 560822576.yaml}      |   0
 .../{-1107549484.yaml => 608106095.yaml}      |   0
 .../{-1006227179.yaml => 702701396.yaml}      |   0
 .../{-1039705736.yaml => 714102535.yaml}      |   0
 .../{-1027180942.yaml => 726842647.yaml}      |   0
 .../{-1051277393.yaml => 748047042.yaml}      |   0
 .../dummy/{-995076880.yaml => 759537023.yaml} |   0
 .../dummy/{-982538817.yaml => 772496165.yaml} |   0
 .../dummy/{-772783050.yaml => 832741068.yaml} |   0
 .../dummy/{-1619843661.yaml => 88011482.yaml} |   0
 .../dummy/{-827862775.yaml => 880346008.yaml} |   0
 src/mtc/src/base_routine.cpp                  | 342 +-----------------
 src/mtc/src/cell_routine.cpp                  | 306 ++--------------
 src/mtc/src/impl/mediator.cpp                 |   6 +-
 src/mtc/src/impl/moveit_mediator.cpp          |   8 +-
 src/mtc/src/mtc2taskspace.cpp                 |   5 +-
 src/mtc/src/reader/robot_reader.cpp           |  43 +++
 src/mtc/src/reader/task_space_reader.cpp      |   2 +-
 src/mtc/src/reader/wing_reader.cpp            | 133 +++++++
 80 files changed, 322 insertions(+), 649 deletions(-)
 create mode 100644 src/mtc/include/reader/robot_reader.h
 create mode 100644 src/mtc/include/reader/wing_reader.h
 rename src/mtc/results/dummy/{1543924248.yaml => -1003092792.yaml} (100%)
 rename src/mtc/results/dummy/{1601455593.yaml => -1010460506.yaml} (100%)
 rename src/mtc/results/dummy/{1587832961.yaml => -1024001513.yaml} (100%)
 rename src/mtc/results/dummy/{1575731070.yaml => -1036064907.yaml} (100%)
 rename src/mtc/results/dummy/{-1890202883.yaml => -211114391.yaml} (100%)
 rename src/mtc/results/dummy/{-1901373430.yaml => -222544024.yaml} (100%)
 rename src/mtc/results/dummy/{-1951250192.yaml => -258879761.yaml} (100%)
 rename src/mtc/results/dummy/{-1962818280.yaml => -270419468.yaml} (100%)
 rename src/mtc/results/dummy/{1850935177.yaml => -652201118.yaml} (100%)
 rename src/mtc/results/dummy/{1895656920.yaml => -696648853.yaml} (100%)
 rename src/mtc/results/dummy/{1821310512.yaml => -742232139.yaml} (100%)
 rename src/mtc/results/dummy/{1807829100.yaml => -755774193.yaml} (100%)
 rename src/mtc/results/dummy/{1795913128.yaml => -767674851.yaml} (100%)
 rename src/mtc/results/dummy/{1868843792.yaml => -789859288.yaml} (100%)
 rename src/mtc/results/dummy/{1855511004.yaml => -803422225.yaml} (100%)
 rename src/mtc/results/dummy/{1843511501.yaml => -815321487.yaml} (100%)
 rename src/mtc/results/dummy/{1656942183.yaml => -846087811.yaml} (100%)
 rename src/mtc/results/dummy/{1711393564.yaml => -852264234.yaml} (100%)
 rename src/mtc/results/dummy/{1697794188.yaml => -865767525.yaml} (100%)
 rename src/mtc/results/dummy/{1685895957.yaml => -877673981.yaml} (100%)
 rename src/mtc/results/dummy/{1701273998.yaml => -890048247.yaml} (100%)
 rename src/mtc/results/dummy/{1759072325.yaml => -899976593.yaml} (100%)
 rename src/mtc/results/dummy/{1745566468.yaml => -914020264.yaml} (100%)
 rename src/mtc/results/dummy/{1733626611.yaml => -925946624.yaml} (100%)
 rename src/mtc/results/dummy/{1567654444.yaml => -934521686.yaml} (100%)
 rename src/mtc/results/dummy/{1555239444.yaml => -947074197.yaml} (100%)
 rename src/mtc/results/dummy/{1588704657.yaml => -958226134.yaml} (100%)
 rename src/mtc/results/dummy/{1648958842.yaml => -962936586.yaml} (100%)
 rename src/mtc/results/dummy/{1635476033.yaml => -976487161.yaml} (100%)
 rename src/mtc/results/dummy/{1612469426.yaml => -979308652.yaml} (100%)
 rename src/mtc/results/dummy/{1623703517.yaml => -988375807.yaml} (100%)
 rename src/mtc/results/dummy/{1599871509.yaml => -991922905.yaml} (100%)
 rename src/mtc/results/dummy/{-542667557.yaml => 1183415122.yaml} (100%)
 rename src/mtc/results/dummy/{-587344676.yaml => 1227695304.yaml} (100%)
 rename src/mtc/results/dummy/{-453685788.yaml => 1272105605.yaml} (100%)
 rename src/mtc/results/dummy/{-442405442.yaml => 1283302731.yaml} (100%)
 rename src/mtc/results/dummy/{-429778748.yaml => 1295853078.yaml} (100%)
 rename src/mtc/results/dummy/{-498149190.yaml => 1316801078.yaml} (100%)
 rename src/mtc/results/dummy/{-487005315.yaml => 1328208922.yaml} (100%)
 rename src/mtc/results/dummy/{-474510554.yaml => 1340801804.yaml} (100%)
 rename src/mtc/results/dummy/{-1564063432.yaml => 144778443.yaml} (100%)
 rename src/mtc/results/dummy/{-1553074118.yaml => 155931149.yaml} (100%)
 rename src/mtc/results/dummy/{-1540350554.yaml => 168492950.yaml} (100%)
 rename src/mtc/results/dummy/{-1608866404.yaml => 189371734.yaml} (100%)
 rename src/mtc/results/dummy/{-1597568458.yaml => 203146650.yaml} (100%)
 rename src/mtc/results/dummy/{-1585194105.yaml => 215783882.yaml} (100%)
 rename src/mtc/results/dummy/{-1358218541.yaml => 254319817.yaml} (100%)
 rename src/mtc/results/dummy/{-1405464563.yaml => 301530140.yaml} (100%)
 rename src/mtc/results/dummy/{-1572377916.yaml => 40651904.yaml} (100%)
 rename src/mtc/results/dummy/{-1180235424.yaml => 434875766.yaml} (100%)
 rename src/mtc/results/dummy/{-1227713112.yaml => 482378175.yaml} (100%)
 rename src/mtc/results/dummy/{-1060422961.yaml => 560822576.yaml} (100%)
 rename src/mtc/results/dummy/{-1107549484.yaml => 608106095.yaml} (100%)
 rename src/mtc/results/dummy/{-1006227179.yaml => 702701396.yaml} (100%)
 rename src/mtc/results/dummy/{-1039705736.yaml => 714102535.yaml} (100%)
 rename src/mtc/results/dummy/{-1027180942.yaml => 726842647.yaml} (100%)
 rename src/mtc/results/dummy/{-1051277393.yaml => 748047042.yaml} (100%)
 rename src/mtc/results/dummy/{-995076880.yaml => 759537023.yaml} (100%)
 rename src/mtc/results/dummy/{-982538817.yaml => 772496165.yaml} (100%)
 rename src/mtc/results/dummy/{-772783050.yaml => 832741068.yaml} (100%)
 rename src/mtc/results/dummy/{-1619843661.yaml => 88011482.yaml} (100%)
 rename src/mtc/results/dummy/{-827862775.yaml => 880346008.yaml} (100%)
 create mode 100644 src/mtc/src/reader/robot_reader.cpp
 create mode 100644 src/mtc/src/reader/wing_reader.cpp

diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 2c19e6d..35d97a1 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1,4 +1,4 @@
 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 26636
+/home/matteo/reachability/devel/./cmake.lock 26851
 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/src/.vscode/c_cpp_properties.json b/src/.vscode/c_cpp_properties.json
index 73da41b..5b82495 100644
--- a/src/.vscode/c_cpp_properties.json
+++ b/src/.vscode/c_cpp_properties.json
@@ -1,26 +1,26 @@
 {
-    "configurations": [
-        {
-            "browse": {
-                "databaseFilename": "${default}",
-                "limitSymbolsToIncludedHeaders": false
-            },
-            "includePath": [
-                "/home/matteo/reachability/devel/include/**",
-                "/opt/ros/noetic/include/**",
-                "/home/matteo/reachability/src/moveit_task_constructor/core/include/**",
-                "/home/matteo/reachability/src/moveit_task_constructor/demo/include/**",
-                "/home/matteo/reachability/src/mtc/include/**",
-                "/home/matteo/reachability/src/reachability/include/**",
-                "/home/matteo/reachability/src/moveit_task_constructor/rviz_marker_tools/include/**",
-                "/usr/include/**"
-            ],
-            "name": "ROS",
-            "intelliSenseMode": "gcc-x64",
-            "compilerPath": "/usr/bin/gcc",
-            "cStandard": "gnu11",
-            "cppStandard": "c++14"
-        }
-    ],
-    "version": 4
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/noetic/include/**",
+        "/home/matteo/reachability/devel/include/**",
+        "/home/matteo/reachability/src/moveit_task_constructor/core/include/**",
+        "/home/matteo/reachability/src/moveit_task_constructor/demo/include/**",
+        "/home/matteo/reachability/src/mtc/include/**",
+        "/home/matteo/reachability/src/reachability/include/**",
+        "/home/matteo/reachability/src/moveit_task_constructor/rviz_marker_tools/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
 }
\ No newline at end of file
diff --git a/src/mtc/CMakeLists.txt b/src/mtc/CMakeLists.txt
index 5175af5..9dd7d2e 100644
--- a/src/mtc/CMakeLists.txt
+++ b/src/mtc/CMakeLists.txt
@@ -81,6 +81,12 @@ src/impl/wing_moveit_decorator.cpp
 src/impl/wing_rviz_decorator.cpp
 src/impl/collision_helper.cpp
 
+src/reader/abstract_param_reader.cpp
+src/reader/robot_reader.cpp
+src/reader/wing_reader.cpp
+
+
+
 )
 
 add_executable(cell_routine src/cell_routine.cpp
@@ -94,6 +100,10 @@ src/impl/mediator.cpp
 src/impl/wing_moveit_decorator.cpp
 src/impl/wing_rviz_decorator.cpp
 src/impl/collision_helper.cpp
+
+src/reader/abstract_param_reader.cpp
+src/reader/robot_reader.cpp
+src/reader/wing_reader.cpp
 )
 
 
@@ -101,6 +111,7 @@ src/impl/collision_helper.cpp
 add_executable(mtc2taskspace src/mtc2taskspace.cpp
 src/reader/abstract_param_reader.cpp
 src/reader/task_space_reader.cpp
+src/reader/robot_reader.cpp
 )
 
 
diff --git a/src/mtc/include/impl/abstract_mediator.h b/src/mtc/include/impl/abstract_mediator.h
index 091b263..c143c0a 100644
--- a/src/mtc/include/impl/abstract_mediator.h
+++ b/src/mtc/include/impl/abstract_mediator.h
@@ -5,6 +5,8 @@
 #include "impl/abstract_robot.h"
 #include "impl/abstract_robot_element.h"
 #include "impl/robot.h"
+#include "reader/wing_reader.h"
+
 
 #include <ros/package.h>
 #include <yaml-cpp/yaml.h>
@@ -14,6 +16,8 @@
 #include <pcl/point_cloud.h>
 #include <pcl/octree/octree.h>
 
+
+
 #define box_size tf2::Vector3(0.0318f, 0.0636f, 0.091f)
 #define R_POS tf2::Vector3(0,0.6525f,0.4425f -0.005f)
 #define L_POS tf2::Vector3(0,-0.6525f,0.4425f -0.005f)
@@ -59,7 +63,7 @@ class Abstract_mediator {
     pcl::PointCloud< pcl::PointXYZ >::Ptr vector_to_cloud(std::vector<pcl::PointXYZ>& vector);
     std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
     
-    virtual void set_wings(std::vector<std::vector<wing_BP>>& wbp)=0;
+    virtual void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp)=0;
     virtual bool check_collision( const int& robot)=0;
     virtual void mediate()=0;
     virtual void build_wings(std::bitset<3>& wing, int& robot)=0;
diff --git a/src/mtc/include/impl/mediator.h b/src/mtc/include/impl/mediator.h
index 28f9003..1716c7e 100644
--- a/src/mtc/include/impl/mediator.h
+++ b/src/mtc/include/impl/mediator.h
@@ -22,7 +22,7 @@ class Mediator : public Abstract_mediator{
     void setup_rviz();
     void calculate(std::vector<tf2::Transform>& ground_per_robot);
     
-    void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
+    void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp) override;
     bool check_collision(const int& robot) override;
     void mediate() override;
     void build_wings(std::bitset<3>& wings, int& robot) override;
diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h
index 98eb160..1d0949e 100644
--- a/src/mtc/include/impl/moveit_mediator.h
+++ b/src/mtc/include/impl/moveit_mediator.h
@@ -93,7 +93,7 @@ class Moveit_mediator : public Abstract_mediator{
     bool check_collision(const int& robot) override;
     void mediate() override;
     void build_wings(std::bitset<3>& wing,int& robot) override;
-    void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
+    void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp) override;
 
     void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
     void publish_tables();
diff --git a/src/mtc/include/reader/abstract_param_reader.h b/src/mtc/include/reader/abstract_param_reader.h
index fb85893..9524d63 100644
--- a/src/mtc/include/reader/abstract_param_reader.h
+++ b/src/mtc/include/reader/abstract_param_reader.h
@@ -4,12 +4,17 @@
 #include "ros/ros.h"
 #include <ros/package.h>
 #include <xmlrpcpp/XmlRpc.h>
-#include <tf/transform_broadcaster.h>
+#include <tf2/LinearMath/Transform.h>
 #include <yaml-cpp/yaml.h>
 #include <fstream>
 #include <regex>
 
 
+struct object_data {
+    std::string name_;
+    tf2::Transform pose_;
+    tf2::Vector3 size_;
+};
 
 class Abstract_param_reader{
     protected:
diff --git a/src/mtc/include/reader/robot_reader.h b/src/mtc/include/reader/robot_reader.h
new file mode 100644
index 0000000..29ad3ea
--- /dev/null
+++ b/src/mtc/include/reader/robot_reader.h
@@ -0,0 +1,23 @@
+#ifndef ROBOT_READER_
+#define ROBOT_READER_
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include <xmlrpcpp/XmlRpc.h>
+
+#include "reader/abstract_param_reader.h"
+
+
+class Robot_reader : public Abstract_param_reader{
+    protected:
+    std::vector<object_data> robot_data_;
+
+    public:
+    Robot_reader(std::shared_ptr<ros::NodeHandle> const& d) : Abstract_param_reader(d){};
+    
+    inline void set_robot_data(std::vector<object_data>& robot_data) {robot_data_ = robot_data;}
+    inline std::vector<object_data> robot_data() {return robot_data_;}
+
+    void read() override;
+};
+#endif
diff --git a/src/mtc/include/reader/wing_reader.h b/src/mtc/include/reader/wing_reader.h
new file mode 100644
index 0000000..ec5956e
--- /dev/null
+++ b/src/mtc/include/reader/wing_reader.h
@@ -0,0 +1,23 @@
+#ifndef WING_READER_
+#define WING_READER_
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include <xmlrpcpp/XmlRpc.h>
+
+#include "reader/abstract_param_reader.h"
+
+
+class Wing_reader : public Abstract_param_reader{
+    protected:
+    std::vector<std::pair<std::vector<object_data>, int>> wing_data_;
+
+    public:
+    Wing_reader(std::shared_ptr<ros::NodeHandle> const& d) : Abstract_param_reader(d){};
+    
+    inline void set_wing_data(std::vector<std::pair<std::vector<object_data>, int>>& wing_data) {wing_data_ = wing_data;}
+    inline std::vector<std::pair<std::vector<object_data>, int>> wing_data() {return wing_data_;}
+
+    void read() override;
+};
+#endif
diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch
index aac714e..91bcb4c 100644
--- a/src/mtc/launch/cell_routine.launch
+++ b/src/mtc/launch/cell_routine.launch
@@ -1,5 +1,5 @@
 <launch>    
-    <arg name="result" default="dummy/-474510554.yaml" />
+    <arg name="result" default="dummy/-211114391.yaml" />
     <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
 
     <!-- this is to change-->
diff --git a/src/mtc/results/dummy/1543924248.yaml b/src/mtc/results/dummy/-1003092792.yaml
similarity index 100%
rename from src/mtc/results/dummy/1543924248.yaml
rename to src/mtc/results/dummy/-1003092792.yaml
diff --git a/src/mtc/results/dummy/1601455593.yaml b/src/mtc/results/dummy/-1010460506.yaml
similarity index 100%
rename from src/mtc/results/dummy/1601455593.yaml
rename to src/mtc/results/dummy/-1010460506.yaml
diff --git a/src/mtc/results/dummy/1587832961.yaml b/src/mtc/results/dummy/-1024001513.yaml
similarity index 100%
rename from src/mtc/results/dummy/1587832961.yaml
rename to src/mtc/results/dummy/-1024001513.yaml
diff --git a/src/mtc/results/dummy/1575731070.yaml b/src/mtc/results/dummy/-1036064907.yaml
similarity index 100%
rename from src/mtc/results/dummy/1575731070.yaml
rename to src/mtc/results/dummy/-1036064907.yaml
diff --git a/src/mtc/results/dummy/-1890202883.yaml b/src/mtc/results/dummy/-211114391.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1890202883.yaml
rename to src/mtc/results/dummy/-211114391.yaml
diff --git a/src/mtc/results/dummy/-1901373430.yaml b/src/mtc/results/dummy/-222544024.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1901373430.yaml
rename to src/mtc/results/dummy/-222544024.yaml
diff --git a/src/mtc/results/dummy/-1951250192.yaml b/src/mtc/results/dummy/-258879761.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1951250192.yaml
rename to src/mtc/results/dummy/-258879761.yaml
diff --git a/src/mtc/results/dummy/-1962818280.yaml b/src/mtc/results/dummy/-270419468.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1962818280.yaml
rename to src/mtc/results/dummy/-270419468.yaml
diff --git a/src/mtc/results/dummy/1850935177.yaml b/src/mtc/results/dummy/-652201118.yaml
similarity index 100%
rename from src/mtc/results/dummy/1850935177.yaml
rename to src/mtc/results/dummy/-652201118.yaml
diff --git a/src/mtc/results/dummy/1895656920.yaml b/src/mtc/results/dummy/-696648853.yaml
similarity index 100%
rename from src/mtc/results/dummy/1895656920.yaml
rename to src/mtc/results/dummy/-696648853.yaml
diff --git a/src/mtc/results/dummy/1821310512.yaml b/src/mtc/results/dummy/-742232139.yaml
similarity index 100%
rename from src/mtc/results/dummy/1821310512.yaml
rename to src/mtc/results/dummy/-742232139.yaml
diff --git a/src/mtc/results/dummy/1807829100.yaml b/src/mtc/results/dummy/-755774193.yaml
similarity index 100%
rename from src/mtc/results/dummy/1807829100.yaml
rename to src/mtc/results/dummy/-755774193.yaml
diff --git a/src/mtc/results/dummy/1795913128.yaml b/src/mtc/results/dummy/-767674851.yaml
similarity index 100%
rename from src/mtc/results/dummy/1795913128.yaml
rename to src/mtc/results/dummy/-767674851.yaml
diff --git a/src/mtc/results/dummy/1868843792.yaml b/src/mtc/results/dummy/-789859288.yaml
similarity index 100%
rename from src/mtc/results/dummy/1868843792.yaml
rename to src/mtc/results/dummy/-789859288.yaml
diff --git a/src/mtc/results/dummy/1855511004.yaml b/src/mtc/results/dummy/-803422225.yaml
similarity index 100%
rename from src/mtc/results/dummy/1855511004.yaml
rename to src/mtc/results/dummy/-803422225.yaml
diff --git a/src/mtc/results/dummy/1843511501.yaml b/src/mtc/results/dummy/-815321487.yaml
similarity index 100%
rename from src/mtc/results/dummy/1843511501.yaml
rename to src/mtc/results/dummy/-815321487.yaml
diff --git a/src/mtc/results/dummy/1656942183.yaml b/src/mtc/results/dummy/-846087811.yaml
similarity index 100%
rename from src/mtc/results/dummy/1656942183.yaml
rename to src/mtc/results/dummy/-846087811.yaml
diff --git a/src/mtc/results/dummy/1711393564.yaml b/src/mtc/results/dummy/-852264234.yaml
similarity index 100%
rename from src/mtc/results/dummy/1711393564.yaml
rename to src/mtc/results/dummy/-852264234.yaml
diff --git a/src/mtc/results/dummy/1697794188.yaml b/src/mtc/results/dummy/-865767525.yaml
similarity index 100%
rename from src/mtc/results/dummy/1697794188.yaml
rename to src/mtc/results/dummy/-865767525.yaml
diff --git a/src/mtc/results/dummy/1685895957.yaml b/src/mtc/results/dummy/-877673981.yaml
similarity index 100%
rename from src/mtc/results/dummy/1685895957.yaml
rename to src/mtc/results/dummy/-877673981.yaml
diff --git a/src/mtc/results/dummy/1701273998.yaml b/src/mtc/results/dummy/-890048247.yaml
similarity index 100%
rename from src/mtc/results/dummy/1701273998.yaml
rename to src/mtc/results/dummy/-890048247.yaml
diff --git a/src/mtc/results/dummy/1759072325.yaml b/src/mtc/results/dummy/-899976593.yaml
similarity index 100%
rename from src/mtc/results/dummy/1759072325.yaml
rename to src/mtc/results/dummy/-899976593.yaml
diff --git a/src/mtc/results/dummy/1745566468.yaml b/src/mtc/results/dummy/-914020264.yaml
similarity index 100%
rename from src/mtc/results/dummy/1745566468.yaml
rename to src/mtc/results/dummy/-914020264.yaml
diff --git a/src/mtc/results/dummy/1733626611.yaml b/src/mtc/results/dummy/-925946624.yaml
similarity index 100%
rename from src/mtc/results/dummy/1733626611.yaml
rename to src/mtc/results/dummy/-925946624.yaml
diff --git a/src/mtc/results/dummy/1567654444.yaml b/src/mtc/results/dummy/-934521686.yaml
similarity index 100%
rename from src/mtc/results/dummy/1567654444.yaml
rename to src/mtc/results/dummy/-934521686.yaml
diff --git a/src/mtc/results/dummy/1555239444.yaml b/src/mtc/results/dummy/-947074197.yaml
similarity index 100%
rename from src/mtc/results/dummy/1555239444.yaml
rename to src/mtc/results/dummy/-947074197.yaml
diff --git a/src/mtc/results/dummy/1588704657.yaml b/src/mtc/results/dummy/-958226134.yaml
similarity index 100%
rename from src/mtc/results/dummy/1588704657.yaml
rename to src/mtc/results/dummy/-958226134.yaml
diff --git a/src/mtc/results/dummy/1648958842.yaml b/src/mtc/results/dummy/-962936586.yaml
similarity index 100%
rename from src/mtc/results/dummy/1648958842.yaml
rename to src/mtc/results/dummy/-962936586.yaml
diff --git a/src/mtc/results/dummy/1635476033.yaml b/src/mtc/results/dummy/-976487161.yaml
similarity index 100%
rename from src/mtc/results/dummy/1635476033.yaml
rename to src/mtc/results/dummy/-976487161.yaml
diff --git a/src/mtc/results/dummy/1612469426.yaml b/src/mtc/results/dummy/-979308652.yaml
similarity index 100%
rename from src/mtc/results/dummy/1612469426.yaml
rename to src/mtc/results/dummy/-979308652.yaml
diff --git a/src/mtc/results/dummy/1623703517.yaml b/src/mtc/results/dummy/-988375807.yaml
similarity index 100%
rename from src/mtc/results/dummy/1623703517.yaml
rename to src/mtc/results/dummy/-988375807.yaml
diff --git a/src/mtc/results/dummy/1599871509.yaml b/src/mtc/results/dummy/-991922905.yaml
similarity index 100%
rename from src/mtc/results/dummy/1599871509.yaml
rename to src/mtc/results/dummy/-991922905.yaml
diff --git a/src/mtc/results/dummy/-542667557.yaml b/src/mtc/results/dummy/1183415122.yaml
similarity index 100%
rename from src/mtc/results/dummy/-542667557.yaml
rename to src/mtc/results/dummy/1183415122.yaml
diff --git a/src/mtc/results/dummy/-587344676.yaml b/src/mtc/results/dummy/1227695304.yaml
similarity index 100%
rename from src/mtc/results/dummy/-587344676.yaml
rename to src/mtc/results/dummy/1227695304.yaml
diff --git a/src/mtc/results/dummy/-453685788.yaml b/src/mtc/results/dummy/1272105605.yaml
similarity index 100%
rename from src/mtc/results/dummy/-453685788.yaml
rename to src/mtc/results/dummy/1272105605.yaml
diff --git a/src/mtc/results/dummy/-442405442.yaml b/src/mtc/results/dummy/1283302731.yaml
similarity index 100%
rename from src/mtc/results/dummy/-442405442.yaml
rename to src/mtc/results/dummy/1283302731.yaml
diff --git a/src/mtc/results/dummy/-429778748.yaml b/src/mtc/results/dummy/1295853078.yaml
similarity index 100%
rename from src/mtc/results/dummy/-429778748.yaml
rename to src/mtc/results/dummy/1295853078.yaml
diff --git a/src/mtc/results/dummy/-498149190.yaml b/src/mtc/results/dummy/1316801078.yaml
similarity index 100%
rename from src/mtc/results/dummy/-498149190.yaml
rename to src/mtc/results/dummy/1316801078.yaml
diff --git a/src/mtc/results/dummy/-487005315.yaml b/src/mtc/results/dummy/1328208922.yaml
similarity index 100%
rename from src/mtc/results/dummy/-487005315.yaml
rename to src/mtc/results/dummy/1328208922.yaml
diff --git a/src/mtc/results/dummy/-474510554.yaml b/src/mtc/results/dummy/1340801804.yaml
similarity index 100%
rename from src/mtc/results/dummy/-474510554.yaml
rename to src/mtc/results/dummy/1340801804.yaml
diff --git a/src/mtc/results/dummy/-1564063432.yaml b/src/mtc/results/dummy/144778443.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1564063432.yaml
rename to src/mtc/results/dummy/144778443.yaml
diff --git a/src/mtc/results/dummy/-1553074118.yaml b/src/mtc/results/dummy/155931149.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1553074118.yaml
rename to src/mtc/results/dummy/155931149.yaml
diff --git a/src/mtc/results/dummy/-1540350554.yaml b/src/mtc/results/dummy/168492950.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1540350554.yaml
rename to src/mtc/results/dummy/168492950.yaml
diff --git a/src/mtc/results/dummy/-1608866404.yaml b/src/mtc/results/dummy/189371734.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1608866404.yaml
rename to src/mtc/results/dummy/189371734.yaml
diff --git a/src/mtc/results/dummy/-1597568458.yaml b/src/mtc/results/dummy/203146650.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1597568458.yaml
rename to src/mtc/results/dummy/203146650.yaml
diff --git a/src/mtc/results/dummy/-1585194105.yaml b/src/mtc/results/dummy/215783882.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1585194105.yaml
rename to src/mtc/results/dummy/215783882.yaml
diff --git a/src/mtc/results/dummy/-1358218541.yaml b/src/mtc/results/dummy/254319817.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1358218541.yaml
rename to src/mtc/results/dummy/254319817.yaml
diff --git a/src/mtc/results/dummy/-1405464563.yaml b/src/mtc/results/dummy/301530140.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1405464563.yaml
rename to src/mtc/results/dummy/301530140.yaml
diff --git a/src/mtc/results/dummy/-1572377916.yaml b/src/mtc/results/dummy/40651904.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1572377916.yaml
rename to src/mtc/results/dummy/40651904.yaml
diff --git a/src/mtc/results/dummy/-1180235424.yaml b/src/mtc/results/dummy/434875766.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1180235424.yaml
rename to src/mtc/results/dummy/434875766.yaml
diff --git a/src/mtc/results/dummy/-1227713112.yaml b/src/mtc/results/dummy/482378175.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1227713112.yaml
rename to src/mtc/results/dummy/482378175.yaml
diff --git a/src/mtc/results/dummy/-1060422961.yaml b/src/mtc/results/dummy/560822576.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1060422961.yaml
rename to src/mtc/results/dummy/560822576.yaml
diff --git a/src/mtc/results/dummy/-1107549484.yaml b/src/mtc/results/dummy/608106095.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1107549484.yaml
rename to src/mtc/results/dummy/608106095.yaml
diff --git a/src/mtc/results/dummy/-1006227179.yaml b/src/mtc/results/dummy/702701396.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1006227179.yaml
rename to src/mtc/results/dummy/702701396.yaml
diff --git a/src/mtc/results/dummy/-1039705736.yaml b/src/mtc/results/dummy/714102535.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1039705736.yaml
rename to src/mtc/results/dummy/714102535.yaml
diff --git a/src/mtc/results/dummy/-1027180942.yaml b/src/mtc/results/dummy/726842647.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1027180942.yaml
rename to src/mtc/results/dummy/726842647.yaml
diff --git a/src/mtc/results/dummy/-1051277393.yaml b/src/mtc/results/dummy/748047042.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1051277393.yaml
rename to src/mtc/results/dummy/748047042.yaml
diff --git a/src/mtc/results/dummy/-995076880.yaml b/src/mtc/results/dummy/759537023.yaml
similarity index 100%
rename from src/mtc/results/dummy/-995076880.yaml
rename to src/mtc/results/dummy/759537023.yaml
diff --git a/src/mtc/results/dummy/-982538817.yaml b/src/mtc/results/dummy/772496165.yaml
similarity index 100%
rename from src/mtc/results/dummy/-982538817.yaml
rename to src/mtc/results/dummy/772496165.yaml
diff --git a/src/mtc/results/dummy/-772783050.yaml b/src/mtc/results/dummy/832741068.yaml
similarity index 100%
rename from src/mtc/results/dummy/-772783050.yaml
rename to src/mtc/results/dummy/832741068.yaml
diff --git a/src/mtc/results/dummy/-1619843661.yaml b/src/mtc/results/dummy/88011482.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1619843661.yaml
rename to src/mtc/results/dummy/88011482.yaml
diff --git a/src/mtc/results/dummy/-827862775.yaml b/src/mtc/results/dummy/880346008.yaml
similarity index 100%
rename from src/mtc/results/dummy/-827862775.yaml
rename to src/mtc/results/dummy/880346008.yaml
diff --git a/src/mtc/src/base_routine.cpp b/src/mtc/src/base_routine.cpp
index e462fcf..f63e1d4 100644
--- a/src/mtc/src/base_routine.cpp
+++ b/src/mtc/src/base_routine.cpp
@@ -14,28 +14,12 @@
 #include "impl/base_by_rotation.h"
 #include <xmlrpcpp/XmlRpc.h>
 #include <filesystem>
+#include "reader/abstract_param_reader.h"
+#include "reader/robot_reader.h"
+#include "reader/wing_reader.h"
+
+
 
-float value_caster(XmlRpc::XmlRpcValue& val){
-    float t = 0;
-    try{
-        switch (val.getType()){
-            case XmlRpc::XmlRpcValue::TypeInt:
-                t = static_cast<int>(val);
-                break;
-            case XmlRpc::XmlRpcValue::TypeDouble:
-                t = static_cast<double>(val);
-                break;
-            case XmlRpc::XmlRpcValue::TypeString:
-                t = std::stof(val);
-                break;
-            default:
-                ROS_INFO("value type is some unknown type");
-            }
-    } catch (XmlRpc::XmlRpcException){
-        ROS_INFO("Something went wrong, returning 0");
-    }
-    return t;
-}
 
 
 int main(int argc, char **argv){
@@ -64,316 +48,24 @@ int main(int argc, char **argv){
     mediator->set_result_vector(results);
     mediator->set_dirname(filename);
 
-    float a = 0; float b = 0;
-    std::vector<std::vector<wing_BP>> blueprints_per_robot(2);
-    std::vector<wing_BP> blueprints(3);
-
-    blueprints[0] = {std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
-    blueprints[1] = {std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
-    blueprints[2] = {std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
-    blueprints_per_robot[0] = blueprints;
-    blueprints_per_robot[1] = blueprints;
 
-    for (int i = 0; i < resources.size(); i++){
-        if(resources[i]["id"] == "table1_table_top"){
-            ROS_INFO("=> Robot: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
+    std::unique_ptr<Abstract_param_reader> robot_reader(new Robot_reader(n));
+    robot_reader->read();
+    std::vector<object_data> rd = static_cast<Robot_reader*>(robot_reader.get())->robot_data();
+    for (int i = 0; i < rd.size() ;i++) mediator->connect_robots(new Robot(rd[i].name_, rd[i].pose_, rd[i].size_));
 
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+    std::unique_ptr<Abstract_param_reader> wing_reader(new Wing_reader(n));
+    wing_reader->read();
+    auto wd = static_cast<Wing_reader*>(wing_reader.get())->wing_data();
 
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ((value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
-            ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
-            std::string name("panda_arm1");
-            mediator->connect_robots(new Robot(name, tf2::Transform(rot, pos), size));
-            ROS_INFO("=> Robot:loading SUCCESS");
-        }
-
-        if(resources[i]["id"] == "table2_table_top"){
-            ROS_INFO("=> Robot: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ((value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
-            ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-            
-            std::string name("panda_arm2");
-            mediator->connect_robots(new Robot(name, tf2::Transform(rot, pos), size));
-            ROS_INFO("=> Robot:loading SUCCESS");
+    for (int i = 0; i < mediator->robots().size(); i++){
+        for(object_data& w : wd[i].first){
+            w.pose_ = mediator->robots()[i]->tf().inverse() * w.pose_;
         }
+        mediator->robots()[i]->set_observer_mask(static_cast<wing_config>(wd[i].second));
     }
 
-    
-    for (int i = 0; i < resources.size(); i++){
-        if(resources[i]["id"] == "table1_right_panel" ) { 
-            ROS_INFO("=> WING: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
-            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
-            /*
-            Wing* w = dynamic_cast<Wing*>(blueprints_per_robot[0][0]);
-            w->set_name("table1_right_panel");
-            w->set_relative_tf(mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
-            w->set_set(size);
-
-            blueprints_per_robot[0][0] = new Wing_rviz_decorator(w);
-            */
-
-            blueprints_per_robot[0][0].name_ = "table1_right_panel";
-            blueprints_per_robot[0][0].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
-            blueprints_per_robot[0][0].size_ = size;
-
-
-            a += std::pow(2, 0);
-            ROS_INFO("=> WING:loading SUCCESS");
-        }
-
-        if(resources[i]["id"] == "table1_front_panel" ) { 
-            ROS_INFO("=> WING: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-            
-            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
-            /*
-            Abstract_robot_element* are = new Wing(std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0));
-            Wing* w = dynamic_cast<Wing*>(are);
-            w->set_name("table1_front_panel");
-            w->set_relative_tf(mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
-            w->set_set(size);
-
-            Abstract_robot_element* wrd = new Wing_rviz_decorator(w);
-            blueprints_per_robot[0][1] = wrd;
-
-            */
-            a += std::pow(2, 1);
-
-            blueprints_per_robot[0][1].name_ = std::string("table1_front_panel");
-                        ROS_INFO("uzguzg");
-            blueprints_per_robot[0][1].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
-            blueprints_per_robot[0][1].size_ = size;
-            ROS_INFO("=> WING:loading SUCCESS");
-        }
-
-        if(resources[i]["id"] == "table1_left_panel" ) { 
-            ROS_INFO("=> WING: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-            
-            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
-            /*
-            Abstract_robot_element* are = new Wing(std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0));
-            Wing* w = dynamic_cast<Wing*>(are);
-            w->set_name("table1_left_panel");
-            w->set_relative_tf(mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
-            w->set_set(size);
-
-            Abstract_robot_element* wrd = new Wing_rviz_decorator(are);
-            blueprints_per_robot[0][2] = wrd;
-            */
-            blueprints_per_robot[0][2].name_ = "table1_left_panel";
-            blueprints_per_robot[0][2].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
-            blueprints_per_robot[0][2].size_ = size;
-            a += std::pow(2, 2);
-            ROS_INFO("=> WING:loading SUCCESS");
-
-        }
-
-        if(resources[i]["id"] == "table2_right_panel" ) { 
-            ROS_INFO("=> WING: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-            
-            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
-            /*
-            Abstract_robot_element* are = new Wing(std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0));
-            Wing* w = dynamic_cast<Wing*>(are);
-            w->set_name("table2_right_panel");
-            w->set_relative_tf(mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
-            w->set_set(size);
-
-            blueprints_per_robot[1][0] = new Wing_rviz_decorator(w);
-            */
-            blueprints_per_robot[1][0].name_ = "table2_right_panel";
-            blueprints_per_robot[1][0].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
-            blueprints_per_robot[1][0].size_ = size;
-            b += std::pow(2, 0);
-            ROS_INFO("=> WING:loading SUCCESS");
-        }
-
-        if(resources[i]["id"] == "table2_front_panel" ) {  
-            ROS_INFO("=> WING: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-            
-            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
-            /*
-            Wing* w = dynamic_cast<Wing*>(blueprints_per_robot[1][1]);                    
-            w->set_name("table2_front_panel");
-            w->set_relative_tf(mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
-            w->set_set(size);
-            blueprints_per_robot[1][1] = new Wing_rviz_decorator(w);
-            */
-            blueprints_per_robot[1][1].name_ = "table2_front_panel";
-            blueprints_per_robot[1][1].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
-            blueprints_per_robot[1][1].size_ = size;
-            ROS_INFO("=> WING:loading SUCCESS");
-            b += std::pow(2, 1);
-
-        }
-
-        if(resources[i]["id"] == "table2_left_panel" ) {          
-            ROS_INFO("=> WING: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-            
-
-            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-            /*
-            Wing* w = dynamic_cast<Wing*>(blueprints_per_robot[1][2]);                    
-            w->set_name("table2_left_panel");
-            w->set_relative_tf(mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
-            w->set_set(size);
-            blueprints_per_robot[1][2] = new Wing_rviz_decorator(w);
-            */
-            
-            blueprints_per_robot[1][2].name_ = "table2_left_panel";
-            blueprints_per_robot[1][2].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
-            blueprints_per_robot[1][2].size_ = size;
-            ROS_INFO("=> WING:loading SUCCESS");
-            b += std::pow(2, 2);
-        }
-    }
-    mediator->robots()[0]->set_observer_mask(static_cast<wing_config>(a));
-    mediator->robots()[1]->set_observer_mask(static_cast<wing_config>(b));
-
-
-    ROS_INFO("ist durch");
-    mediator->set_wings(blueprints_per_robot);
+    mediator->set_wings(wd);
     mediator->mediate();
 
 
diff --git a/src/mtc/src/cell_routine.cpp b/src/mtc/src/cell_routine.cpp
index b9a4c33..e56dc02 100644
--- a/src/mtc/src/cell_routine.cpp
+++ b/src/mtc/src/cell_routine.cpp
@@ -10,28 +10,10 @@
 #include "impl/moveit_robot.h"
 #include "impl/collision_helper.h"
 #include <xmlrpcpp/XmlRpc.h>
+#include "reader/abstract_param_reader.h"
+#include "reader/robot_reader.h"
+#include "reader/wing_reader.h"
 
-float value_caster(XmlRpc::XmlRpcValue& val){
-    float t = 0;
-    try{
-        switch (val.getType()){
-            case XmlRpc::XmlRpcValue::TypeInt:
-                t = static_cast<int>(val);
-                break;
-            case XmlRpc::XmlRpcValue::TypeDouble:
-                t = static_cast<double>(val);
-                break;
-            case XmlRpc::XmlRpcValue::TypeString:
-                t = std::stof(val);
-                break;
-            default:
-                ROS_INFO("value type is some unknown type");
-            }
-    } catch (XmlRpc::XmlRpcException){
-        ROS_INFO("Something went wrong, returning 0");
-    }
-    return t;
-}
 
 int main(int argc, char **argv){
     ros::init(argc, argv, "cell_routine");
@@ -57,280 +39,34 @@ int main(int argc, char **argv){
 
     moveit_mediator->load_robot_description();
 
-    std::vector<std::vector<wing_BP>> blueprints_per_robot(2);
-    std::vector<wing_BP> blueprints(3);
-    blueprints[0] = {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
-    blueprints[1] = {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
-    blueprints[2] = {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
-    blueprints_per_robot[0] = blueprints;
-    blueprints_per_robot[1] = blueprints;
-    int a = 0; int b = 0;
-
-for (int i = 0; i < resources.size(); i++){
-        if(resources[i]["id"] == "table1_table_top"){
-            ROS_INFO("=> Robot: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ((value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-            
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
-            std::string name("panda_arm1");
-            moveit_mediator->task_map().insert({name, std::vector<moveit::task_constructor::Task>()});
-            Abstract_robot* robot1 = new Moveit_robot(name, tf2::Transform(rot, pos), size);
-            Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(robot1);
-            ceti1->set_observer_mask(wing_config::RML);
-            mediator->connect_robots(robot1);
-            ROS_INFO("=> Robot:loading SUCCESS");
-        }
-
-        if(resources[i]["id"] == "table2_table_top"){
-            ROS_INFO("=> Robot: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ((value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
-            ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+    std::unique_ptr<Abstract_param_reader> robot_reader(new Robot_reader(nnh));
+    robot_reader->read();
+    std::vector<object_data> rd = static_cast<Robot_reader*>(robot_reader.get())->robot_data();
+    for (int i = 0; i < rd.size() ;i++) mediator->connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_));
 
-            std::string name("panda_arm2");
-            moveit_mediator->task_map().insert({name, std::vector<moveit::task_constructor::Task>()});
-            Abstract_robot* robot2 = new Moveit_robot(name, tf2::Transform(rot, pos), size);
-            Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(robot2);
-            ceti2->set_observer_mask(wing_config::RML);
-            mediator->connect_robots(robot2);
+    std::unique_ptr<Abstract_param_reader> wing_reader(new Wing_reader(nnh));
+    wing_reader->read();
+    auto wd = static_cast<Wing_reader*>(wing_reader.get())->wing_data();
 
-            ROS_INFO("=> Robot:loading SUCCESS");
+    for (int i = 0; i < mediator->robots().size(); i++){
+        for(object_data& w : wd[i].first){
+            w.pose_ = mediator->robots()[i]->tf().inverse() * w.pose_;
         }
+        mediator->robots()[i]->set_observer_mask(static_cast<wing_config>(wd[i].second));
     }
 
-    
-    for (int i = 0; i < resources.size(); i++){
-        if(resources[i]["id"] == "table1_right_panel" ) { 
-            ROS_INFO("=> WING: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])+ size.getZ()/2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
-            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
-            blueprints_per_robot[0][0].name_ = "table1_right_panel";
-            blueprints_per_robot[0][0].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
-            blueprints_per_robot[0][0].size_ = size;
-
-
-            a += std::pow(2, 0);
-            ROS_INFO("=> WING:loading SUCCESS");
-        }
-
-        if(resources[i]["id"] == "table1_front_panel" ) { 
-            ROS_INFO("=> WING: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-            
-            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
-            a += std::pow(2, 1);
-
-            blueprints_per_robot[0][1].name_ = std::string("table1_front_panel");
-            blueprints_per_robot[0][1].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
-            blueprints_per_robot[0][1].size_ = size;
-            ROS_INFO("=> WING:loading SUCCESS");
-        }
-
-        if(resources[i]["id"] == "table1_left_panel" ) { 
-            ROS_INFO("=> WING: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
 
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-            
-            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
-            blueprints_per_robot[0][2].name_ = "table1_left_panel";
-            blueprints_per_robot[0][2].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
-            blueprints_per_robot[0][2].size_ = size;
-            a += std::pow(2, 2);
-            ROS_INFO("=> WING:loading SUCCESS");
-
-        }
-
-        if(resources[i]["id"] == "table2_right_panel" ) { 
-            ROS_INFO("=> WING: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])+ size.getZ()/2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-            
-            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
-            blueprints_per_robot[1][0].name_ = "table2_right_panel";
-            blueprints_per_robot[1][0].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
-            blueprints_per_robot[1][0].size_ = size;
-            b += std::pow(2, 0);
-            ROS_INFO("=> WING:loading SUCCESS");
-        }
-
-        if(resources[i]["id"] == "table2_front_panel" ) {  
-            ROS_INFO("=> WING: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])+ size.getZ()/2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-            
-            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
-            blueprints_per_robot[1][1].name_ = "table2_front_panel";
-            blueprints_per_robot[1][1].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
-            blueprints_per_robot[1][1].size_ = size;
-            ROS_INFO("=> WING:loading SUCCESS");
-            b += std::pow(2, 1);
-
-        }
-
-        if(resources[i]["id"] == "table2_left_panel" ) {          
-            ROS_INFO("=> WING: description found, loading...");
-            tf2::Vector3 pos;
-            tf2::Vector3 size;
-            tf2::Quaternion rot;
-
-            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
-            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
-            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-
-            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
-            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
-            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])+ size.getZ()/2) :pos.setZ(0);
-            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
-            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
-            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
-            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-            
+    moveit_mediator->set_wings(wd);
+    Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(mediator->robots()[0]);
+    Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(mediator->robots()[1]);
+    
 
-            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
-            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
-            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-  
-            blueprints_per_robot[1][2].name_ = "table2_left_panel";
-            blueprints_per_robot[1][2].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
-            blueprints_per_robot[1][2].size_ = size;
-            ROS_INFO("=> WING:loading SUCCESS");
-            b += std::pow(2, 2);
-        }
+    for (int i = 0; i < mediator->robots().size(); i++){
+        std::bitset<3> bs = wd[i].second;
+        moveit_mediator->build_wings(bs, i);
     }
 
 
-
-    moveit_mediator->set_wings(blueprints_per_robot);
-    Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(mediator->robots()[0]);
-    Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(mediator->robots()[1]);
-    
-    std::bitset<3> ia = a;
-    std::bitset<3> ib = b;
-    int r1 = 0; int r2 = 1;
-    moveit_mediator->build_wings(ia, r1);
-    moveit_mediator->build_wings(ib, r2);
     //moveit::planning_interface::PlanningSceneInterface::applyCollisionObject()
     ceti1->notify();
     ceti2->notify();
diff --git a/src/mtc/src/impl/mediator.cpp b/src/mtc/src/impl/mediator.cpp
index a530e8a..b3c1ffc 100644
--- a/src/mtc/src/impl/mediator.cpp
+++ b/src/mtc/src/impl/mediator.cpp
@@ -37,12 +37,12 @@ void Mediator::setup_rviz(){
     //pub_->publish(ma);
 }
 
-void Mediator::set_wings(std::vector<std::vector<wing_BP>>& wbp){
+void Mediator::set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp){
 
     for (int i =0; i < wbp.size(); i++){
         std::vector<Abstract_robot_element*> v;
-        for (int j =0; j < wbp[i].size(); j++){
-            Abstract_robot_element* are = new Wing_rviz_decorator( new Wing(wbp[i][j].name_, wbp[i][j].pos_,wbp[i][j].size_));
+        for (int j =0; j < wbp[i].first.size(); j++){
+            Abstract_robot_element* are = new Wing_rviz_decorator( new Wing(wbp[i].first[j].name_, wbp[i].first[j].pose_,wbp[i].first[j].size_));
             v.push_back(are);
         }
         wings_.push_back(v);
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index 0f1e9ce..2071a41 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -71,18 +71,20 @@ void Moveit_mediator::mediate(){
 void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){
     std::bitset<3> result = robots_[robot]->observer_mask() & wing;
     Robot* ceti = dynamic_cast<Robot*>(robots_[robot]);
+
     for (std::size_t i = 0; i < result.size(); i++){
         if (result[i]){
+			ROS_INFO("%i", i);
             ceti->register_observers(wings_[robot][i]);
         }
     }
 }
 
-void Moveit_mediator::set_wings(std::vector<std::vector<wing_BP>>& wbp){
+void Moveit_mediator::set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp){
     for (int i =0; i < wbp.size(); i++){
         std::vector<Abstract_robot_element*> v;
-        for (int j =0; j < wbp[i].size(); j++){
-            Abstract_robot_element* are = new Wing_moveit_decorator( new Wing(wbp[i][j].name_, wbp[i][j].pos_,wbp[i][j].size_));
+        for (int j =0; j < wbp[i].first.size(); j++){
+            Abstract_robot_element* are = new Wing_moveit_decorator( new Wing(wbp[i].first[j].name_, wbp[i].first[j].pose_,wbp[i].first[j].size_));
             v.push_back(are);
         }
         wings_.push_back(v);
diff --git a/src/mtc/src/mtc2taskspace.cpp b/src/mtc/src/mtc2taskspace.cpp
index bff2f43..e1b78f5 100644
--- a/src/mtc/src/mtc2taskspace.cpp
+++ b/src/mtc/src/mtc2taskspace.cpp
@@ -1,12 +1,13 @@
 #include "reader/abstract_param_reader.h"
 #include "reader/task_space_reader.h"
+#include "reader/robot_reader.h"
 #include <regex>
 
 int main(int argc, char **argv){
     ros::init(argc, argv, "mtc2taskspace");
     std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
 
-    std::unique_ptr<Abstract_param_reader> task_space_reader(new Task_space_reader(n));
-    task_space_reader->read();
+    std::unique_ptr<Abstract_param_reader> ts_reader(new Task_space_reader(n));
+    ts_reader->read();
     return 0;
 }
\ No newline at end of file
diff --git a/src/mtc/src/reader/robot_reader.cpp b/src/mtc/src/reader/robot_reader.cpp
new file mode 100644
index 0000000..a2e3b57
--- /dev/null
+++ b/src/mtc/src/reader/robot_reader.cpp
@@ -0,0 +1,43 @@
+#include "reader/robot_reader.h"
+
+void Robot_reader::read(){
+    std::string filename = "dummy.yaml";
+    //nh_->getParam("/resource_name", filename);
+
+    ROS_INFO("--- ROBOT_READER ---");
+    XmlRpc::XmlRpcValue resources;
+    nh_->getParam("/objects", resources);
+    std::regex rx("table([0-9]+)_table_top");
+    std::smatch match;
+
+    for (int i = 0; i < resources.size(); i++){
+        std::string str = resources[i]["id"];
+        if(std::regex_match(str, match, rx)){
+            ROS_INFO("=> Robot: description found, loading...");
+            std::stringstream ss;
+            ss << "panda_arm" << match[1];
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["size"].hasMember("length")) ? size.setX(float_of(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(float_of(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(float_of(resources[i]["size"]["height"])) :size.setZ(0);
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(float_of(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(float_of(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ((float_of(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(float_of(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(float_of(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(float_of(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(float_of(resources[i]["orientation"]["w"])) :rot.setW(0);
+
+            ROS_INFO("--- Robot: %s ---", ss.str().c_str());
+            ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            robot_data_.push_back({ss.str().c_str(), tf2::Transform(rot,pos), size});
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/mtc/src/reader/task_space_reader.cpp b/src/mtc/src/reader/task_space_reader.cpp
index fa623b8..5fef734 100644
--- a/src/mtc/src/reader/task_space_reader.cpp
+++ b/src/mtc/src/reader/task_space_reader.cpp
@@ -2,7 +2,7 @@
 
 
 void Task_space_reader::read(){
-    std::string filename = "penis";
+    std::string filename = "dummy.yaml";
     //nh_->getParam("/resource_name", filename);
 
     XmlRpc::XmlRpcValue resources;
diff --git a/src/mtc/src/reader/wing_reader.cpp b/src/mtc/src/reader/wing_reader.cpp
new file mode 100644
index 0000000..0af4e42
--- /dev/null
+++ b/src/mtc/src/reader/wing_reader.cpp
@@ -0,0 +1,133 @@
+#include "reader/wing_reader.h"
+
+void Wing_reader::read(){
+    XmlRpc::XmlRpcValue resources;
+    nh_->getParam("/objects", resources);
+
+    std::regex rx("table([0-9]+)_table_top");
+    std::smatch match;
+
+    ROS_INFO("--- WING_READER ---");
+    for(int i = 0; i < resources.size(); i++){
+        std::string str = resources[i]["id"];
+        if(std::regex_match(str, match, rx)){
+            std::vector<object_data> wd;
+            wd.push_back({"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)});
+            wd.push_back({"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)});
+            wd.push_back({"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)});
+            std::pair<std::vector<object_data>, int> pair;
+            pair.first = wd;
+            pair.second = 0;
+            wing_data_.push_back(pair);
+        }
+    }
+
+
+    rx.assign("table([0-9]+)_right_panel");
+    for(int i = 0; i < resources.size(); i++){
+      std::string str = resources[i]["id"];
+      if (std::regex_match(str, match, rx)){
+        ROS_INFO("=> WING: description found, loading...");
+        tf2::Vector3 pos;
+        tf2::Vector3 size;
+        tf2::Quaternion rot;
+
+        
+        (resources[i]["size"].hasMember("length")) ? size.setX(float_of(resources[i]["size"]["length"])) :size.setX(0);
+        (resources[i]["size"].hasMember("width")) ? size.setY(float_of(resources[i]["size"]["width"])) :size.setY(0);
+        (resources[i]["size"].hasMember("height")) ? size.setZ(float_of(resources[i]["size"]["height"])) :size.setZ(0);
+
+        (resources[i]["pos"].hasMember("x")) ? pos.setX(float_of(resources[i]["pos"]["x"])) :pos.setX(0);
+        (resources[i]["pos"].hasMember("y")) ? pos.setY(float_of(resources[i]["pos"]["y"])) :pos.setY(0);
+        (resources[i]["pos"].hasMember("z")) ? pos.setZ(float_of(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
+        (resources[i]["orientation"].hasMember("x")) ? rot.setX(float_of(resources[i]["orientation"]["x"])) :rot.setX(0);
+        (resources[i]["orientation"].hasMember("y")) ? rot.setY(float_of(resources[i]["orientation"]["y"])) :rot.setY(0);
+        (resources[i]["orientation"].hasMember("z")) ? rot.setZ(float_of(resources[i]["orientation"]["z"])) :rot.setZ(0);
+        (resources[i]["orientation"].hasMember("w")) ? rot.setW(float_of(resources[i]["orientation"]["w"])) :rot.setW(0);
+
+        ROS_INFO("--- Wing %s ---", str.c_str());
+        ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+        ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+        ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+
+        wing_data_[std::atoi(match[1].str().c_str())-1].first[0].name_ = str.c_str();
+        wing_data_[std::atoi(match[1].str().c_str())-1].first[0].pose_ = tf2::Transform(rot.normalized(), pos); // mediator->robots()[0]->tf().inverse() *
+        wing_data_[std::atoi(match[1].str().c_str())-1].first[0].size_ = size;
+        wing_data_[std::atoi(match[1].str().c_str())-1].second += std::pow(2, 0);
+      }
+    }
+
+    rx.assign("table([0-9]+)_front_panel");
+    for(int i = 0; i < resources.size(); i++){
+      std::string str = resources[i]["id"];
+      if (std::regex_match(str, match, rx)){
+        ROS_INFO("=> WING: description found, loading...");
+        tf2::Vector3 pos;
+        tf2::Vector3 size;
+        tf2::Quaternion rot;
+
+        
+        (resources[i]["size"].hasMember("length")) ? size.setX(float_of(resources[i]["size"]["length"])) :size.setX(0);
+        (resources[i]["size"].hasMember("width")) ? size.setY(float_of(resources[i]["size"]["width"])) :size.setY(0);
+        (resources[i]["size"].hasMember("height")) ? size.setZ(float_of(resources[i]["size"]["height"])) :size.setZ(0);
+
+        (resources[i]["pos"].hasMember("x")) ? pos.setX(float_of(resources[i]["pos"]["x"])) :pos.setX(0);
+        (resources[i]["pos"].hasMember("y")) ? pos.setY(float_of(resources[i]["pos"]["y"])) :pos.setY(0);
+        (resources[i]["pos"].hasMember("z")) ? pos.setZ(float_of(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
+        (resources[i]["orientation"].hasMember("x")) ? rot.setX(float_of(resources[i]["orientation"]["x"])) :rot.setX(0);
+        (resources[i]["orientation"].hasMember("y")) ? rot.setY(float_of(resources[i]["orientation"]["y"])) :rot.setY(0);
+        (resources[i]["orientation"].hasMember("z")) ? rot.setZ(float_of(resources[i]["orientation"]["z"])) :rot.setZ(0);
+        (resources[i]["orientation"].hasMember("w")) ? rot.setW(float_of(resources[i]["orientation"]["w"])) :rot.setW(0);
+
+        ROS_INFO("--- Wing %s ---", str.c_str());
+        ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+        ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+        ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+
+        wing_data_[std::atoi(match[1].str().c_str())-1].first[1].name_ = str.c_str();
+        wing_data_[std::atoi(match[1].str().c_str())-1].first[1].pose_ = tf2::Transform(rot.normalized(), pos); // mediator->robots()[0]->tf().inverse() *
+        wing_data_[std::atoi(match[1].str().c_str())-1].first[1].size_ = size;
+        wing_data_[std::atoi(match[1].str().c_str())-1].second += std::pow(2, 1);
+
+
+      }
+    }
+
+    rx.assign("table([0-9]+)_left_panel");
+    for(int i = 0; i < resources.size(); i++){
+      std::string str = resources[i]["id"];
+      if (std::regex_match(str, match, rx)){
+        ROS_INFO("=> WING: description found, loading...");
+        tf2::Vector3 pos;
+        tf2::Vector3 size;
+        tf2::Quaternion rot;
+
+        
+        (resources[i]["size"].hasMember("length")) ? size.setX(float_of(resources[i]["size"]["length"])) :size.setX(0);
+        (resources[i]["size"].hasMember("width")) ? size.setY(float_of(resources[i]["size"]["width"])) :size.setY(0);
+        (resources[i]["size"].hasMember("height")) ? size.setZ(float_of(resources[i]["size"]["height"])) :size.setZ(0);
+
+        (resources[i]["pos"].hasMember("x")) ? pos.setX(float_of(resources[i]["pos"]["x"])) :pos.setX(0);
+        (resources[i]["pos"].hasMember("y")) ? pos.setY(float_of(resources[i]["pos"]["y"])) :pos.setY(0);
+        (resources[i]["pos"].hasMember("z")) ? pos.setZ(float_of(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
+        (resources[i]["orientation"].hasMember("x")) ? rot.setX(float_of(resources[i]["orientation"]["x"])) :rot.setX(0);
+        (resources[i]["orientation"].hasMember("y")) ? rot.setY(float_of(resources[i]["orientation"]["y"])) :rot.setY(0);
+        (resources[i]["orientation"].hasMember("z")) ? rot.setZ(float_of(resources[i]["orientation"]["z"])) :rot.setZ(0);
+        (resources[i]["orientation"].hasMember("w")) ? rot.setW(float_of(resources[i]["orientation"]["w"])) :rot.setW(0);
+
+        ROS_INFO("--- Wing %s ---", str.c_str());
+        ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+        ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+        ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+
+        wing_data_[std::atoi(match[1].str().c_str())-1].first[2].name_ = str.c_str();
+        wing_data_[std::atoi(match[1].str().c_str())-1].first[2].pose_ = tf2::Transform(rot.normalized(), pos); // mediator->robots()[0]->tf().inverse() *
+        wing_data_[std::atoi(match[1].str().c_str())-1].first[2].size_ = size;
+        wing_data_[std::atoi(match[1].str().c_str())-1].second += std::pow(2, 2);
+
+      }
+    }
+}
-- 
GitLab