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