00001
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
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
00079
00080
00081
00082
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
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);
00122
00123
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
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
00211 assimp_file_export (ifname, ofname);
00212 } else {
00213
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
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
00280 if (export_collision_mesh) {
00281 assimp_file_export (ifname, ofname, "stl");
00282 } else {
00283 assimp_file_export (ifname, ofname);
00284 }
00285 } else {
00286
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
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
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
00393
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
00492
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
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
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
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 }