some fixes

src/mtc/src/impl/mediator.cpp

@@ -301,7 +301,7 @@ void Mediator::write_file(Robot* A, Robot* B){
 
     float pos_x = A->tf().getOrigin().getX();
     float pos_y = A->tf().getOrigin().getY();
-    float pos_z = A->tf().getOrigin().getZ() *2 - size_z/2;
+    float pos_z = A->tf().getOrigin().getZ() *2 ;
     float rot_x = A->tf().getRotation().getX();
     float rot_y = A->tf().getRotation().getY();
     float rot_z = A->tf().getRotation().getZ();
@@ -332,7 +332,7 @@ void Mediator::write_file(Robot* A, Robot* B){
 
     pos_x = B->tf().getOrigin().getX();
     pos_y = B->tf().getOrigin().getY();
-    pos_z = B->tf().getOrigin().getZ() *2 - size_z/2;
+    pos_z = B->tf().getOrigin().getZ() *2 ;
     rot_x = B->tf().getRotation().getX();
     rot_y = B->tf().getRotation().getY();
     rot_z = B->tf().getRotation().getZ();
@@ -364,7 +364,7 @@ void Mediator::write_file(Robot* A, Robot* B){
         float width = w->size().getY();
         float height = w->size().getZ();
 
-        ss << "{ 'id': '" << w->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z + height/2) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n";
+        ss << "{ 'id': '" << w->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z - 0.25*height) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n";
     }
 
     
@@ -383,7 +383,7 @@ void Mediator::write_file(Robot* A, Robot* B){
         float width = w->size().getY();
         float height = w->size().getZ();
 
-        ss << "{ 'id': '" << w->name() << "',  'pos': { 'x': "<< std::to_string(x) << ", 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z + height/2) << " },'size': { 'length': "<< std::to_string(length) << ",'width': "<< std::to_string(width) << ",'height': "<< std::to_string(height) << " },'orientation': { 'x': "<< std::to_string(qx) << ", 'y': "<< std::to_string(qy) << ", 'z': "<< std::to_string(qz) << ", 'w': "<< std::to_string(qw) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, \n";
+        ss << "{ 'id': '" << w->name() << "',  'pos': { 'x': "<< std::to_string(x) << ", 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z - 0.25*height) << " },'size': { 'length': "<< std::to_string(length) << ",'width': "<< std::to_string(width) << ",'height': "<< std::to_string(height) << " },'orientation': { 'x': "<< std::to_string(qx) << ", 'y': "<< std::to_string(qy) << ", 'z': "<< std::to_string(qz) << ", 'w': "<< std::to_string(qw) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, \n";
     }
     tf2::Transform tf_arm =  A->tf() * A->root_tf();
     float arm_x = tf_arm.getOrigin().getX();

src/mtc/src/mtc2taskspace.cpp

@@ -6,7 +6,9 @@ int main(int argc, char **argv){
     ros::init(argc, argv, "mtc2taskspace");
     ros::NodeHandle n;
 
-    std::string path;
+    std::string filename;
+    n.getParam("/resource_name", filename);
+
     XmlRpc::XmlRpcValue resources;
     n.getParam("/objects", resources);
 
@@ -60,34 +62,25 @@ int main(int argc, char **argv){
 
     // lets make some result data
     ROS_INFO("writing file...");
-    std::ofstream o(ros::package::getPath("mtc") + "/descriptions/dummy2.yaml");
+    std::ofstream o(ros::package::getPath("mtc") + "/descriptions/" + filename);
     YAML::Node node;
 
     ROS_INFO("writing items for robot1...");
     for(tf2::Vector3& vec : robot_1){
-      node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));
+      node["task"]["groups"]["panda_arm1"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));
     }
 
     ROS_INFO("writing items for robot2...");
     for(tf2::Vector3& vec : robot_2){
-      node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));
+      node["task"]["groups"]["panda_arm2"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));
     }
 
     ROS_INFO("writing intersection...");
     for(tf2::Vector3& vec : robot_middle){
-      node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
-      node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
+      node["task"]["groups"]["panda_arm1"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
+      node["task"]["groups"]["panda_arm2"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
     }
 
-    ROS_INFO("writing intersection...");
-
-    for(tf2::Vector3& vec : robot_middle){
-      node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
-      node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
-    }
-
-    node.SetStyle(YAML::EmitterStyle::Flow);
-
     o << node;
     return 0;
 }
\ No newline at end of file