generate 'sortierkonfiguartion' for each robot as result, can be run with...

generate 'sortierkonfiguartion' for each robot as result, can be run with auerswald package, ToDo: generate launch files

results/dummy/1938901935/configs/1938901935_panda_arm2.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100004, 1.305, 0.91], 'rot': [0, -0, 1.33714e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100004, 1.305, 0.99], 'rot': [0, -0, 1.33714e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/2082550165/configs/2082550165_panda_arm1.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -0.200002, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -0.200002, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/2082550165/configs/2082550165_panda_arm2.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-6.04679e-06, 1.4025, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-6.04679e-06, 1.4025, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/493882106/configs/493882106_panda_arm1.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100002, 0.099998, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100002, 0.099998, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/493882106/configs/493882106_panda_arm2.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100005, 1.405, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100005, 1.405, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/66805984/configs/66805984_panda_arm1.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.200002, -2.04146e-06, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.200002, -2.04146e-06, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/66805984/configs/66805984_panda_arm2.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.200005, 1.305, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.200005, 1.305, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/-1704450111.yaml → results/dummy/812255525/812255525.yaml

@@ -7,7 +7,7 @@
 { 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table1_table_top', 'pos': { 'x': -0.000002 , 'y': -0.200002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
+{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': -0.000002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
 { 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
 { 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
@@ -16,11 +16,11 @@
 { 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table2_table_top', 'pos': { 'x': -0.000006 , 'y': 1.402498 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000003 } },
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.200000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel' , 'pos': { 'x': -0.000008 , 'y': 2.054998 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005 , 'y': 1.304998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000003 } },
+{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_right_panel' , 'pos': { 'x': -0.100007 , 'y': 1.957498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
 { 'id': 'blue1',  'type': 'BOX', 'pos': { 'x': 0.1, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
 { 'id': 'blue2',  'type': 'BOX', 'pos': { 'x': 0.2, 'y': 0.3, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
 { 'id': 'blue3',  'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.1, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
@@ -28,6 +28,6 @@
 { 'id': 'green2',  'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.91, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
 { 'id': 'red1',  'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.6, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
 { 'id': 'red2',  'type': 'BOX', 'pos': { 'x': 0.3, 'y': 1.41, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
-{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.200003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
-{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220006, 'y': 1.402497, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
 ]}
\ No newline at end of file

results/dummy/812255525/configs/812255525_panda_arm1.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100002, -2.04146e-06, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100002, -2.04146e-06, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/812255525/configs/812255525_panda_arm2.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100005, 1.305, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100005, 1.305, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/95613651/configs/95613651_panda_arm1.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -2.04146e-06, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -2.04146e-06, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/95613651/configs/95613651_panda_arm2.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-5.30321e-06, 1.305, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-5.30321e-06, 1.305, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

src/config_routine.cpp

@@ -23,9 +23,6 @@ int main(int argc, char *argv[]) {
         mediator->connectRobots(std::move(ceti_panda));
     }
 
-    std::string demo_robot;
-    n->getParam("/reference_robot", demo_robot);
-    mediator->setReferenceRobot(demo_robot);
     mediator->mediate();
 
     while (ros::ok()){

src/mediator/base_calculation_mediator.cpp

@@ -553,6 +553,12 @@ void Base_calculation_mediator::write_file(){
 */
 
 void BaseCalculationMediator::writeFile(std::vector<protobuf_entry>& wc_solution){
+
+    std::string resultFile = std::to_string(static_cast<int>(ros::Time::now().toNSec()));
+    std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" +  filename_ + "/" + resultFile);
+    std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" +  filename_ + "/" + resultFile + "/configs");
+    std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" +  filename_ + "/" + resultFile + "/launch");
+
     std::stringstream ss;
     std::stringstream panel_ss;
     std::stringstream root_ss;
@@ -604,6 +610,45 @@ void BaseCalculationMediator::writeFile(std::vector<protobuf_entry>& wc_solution
             ss << "{ 'id' : 'table" << match[1] << "_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
             ss << "{ 'id' : 'table" << match[1] << "_table_top', 'pos': { 'x': " << std::to_string(pos_x) << " , 'y': "<< std::to_string(pos_y) << " , 'z': "<< std::to_string(pos_z) << " },'size': { 'length': "<< std::to_string(size_x) << " ,'width': "<< std::to_string(size_y) << " ,'height': "<< std::to_string(size_z) << " },'orientation': { 'x': " << std::to_string(rot_x) << " , 'y': " << std::to_string(rot_y) << " , 'z': " << std::to_string(rot_z) << " , 'w': " << std::to_string(rot_w) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': " << std::to_string(r) << " , 'p': " << std::to_string(p) << " , 'y': " << std::to_string(y) << " } },\n";
 
+            std::stringstream config;
+            config << "mapspace: {'dim': [";
+            config << size_x/2 << ", ";
+            config << size_y/2 << ", 0.04], ";
+            config << "'pos': [";
+            config << pos_x << ", ";
+            config << pos_y << ", ";
+            config << (pos_z + size_z/2.0f + 0.04f/2.0f) << "], ";
+            config << "'rot': [";
+            config << r << ", " << p << ", " << y <<"], ";
+            config << "'verbose': false }\n";
+
+            config << "voxel_size: 0.02 \n";
+
+            config << "voxel_space: {'dim': [";
+            config << (size_x + 0.1f)/2 << ", ";
+            config << (size_y + 0.1f)/2 << ", 0.2], ";
+            config << "'pos': [";
+            config << pos_x << ", ";
+            config << pos_y << ", ";
+            config << (pos_z + size_z/2.0f + 0.2f/2.0f) << "], ";
+            config << "'rot': [";
+            config << r << ", " << p << ", " << y <<"]}\n";
+
+            config << "voxel_verbose_level: 0\n";
+            config << "translation_rate: 0.03\n";
+            config << "rotation_rate: 45.0\n";
+            config << "rotation_max: 90.0\n";
+            config << "clearance: 0.01\n";
+            config << "min_quality: 0.1\n";
+            config << "max_candidate_count: 10\n";
+            config << "allow_dependencies: true\n";
+            config << "top_grasps_only: true\n";
+
+            std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/configs/" + resultFile + "_"+ wc_solution[c].name_+".yaml");
+            o << config.str();
+            o.close();
+
+
             std::bitset<3> panel_mask(wc_solution[c].wing_config_);
             for(std::size_t i = 0; i < ceti->observers().size(); i++){
                 if(ceti->observerMask()[i] & panel_mask[i]){
@@ -645,7 +690,7 @@ void BaseCalculationMediator::writeFile(std::vector<protobuf_entry>& wc_solution
    
 
 
-    std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + std::to_string(static_cast<int>(ros::Time::now().toNSec())) + ".yaml");
+    std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/" + resultFile + ".yaml");
     ss << panel_ss.str();
     ss << box_ss.str();
     ss << root_ss.str();

src/mediator/grasp_mediator.cpp

@@ -2,9 +2,12 @@
 
 GraspMediator::GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh) 
     : AbstractMediator(nh)
-    , grasp_pipeline_demo_(std::make_unique<moveit_grasps_demo::GraspPipelineDemo>())
     , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
+    , grasp_pipeline_demo_(std::make_unique<moveit_grasps_demo::GraspPipelineDemo>())
     {
+        nh->getParam("/referenceRobot", referenceRobot_); 
+        nh->getParam("/autoMapping", autoMapping_);
+        
     }
 
 void GraspMediator::connectRobots(std::unique_ptr<AbstractRobotDecorator> robot) {
@@ -51,6 +54,7 @@ void GraspMediator::setPanel(){
 
 void GraspMediator::mediate() {
     setPanel();
+    //if(autoMapping_)mapReconfigure();
     ros::Duration(2).sleep();
     std::vector<Cuboid> relevant_box;
     std::vector<Cuboid> relevant_obstacle;