collada_to_urdf.cpp
Go to the documentation of this file.
00001 /* Author: Yohei Kakiuchi */
00002 #include <ros/ros.h>
00003 #include <urdf/model.h>
00004 
00005 #include <collada_parser/collada_parser.h>
00006 #include <urdf_parser/urdf_parser.h>
00007 
00008 #if defined(ASSIMP_UNIFIED_HEADER_NAMES)
00009 #include <assimp/IOSystem.hpp>
00010 #include <assimp/IOStream.hpp>
00011 #include <assimp/Importer.hpp>
00012 #include <assimp/Exporter.hpp>
00013 #include <assimp/postprocess.h>
00014 #include <assimp/scene.h>
00015 #else
00016 #include <assimp.hpp>
00017 #if defined(ASSIMP_EXPORT_API)
00018 #include <assimp/export.hpp>
00019 #endif
00020 #include <aiScene.h>
00021 #include <aiPostProcess.h>
00022 #endif
00023 
00024 #include <iostream>
00025 #include <fstream>
00026 
00027 #include <boost/program_options.hpp>
00028 #include <boost/filesystem/path.hpp>
00029 #include <boost/filesystem/operations.hpp>
00030 
00031 #include <tf/LinearMath/Transform.h>
00032 #include <tf/LinearMath/Quaternion.h>
00033 
00034 #undef GAZEBO_1_3
00035 
00036 #define GAZEBO_1_3
00037 
00038 using namespace urdf;
00039 using namespace std;
00040 
00041 bool use_simple_visual = false;
00042 bool use_simple_collision = false;
00043 bool add_gazebo_description = false;
00044 bool use_assimp_export = false;
00045 bool use_same_collision_as_visual = true;
00046 bool rotate_inertia_frame = true;
00047 bool export_collision_mesh = false;
00048 
00049 string mesh_dir = "/tmp";
00050 string arobot_name = "";
00051 string output_file = "";
00052 string mesh_prefix = "";
00053 
00054 #define PRINT_ORIGIN(os, origin) \
00055 os << "xyz: " << origin.position.x << " " << origin.position.y << " " << origin.position.z << " "; \
00056 { double r,p,y; origin.rotation.getRPY(r, p, y); \
00057   os << "rpy: " << r << " " << p << " " << y; }
00058 
00059 #define PRINT_ORIGIN_XML(os, origin) \
00060   os << "xyz=\"" << origin.position.x << " " << origin.position.y << " " << origin.position.z << "\""; \
00061   { double h___r, h___p, h___y; \
00062      origin.rotation.getRPY(h___r, h___p, h___y); \
00063   os << " rpy=\"" << h___r << " " << h___p << " " << h___y << "\""; }
00064 
00065 #define PRINT_GEOM(os, geometry) \
00066   if ( geometry->type == urdf::Geometry::MESH ) { os << "geom: name: " << ((urdf::Mesh *)geometry.get())->filename; }
00067 
00068 void assimp_file_export(std::string fname, std::string ofname,
00069                         std::string mesh_type = "collada") {
00070 #if defined(ASSIMP_EXPORT_API)
00071   if (fname.find("file://") == 0) {
00072     fname.erase(0, strlen("file://"));
00073   }
00074   Assimp::Importer importer;
00075   /*
00076   { // ignore UP_DIRECTION tag in collada
00077     bool existing;
00078     importer.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true, &existing);
00079     if(existing) {
00080       fprintf(stderr, ";; OverWrite : Ignore UP_DIRECTION", existing);
00081     }
00082   }
00083   */
00084   const aiScene* scene = importer.ReadFile(fname.c_str(),
00085                                            aiProcess_Triangulate            |
00086                                            aiProcess_GenNormals             |
00087                                            aiProcess_JoinIdenticalVertices  |
00088                                            aiProcess_SplitLargeMeshes       |
00089                                            aiProcess_OptimizeMeshes         |
00090                                            aiProcess_SortByPType);
00091 
00092   if (!scene) {
00093     std::string str( importer.GetErrorString() );
00094     std::cerr << ";; " << str << std::endl;
00095     return;
00096   }
00097 
00098   Assimp::Exporter aexpt;
00099   aiReturn ret = aexpt.Export(scene, mesh_type, ofname);
00100   if ( ret != AI_SUCCESS ) {
00101     std::string str( "assimp error" );
00102     std::cerr << ";; " << str << std::endl;
00103   }
00104 #endif
00105 }
00106 
00107 // assimp bounding box calculation
00108 void assimp_calc_bbox(string fname, float &minx, float &miny, float &minz,
00109                       float &maxx, float &maxy, float &maxz) {
00110 
00111   if (fname.find("file://") == 0) {
00112     fname.erase(0, strlen("file://"));
00113   }
00114 
00115   Assimp::Importer importer;
00116   const aiScene* scene = importer.ReadFile(fname.c_str(),
00117                                            aiProcess_Triangulate            |
00118                                            aiProcess_JoinIdenticalVertices  |
00119                                            aiProcess_SortByPType);   // aiProcess_GenNormals
00120                                                                      // aiProcess_GenSmoothNormals
00121                                                                      // aiProcess_SplitLargeMeshes
00122   if (!scene) {
00123     std::string str( importer.GetErrorString() );
00124     std::cerr << ";; " << str << std::endl;
00125     return;
00126   }
00127 
00128   aiNode *node = scene->mRootNode;
00129 
00130   bool found = false;
00131   if(node->mNumMeshes > 0 && node->mMeshes != NULL) {
00132     std::cerr << "Root node has meshes " << node->mMeshes << std::endl;;
00133     found = true;
00134   } else {
00135     for (unsigned int i=0; i < node->mNumChildren; ++i) {
00136       if(node->mChildren[i]->mNumMeshes > 0 && node->mChildren[i]->mMeshes != NULL) {
00137         std::cerr << "Child " << i << " has meshes" << std::endl;
00138         node = node->mChildren[i];
00139         found = true;
00140         break;
00141       }
00142     }
00143   }
00144   if(found == false) {
00145     std::cerr << "Can't find meshes in file" << std::endl;
00146     return;
00147   }
00148 
00149   aiMatrix4x4 transform = node->mTransformation;
00150 
00151   // copy vertices
00152   maxx = maxy = maxz = -100000000.0;
00153   minx = miny = minz =  100000000.0;
00154 
00155   std::cerr << ";; num meshes: " << node->mNumMeshes << std::endl;
00156   for (unsigned int m = 0; m < node->mNumMeshes; m++) {
00157     aiMesh *a = scene->mMeshes[node->mMeshes[m]];
00158     std::cerr << ";; num vertices: " << a->mNumVertices << std::endl;
00159 
00160     for (unsigned int i = 0 ; i < a->mNumVertices ; ++i) {
00161       aiVector3D p;
00162       p.x = a->mVertices[i].x;
00163       p.y = a->mVertices[i].y;
00164       p.z = a->mVertices[i].z;
00165       p *= transform;
00166 
00167       if ( maxx < p.x ) {
00168         maxx = p.x;
00169       }
00170       if ( maxy < p.y ) {
00171         maxy = p.y;
00172       }
00173       if ( maxz < p.z ) {
00174         maxz = p.z;
00175       }
00176       if ( minx > p.x ) {
00177         minx = p.x;
00178       }
00179       if ( miny > p.y ) {
00180         miny = p.y;
00181       }
00182       if ( minz > p.z ) {
00183         minz = p.z;
00184       }
00185     }
00186   }
00187 }
00188 
00189 void addChildLinkNamesXML(urdf::LinkConstSharedPtr link, ofstream& os)
00190 {
00191   os << "  <link name=\"" << link->name << "\">" << endl;
00192   if ( !!link->visual ) {
00193     os << "    <visual>" << endl;
00194 
00195     if (!use_simple_visual) {
00196       os << "      <origin ";
00197       PRINT_ORIGIN_XML(os, link->visual->origin);
00198       os << "/>" << endl;
00199       os << "      <geometry>" << endl;
00200       if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
00201         std::string ifname (((urdf::Mesh *)link->visual->geometry.get())->filename);
00202         if (ifname.find("file://") == 0) {
00203           ifname.erase(0, strlen("file://"));
00204         }
00205         std::string ofname (mesh_dir + "/" + link->name + "_mesh.dae");
00206 
00207         if (use_assimp_export) {
00208           // using collada export
00209           assimp_file_export (ifname, ofname);
00210         } else {
00211           // copy to ofname
00212           std::ofstream tmp_os;
00213           tmp_os.open(ofname.c_str());
00214           std::ifstream is;
00215           is.open(ifname.c_str());
00216           std::string buf;
00217           while(is && getline(is, buf)) tmp_os << buf << std::endl;
00218           is.close();
00219           tmp_os.close();
00220         }
00221         if (mesh_prefix != "") {
00222           os << "        <mesh filename=\"" << mesh_prefix + "/" + link->name + "_mesh.dae" << "\" scale=\"1 1 1\" />" << endl;
00223         } else {
00224           os << "        <mesh filename=\"" << "file://" << ofname << "\" scale=\"1 1 1\" />" << endl;
00225         }
00226       }
00227       os << "      </geometry>" << endl;
00228     } else {
00229       // simple visual
00230       float ax,ay,az,bx,by,bz;
00231       if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
00232         assimp_calc_bbox(((urdf::Mesh *)link->visual->geometry.get())->filename,
00233                          ax, ay, az, bx, by, bz);
00234       }
00235       os << "      <origin ";
00236       urdf::Pose pp = link->visual->origin;
00237 
00238       pp.position.x += ( ax + bx ) / 2 ;
00239       pp.position.y += ( ay + by ) / 2 ;
00240       pp.position.z += ( az + bz ) / 2 ;
00241       PRINT_ORIGIN_XML(os, pp);
00242       os << "/>" << endl;
00243 
00244       os << "      <geometry>" << endl;
00245       os << "         <box size=\"" << bx - ax << " " << by - ay << " " << bz - az << "\"/>" << endl;
00246       os << "      </geometry>" << endl;
00247     }
00248     os << "    </visual>" << endl;
00249   }
00250   if ( !!link->collision ) {
00251     os << "    <collision>" << endl;
00252     if (!use_simple_collision) {
00253       os << "      <origin ";
00254       PRINT_ORIGIN_XML(os, link->collision->origin);
00255       os << "/>" << endl;
00256       os << "      <geometry>" << endl;
00257 
00258       if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
00259         std::string ifname;
00260         if (use_same_collision_as_visual) {
00261           ifname.assign (((urdf::Mesh *)link->visual->geometry.get())->filename);
00262         } else {
00263           ifname.assign (((urdf::Mesh *)link->collision->geometry.get())->filename);
00264         }
00265         if (ifname.find("file://") == 0) {
00266           ifname.erase(0, strlen("file://"));
00267         }
00268         std::string oofname;
00269         if (export_collision_mesh) {
00270           oofname = link->name + "_mesh.stl";
00271         } else {
00272           oofname = link->name + "_mesh.dae";
00273         }
00274         std::string ofname = (mesh_dir + "/" + oofname);
00275 
00276         if (use_assimp_export) {
00277           // using collada export
00278           if (export_collision_mesh) {
00279             assimp_file_export (ifname, ofname, "stl");
00280           } else {
00281             assimp_file_export (ifname, ofname);
00282           }
00283         } else {
00284           // copy to ofname
00285           std::ofstream tmp_os;
00286           tmp_os.open(ofname.c_str());
00287           std::ifstream is;
00288           is.open(ifname.c_str());
00289           std::string buf;
00290           while(is && getline(is, buf)) tmp_os << buf << std::endl;
00291           is.close();
00292           tmp_os.close();
00293         }
00294         if (mesh_prefix != "") {
00295           os << "        <mesh filename=\"" << mesh_prefix + "/" + oofname;
00296         } else {
00297           os << "        <mesh filename=\"" << "file://" << ofname;
00298         }
00299         os << "\" scale=\"1 1 1\" />" << endl;
00300       }
00301       os << "      </geometry>" << endl;
00302     } else {
00303       // simple collision
00304       float ax,ay,az,bx,by,bz;
00305       if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
00306         assimp_calc_bbox(std::string ( ((urdf::Mesh *)link->visual->geometry.get())->filename ),
00307                          ax, ay, az, bx, by, bz);
00308       }
00309       os << "      <origin ";
00310       urdf::Pose pp = link->visual->origin;
00311       pp.position.x += ( ax + bx ) / 2 ;
00312       pp.position.y += ( ay + by ) / 2 ;
00313       pp.position.z += ( az + bz ) / 2 ;
00314       PRINT_ORIGIN_XML(os, pp);
00315       os << "/>" << endl;
00316 
00317       os << "      <geometry>" << endl;
00318       os << "         <box size=\"" << bx - ax << " " << by - ay << " " << bz - az << "\"/>" << endl;
00319       os << "      </geometry>" << endl;
00320     }
00321     os << "    </collision>" << endl;
00322   }
00323   if ( !!link->inertial ) {
00324     if (!rotate_inertia_frame) {
00325       os << "    <inertial>" << endl;
00326       os << "      <mass value=\"" << link->inertial->mass << "\" />" << endl;
00327       os << "      <origin ";
00328       PRINT_ORIGIN_XML(os, link->inertial->origin);
00329       os << "/>" << endl;
00330       os << "      <inertia ixx=\"" << link->inertial->ixx << "\" ";
00331       os << "ixy=\"" << link->inertial->ixy << "\" ";
00332       os << "ixz=\"" << link->inertial->ixz << "\" ";
00333       os << "iyy=\"" << link->inertial->iyy << "\" ";
00334       os << "iyz=\"" << link->inertial->iyz << "\" ";
00335       os << "izz=\"" << link->inertial->izz << "\"/>" << endl;
00336       os << "    </inertial>" << endl;
00337     } else {
00338       // rotation should be identity
00339       os << "    <inertial>" << endl;
00340       os << "      <mass value=\"" << link->inertial->mass << "\" />" << endl;
00341       os << "      <origin ";
00342 
00343       tf::Quaternion qt (link->inertial->origin.rotation.x,
00344                          link->inertial->origin.rotation.y,
00345                          link->inertial->origin.rotation.z,
00346                          link->inertial->origin.rotation.w);
00347       tf::Matrix3x3 mat (qt);
00348       tf::Matrix3x3 tmat (mat.transpose());
00349       tf::Matrix3x3 imat (link->inertial->ixx, link->inertial->ixy, link->inertial->ixz,
00350                           link->inertial->ixy, link->inertial->iyy, link->inertial->iyz,
00351                           link->inertial->ixz, link->inertial->iyz, link->inertial->izz);
00352 #define DEBUG_MAT(mat)                                                  \
00353       cout << "#2f((" << mat[0][0] << " " << mat[0][1] << " " << mat[0][2] << ")"; \
00354       cout << "(" << mat[1][0] << " " << mat[1][1] << " " << mat[1][2] << ")"; \
00355       cout << "(" << mat[2][0] << " " << mat[2][1] << " " << mat[2][2] << "))" << endl;
00356 
00357 #if DEBUG
00358       DEBUG_MAT(mat);
00359       DEBUG_MAT(tmat);
00360       DEBUG_MAT(imat);
00361 #endif
00362 
00363       imat = ( mat * imat * tmat );
00364 
00365 #if DEBUG
00366       DEBUG_MAT(imat);
00367 #endif
00368 
00369       urdf::Pose t_pose (link->inertial->origin);
00370       t_pose.rotation.clear();
00371 
00372       PRINT_ORIGIN_XML(os, t_pose);
00373       os << "/>" << endl;
00374 
00375       os << "      <inertia ixx=\"" << imat[0][0] << "\" ";
00376       os << "ixy=\"" << imat[0][1] << "\" ";
00377       os << "ixz=\"" << imat[0][2] << "\" ";
00378       os << "iyy=\"" << imat[1][1] << "\" ";
00379       os << "iyz=\"" << imat[1][2] << "\" ";
00380       os << "izz=\"" << imat[2][2] << "\"/>" << endl;
00381       os << "    </inertial>" << endl;
00382     }
00383   }
00384   os << "  </link>" << endl;
00385 
00386 #ifdef GAZEBO_1_3
00387   if ( add_gazebo_description ) {
00388     os << "  <gazebo reference=\"" << link->name << "\">" << endl;
00389     os << "    <mu1>0.9</mu1>" << endl;
00390     os << "    <mu2>0.9</mu2>" << endl;
00391     os << "  </gazebo>" << endl;
00392   }
00393 #endif
00394 
00395   for (std::vector<urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
00396     addChildLinkNamesXML(*child, os);
00397 }
00398 
00399 void addChildJointNamesXML(urdf::LinkConstSharedPtr link, ofstream& os)
00400 {
00401   double r, p, y;
00402   for (std::vector<urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
00403     (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y);
00404     std::string jtype;
00405     if ( (*child)->parent_joint->type == urdf::Joint::UNKNOWN ) {
00406       jtype = std::string("unknown");
00407     } else if ( (*child)->parent_joint->type == urdf::Joint::REVOLUTE ) {
00408       jtype = std::string("revolute");
00409     } else if ( (*child)->parent_joint->type == urdf::Joint::CONTINUOUS ) {
00410       jtype = std::string("continuous");
00411     } else if ( (*child)->parent_joint->type == urdf::Joint::PRISMATIC ) {
00412       jtype = std::string("prismatic");
00413     } else if ( (*child)->parent_joint->type == urdf::Joint::FLOATING ) {
00414       jtype = std::string("floating");
00415     } else if ( (*child)->parent_joint->type == urdf::Joint::PLANAR ) {
00416       jtype = std::string("planar");
00417     } else if ( (*child)->parent_joint->type == urdf::Joint::FIXED ) {
00418       jtype = std::string("fixed");
00419     } else {
00421     }
00422 
00423     os << "  <joint name=\"" <<  (*child)->parent_joint->name << "\" type=\"" << jtype << "\">" << endl;
00424     os << "    <parent link=\"" << link->name << "\"/>" << endl;
00425     os << "    <child  link=\"" <<  (*child)->name << "\"/>" << endl;
00426     os << "    <origin xyz=\"" <<  (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " ";
00427     os << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " ";
00428     os << (*child)->parent_joint->parent_to_joint_origin_transform.position.z;
00429     os << "\" rpy=\"" << r << " " << p << " " << y << " " << "\"/>" << endl;
00430     os << "    <axis   xyz=\"" <<  (*child)->parent_joint->axis.x << " ";
00431     os << (*child)->parent_joint->axis.y << " " << (*child)->parent_joint->axis.z << "\"/>" << endl;
00432     {
00433       urdf::JointSharedPtr jt((*child)->parent_joint);
00434 
00435       if ( !!jt->limits ) {
00436         os << "    <limit ";
00437         os << "lower=\"" << jt->limits->lower << "\"";
00438         os << " upper=\"" << jt->limits->upper << "\"";
00439         if (jt->limits->effort == 0.0) {
00440           os << " effort=\"100\"";
00441         } else {
00442           os << " effort=\"" << jt->limits->effort << "\"";
00443         }
00444         os << " velocity=\"" << jt->limits->velocity << "\"";
00445         os << " />" << endl;
00446       }
00447       if ( !!jt->dynamics ) {
00448         os << "    <dynamics ";
00449         os << "damping=\"" << jt->dynamics->damping << "\"";
00450         os << " friction=\"" << jt->dynamics->friction << "\"";
00451         os << " />" << endl;
00452       } else {
00453         os << "    <dynamics ";
00454         os << "damping=\"0.2\"";
00455         os << " friction=\"0\"";
00456         os << " />" << endl;
00457       }
00458 #ifdef GAZEBO_1_3
00459 #if 0
00460       os << "    <safety_controller";
00461       os << " k_position=\"10\"";
00462       os << " k_velocity=\"10\"";
00463       os << " soft_lower_limit=\"-10\"";
00464       os << " soft_upper_limit=\"10\"";
00465       os << "/>" << endl;
00466 #endif
00467 #endif
00468     }
00469 
00470     os << "  </joint>" << endl;
00471 
00472     if ( add_gazebo_description ) {
00473       os << "  <transmission type=\"pr2_mechanism_model/SimpleTransmission\" name=\"";
00474       os << (*child)->parent_joint->name << "_trans\" >" << endl;
00475       os << "    <actuator name=\"" << (*child)->parent_joint->name << "_motor\" />" << endl;
00476       os << "    <joint name=\"" << (*child)->parent_joint->name << "\" />" << endl;
00477       os << "    <mechanicalReduction>1</mechanicalReduction>" << endl;
00478       //os << "    <motorTorqueConstant>1</motorTorqueConstant>" << endl;
00479       //os << "    <pulsesPerRevolution>90000</pulsesPerRevolution>" << endl;
00480       os << "  </transmission>" << endl;
00481 #ifdef GAZEBO_1_3
00482       os << "  <gazebo reference=\"" << (*child)->parent_joint->name << "\">" << endl;
00483       os << "    <cfmDamping>0.4</cfmDamping>" << endl;
00484       os << "  </gazebo>" << endl;
00485 #endif
00486     }
00487     addChildJointNamesXML(*child, os);
00488   }
00489 }
00490 
00491 void printTreeXML(urdf::LinkConstSharedPtr link, string name, string file)
00492 {
00493   std::ofstream os;
00494   os.open(file.c_str());
00495   os << "<?xml version=\"1.0\"?>" << endl;
00496   os << "<robot name=\"" << name << "\"" << endl;
00497   os << "       xmlns:xi=\"http://www.w3.org/2001/XInclude\">" << endl;
00498 
00499   addChildLinkNamesXML(link, os);
00500 
00501   addChildJointNamesXML(link, os);
00502 
00503   os << "</robot>" << endl;
00504   os.close();
00505 }
00506 
00507 namespace po = boost::program_options;
00508 // using namespace std;
00509 
00510 int main(int argc, char** argv)
00511 {
00512   string inputfile;
00513 
00514   po::options_description desc("Usage: collada_to_urdf input.dae [options]\n  Options for collada_to_urdf");
00515   desc.add_options()
00516     ("help", "produce help message")
00517     ("simple_visual,V", "use bounding box for visual")
00518     ("simple_collision,C", "use bounding box for collision")
00519     ("export_collision_mesh", "export collision mesh as STL")
00520     ("add_gazebo_description,G", "add description for using on gazebo")
00521     ("use_assimp_export,A", "use assimp library for exporting mesh")
00522     ("use_collision,U", "use collision geometry (default collision is the same as visual)")
00523     ("original_inertia_rotation,R", "does not rotate inertia frame")
00524     ("robot_name,N", po::value< vector<string> >(), "output robot name")
00525     ("mesh_output_dir", po::value< vector<string> >(), "directory for outputing")
00526     ("mesh_prefix", po::value< vector<string> >(), "prefix of mesh files")
00527     ("output_file,O", po::value< vector<string> >(), "output file")
00528     ("input_file", po::value< vector<string> >(), "input file")
00529     ;
00530 
00531   po::positional_options_description p;
00532   p.add("input_file", -1);
00533 
00534   po::variables_map vm;
00535   try {
00536     po::store(po::command_line_parser(argc, argv).
00537               options(desc).positional(p).run(), vm);
00538     po::notify(vm);
00539   }
00540   catch (po::error e) {
00541     cerr << ";; option parse error / " << e.what() << endl;
00542     return 1;
00543   }
00544 
00545   if (vm.count("help")) {
00546     cout << desc << "\n";
00547     return 1;
00548   }
00549   if (vm.count("simple_visual")) {
00550     use_simple_visual = true;
00551     cerr << ";; Using simple_visual" << endl;
00552   }
00553   if (vm.count("simple_collision")) {
00554     use_simple_collision = true;
00555     cerr << ";; Using simple_collision" << endl;
00556   }
00557   if (vm.count("add_gazebo_description")) {
00558     add_gazebo_description = true;
00559     cerr << ";; Adding gazebo description" << endl;
00560   }
00561   if (vm.count("use_assimp_export")) {
00562 #if defined(ASSIMP_EXPORT_API)
00563     use_assimp_export = true;
00564 #endif
00565     cerr << ";; Use assimp export" << endl;
00566   }
00567   if (vm.count("original_inertia_rotation")) {
00568     rotate_inertia_frame = false;
00569     cerr << ";; Does not rotate inertia frame" << endl;
00570   }
00571   if (vm.count("export_collision_mesh")) {
00572     export_collision_mesh = true;
00573     cerr << ";; erxport collision mesh as STL" << endl;
00574   }
00575   if (vm.count("output_file")) {
00576     vector<string> aa = vm["output_file"].as< vector<string> >();
00577     cerr << ";; output file is: "
00578          <<  aa[0] << endl;
00579     output_file = aa[0];
00580   }
00581   if (vm.count("robot_name")) {
00582     vector<string> aa = vm["robot_name"].as< vector<string> >();
00583     cerr << ";; robot_name is: "
00584          <<  aa[0] << endl;
00585     arobot_name = aa[0];
00586   }
00587   if (vm.count("mesh_prefix")) {
00588     vector<string> aa = vm["mesh_prefix"].as< vector<string> >();
00589     cerr << ";; mesh_prefix is: "
00590          <<  aa[0] << endl;
00591     mesh_prefix = aa[0];
00592   }
00593   if (vm.count("mesh_output_dir")) {
00594     vector<string> aa = vm["mesh_output_dir"].as< vector<string> >();
00595     cerr << ";; Mesh output directory is: "
00596          <<  aa[0] << endl;
00597     mesh_dir = aa[0];
00598     // check directory existence
00599     boost::filesystem::path mpath( mesh_dir );
00600     try {
00601       if ( ! boost::filesystem::is_directory(mpath) ) {
00602         boost::filesystem::create_directory ( mpath );
00603       }
00604     }
00605     catch ( boost::filesystem::filesystem_error e ) {
00606       cerr << ";; mesh output directory error / " << e.what() << endl;
00607       return 1;
00608     }
00609   }
00610   if (vm.count("input_file")) {
00611     vector<string> aa = vm["input_file"].as< vector<string> >();
00612     cerr << ";; Input file is: "
00613          <<  aa[0] << endl;
00614     inputfile = aa[0];
00615   }
00616 
00617   if(inputfile == "") {
00618     cerr << desc << endl;
00619     return 1;
00620   }
00621 
00622   std::string xml_string;
00623   std::fstream xml_file(inputfile.c_str(), std::fstream::in);
00624   while ( xml_file.good() )
00625   {
00626     std::string line;
00627     std::getline( xml_file, line);
00628     xml_string += (line + "\n");
00629   }
00630   xml_file.close();
00631 
00632   urdf::ModelInterfaceSharedPtr robot;
00633   if( xml_string.find("<COLLADA") != std::string::npos )
00634   {
00635     ROS_DEBUG("Parsing robot collada xml string");
00636     robot = parseCollada(xml_string);
00637   }
00638   else
00639   {
00640     ROS_DEBUG("Parsing robot urdf xml string");
00641     robot = parseURDF(xml_string);
00642   }
00643   if (!robot){
00644     std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
00645     return -1;
00646   }
00647 
00648   if (arobot_name == "") {
00649     arobot_name = robot->getName();
00650   }
00651   if (output_file == "") {
00652     output_file =  arobot_name + ".urdf";
00653   }
00654   printTreeXML (robot->getRoot(), arobot_name, output_file);
00655 
00656   return 0;
00657 }


collada_urdf
Author(s): Tim Field, Rosen Diankov, Ioan Sucan , Jackie Kay
autogenerated on Fri Aug 11 2017 02:47:50