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


collada_urdf
Author(s): Tim Field, Rosen Diankov
autogenerated on Thu Aug 27 2015 14:41:02