collada_to_urdf.cpp
Go to the documentation of this file.
1 /* Author: Yohei Kakiuchi */
2 
3 #include <iostream>
4 #include <fstream>
5 #include <string>
6 #include <vector>
7 
8 #include <ros/console.h>
9 #include <urdf/model.h>
10 
12 #include <urdf_parser/urdf_parser.h>
13 
14 #if defined(ASSIMP_UNIFIED_HEADER_NAMES)
15 #include <assimp/IOSystem.hpp>
16 #include <assimp/IOStream.hpp>
17 #include <assimp/Importer.hpp>
18 #include <assimp/Exporter.hpp>
19 #include <assimp/postprocess.h>
20 #include <assimp/scene.h>
21 #else
22 #include <assimp.hpp>
23 #if defined(ASSIMP_EXPORT_API)
24 #include <assimp/export.hpp>
25 #endif
26 #include <aiScene.h>
27 #include <aiPostProcess.h>
28 #endif
29 
30 #include <boost/program_options.hpp>
31 #include <boost/filesystem/path.hpp>
32 #include <boost/filesystem/operations.hpp>
33 
34 #include <Eigen/Geometry>
35 
36 #undef GAZEBO_1_3
37 
38 #define GAZEBO_1_3
39 
40 using namespace urdf;
41 using namespace std;
42 
43 bool use_simple_visual = false;
44 bool use_simple_collision = false;
46 bool use_assimp_export = false;
49 bool export_collision_mesh = false;
50 
51 string mesh_dir = "/tmp";
52 string arobot_name = "";
53 string output_file = "";
54 string mesh_prefix = "";
55 
56 #define PRINT_ORIGIN(os, origin) \
57 os << "xyz: " << origin.position.x << " " << origin.position.y << " " << origin.position.z << " "; \
58 { double r,p,y; origin.rotation.getRPY(r, p, y); \
59  os << "rpy: " << r << " " << p << " " << y; }
60 
61 #define PRINT_ORIGIN_XML(os, origin) \
62  os << "xyz=\"" << origin.position.x << " " << origin.position.y << " " << origin.position.z << "\""; \
63  { double h___r, h___p, h___y; \
64  origin.rotation.getRPY(h___r, h___p, h___y); \
65  os << " rpy=\"" << h___r << " " << h___p << " " << h___y << "\""; }
66 
67 #define PRINT_GEOM(os, geometry) \
68  if ( geometry->type == urdf::Geometry::MESH ) { os << "geom: name: " << ((urdf::Mesh *)geometry.get())->filename; }
69 
70 void assimp_file_export(std::string fname, std::string ofname,
71  std::string mesh_type = "collada") {
72 #if defined(ASSIMP_EXPORT_API)
73  if (fname.find("file://") == 0) {
74  fname.erase(0, strlen("file://"));
75  }
76  Assimp::Importer importer;
77  /*
78  { // ignore UP_DIRECTION tag in collada
79  bool existing;
80  importer.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true, &existing);
81  if(existing) {
82  fprintf(stderr, ";; OverWrite : Ignore UP_DIRECTION", existing);
83  }
84  }
85  */
86  const aiScene* scene = importer.ReadFile(fname.c_str(),
87  aiProcess_Triangulate |
88  aiProcess_GenNormals |
89  aiProcess_JoinIdenticalVertices |
90  aiProcess_SplitLargeMeshes |
91  aiProcess_OptimizeMeshes |
92  aiProcess_SortByPType);
93 
94  if (!scene) {
95  std::string str( importer.GetErrorString() );
96  std::cerr << ";; " << str << std::endl;
97  return;
98  }
99 
100  Assimp::Exporter aexpt;
101  aiReturn ret = aexpt.Export(scene, mesh_type, ofname);
102  if ( ret != AI_SUCCESS ) {
103  std::string str( "assimp error" );
104  std::cerr << ";; " << str << std::endl;
105  }
106 #endif
107 }
108 
109 // assimp bounding box calculation
110 void assimp_calc_bbox(string fname, float &minx, float &miny, float &minz,
111  float &maxx, float &maxy, float &maxz) {
112 
113  if (fname.find("file://") == 0) {
114  fname.erase(0, strlen("file://"));
115  }
116 
117  Assimp::Importer importer;
118  const aiScene* scene = importer.ReadFile(fname.c_str(),
119  aiProcess_Triangulate |
120  aiProcess_JoinIdenticalVertices |
121  aiProcess_SortByPType); // aiProcess_GenNormals
122  // aiProcess_GenSmoothNormals
123  // aiProcess_SplitLargeMeshes
124  if (!scene) {
125  std::string str( importer.GetErrorString() );
126  std::cerr << ";; " << str << std::endl;
127  return;
128  }
129 
130  aiNode *node = scene->mRootNode;
131 
132  bool found = false;
133  if(node->mNumMeshes > 0 && node->mMeshes != NULL) {
134  std::cerr << "Root node has meshes " << node->mMeshes << std::endl;;
135  found = true;
136  } else {
137  for (unsigned int i=0; i < node->mNumChildren; ++i) {
138  if(node->mChildren[i]->mNumMeshes > 0 && node->mChildren[i]->mMeshes != NULL) {
139  std::cerr << "Child " << i << " has meshes" << std::endl;
140  node = node->mChildren[i];
141  found = true;
142  break;
143  }
144  }
145  }
146  if(found == false) {
147  std::cerr << "Can't find meshes in file" << std::endl;
148  return;
149  }
150 
151  aiMatrix4x4 transform = node->mTransformation;
152 
153  // copy vertices
154  maxx = maxy = maxz = -100000000.0;
155  minx = miny = minz = 100000000.0;
156 
157  std::cerr << ";; num meshes: " << node->mNumMeshes << std::endl;
158  for (unsigned int m = 0; m < node->mNumMeshes; m++) {
159  aiMesh *a = scene->mMeshes[node->mMeshes[m]];
160  std::cerr << ";; num vertices: " << a->mNumVertices << std::endl;
161 
162  for (unsigned int i = 0 ; i < a->mNumVertices ; ++i) {
163  aiVector3D p;
164  p.x = a->mVertices[i].x;
165  p.y = a->mVertices[i].y;
166  p.z = a->mVertices[i].z;
167  p *= transform;
168 
169  if ( maxx < p.x ) {
170  maxx = p.x;
171  }
172  if ( maxy < p.y ) {
173  maxy = p.y;
174  }
175  if ( maxz < p.z ) {
176  maxz = p.z;
177  }
178  if ( minx > p.x ) {
179  minx = p.x;
180  }
181  if ( miny > p.y ) {
182  miny = p.y;
183  }
184  if ( minz > p.z ) {
185  minz = p.z;
186  }
187  }
188  }
189 }
190 
191 void addChildLinkNamesXML(urdf::LinkConstSharedPtr link, ofstream& os)
192 {
193  os << " <link name=\"" << link->name << "\">" << endl;
194  if ( !!link->visual ) {
195  os << " <visual>" << endl;
196 
197  if (!use_simple_visual) {
198  os << " <origin ";
199  PRINT_ORIGIN_XML(os, link->visual->origin);
200  os << "/>" << endl;
201  os << " <geometry>" << endl;
202  if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
203  std::string ifname (((urdf::Mesh *)link->visual->geometry.get())->filename);
204  if (ifname.find("file://") == 0) {
205  ifname.erase(0, strlen("file://"));
206  }
207  std::string ofname (mesh_dir + "/" + link->name + "_mesh.dae");
208 
209  if (use_assimp_export) {
210  // using collada export
211  assimp_file_export (ifname, ofname);
212  } else {
213  // copy to ofname
214  std::ofstream tmp_os;
215  tmp_os.open(ofname.c_str());
216  std::ifstream is;
217  is.open(ifname.c_str());
218  std::string buf;
219  while(is && getline(is, buf)) tmp_os << buf << std::endl;
220  is.close();
221  tmp_os.close();
222  }
223  if (mesh_prefix != "") {
224  os << " <mesh filename=\"" << mesh_prefix + "/" + link->name + "_mesh.dae" << "\" scale=\"1 1 1\" />" << endl;
225  } else {
226  os << " <mesh filename=\"" << "file://" << ofname << "\" scale=\"1 1 1\" />" << endl;
227  }
228  }
229  os << " </geometry>" << endl;
230  } else {
231  // simple visual
232  float ax,ay,az,bx,by,bz;
233  if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
234  assimp_calc_bbox(((urdf::Mesh *)link->visual->geometry.get())->filename,
235  ax, ay, az, bx, by, bz);
236  }
237  os << " <origin ";
238  urdf::Pose pp = link->visual->origin;
239 
240  pp.position.x += ( ax + bx ) / 2 ;
241  pp.position.y += ( ay + by ) / 2 ;
242  pp.position.z += ( az + bz ) / 2 ;
243  PRINT_ORIGIN_XML(os, pp);
244  os << "/>" << endl;
245 
246  os << " <geometry>" << endl;
247  os << " <box size=\"" << bx - ax << " " << by - ay << " " << bz - az << "\"/>" << endl;
248  os << " </geometry>" << endl;
249  }
250  os << " </visual>" << endl;
251  }
252  if ( !!link->collision ) {
253  os << " <collision>" << endl;
254  if (!use_simple_collision) {
255  os << " <origin ";
256  PRINT_ORIGIN_XML(os, link->collision->origin);
257  os << "/>" << endl;
258  os << " <geometry>" << endl;
259 
260  if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
261  std::string ifname;
263  ifname.assign (((urdf::Mesh *)link->visual->geometry.get())->filename);
264  } else {
265  ifname.assign (((urdf::Mesh *)link->collision->geometry.get())->filename);
266  }
267  if (ifname.find("file://") == 0) {
268  ifname.erase(0, strlen("file://"));
269  }
270  std::string oofname;
271  if (export_collision_mesh) {
272  oofname = link->name + "_mesh.stl";
273  } else {
274  oofname = link->name + "_mesh.dae";
275  }
276  std::string ofname = (mesh_dir + "/" + oofname);
277 
278  if (use_assimp_export) {
279  // using collada export
280  if (export_collision_mesh) {
281  assimp_file_export (ifname, ofname, "stl");
282  } else {
283  assimp_file_export (ifname, ofname);
284  }
285  } else {
286  // copy to ofname
287  std::ofstream tmp_os;
288  tmp_os.open(ofname.c_str());
289  std::ifstream is;
290  is.open(ifname.c_str());
291  std::string buf;
292  while(is && getline(is, buf)) tmp_os << buf << std::endl;
293  is.close();
294  tmp_os.close();
295  }
296  if (mesh_prefix != "") {
297  os << " <mesh filename=\"" << mesh_prefix + "/" + oofname;
298  } else {
299  os << " <mesh filename=\"" << "file://" << ofname;
300  }
301  os << "\" scale=\"1 1 1\" />" << endl;
302  }
303  os << " </geometry>" << endl;
304  } else {
305  // simple collision
306  float ax,ay,az,bx,by,bz;
307  if ( link->visual->geometry->type == urdf::Geometry::MESH ) {
308  assimp_calc_bbox(std::string ( ((urdf::Mesh *)link->visual->geometry.get())->filename ),
309  ax, ay, az, bx, by, bz);
310  }
311  os << " <origin ";
312  urdf::Pose pp = link->visual->origin;
313  pp.position.x += ( ax + bx ) / 2 ;
314  pp.position.y += ( ay + by ) / 2 ;
315  pp.position.z += ( az + bz ) / 2 ;
316  PRINT_ORIGIN_XML(os, pp);
317  os << "/>" << endl;
318 
319  os << " <geometry>" << endl;
320  os << " <box size=\"" << bx - ax << " " << by - ay << " " << bz - az << "\"/>" << endl;
321  os << " </geometry>" << endl;
322  }
323  os << " </collision>" << endl;
324  }
325  if ( !!link->inertial ) {
326  if (!rotate_inertia_frame) {
327  os << " <inertial>" << endl;
328  os << " <mass value=\"" << link->inertial->mass << "\" />" << endl;
329  os << " <origin ";
330  PRINT_ORIGIN_XML(os, link->inertial->origin);
331  os << "/>" << endl;
332  os << " <inertia ixx=\"" << link->inertial->ixx << "\" ";
333  os << "ixy=\"" << link->inertial->ixy << "\" ";
334  os << "ixz=\"" << link->inertial->ixz << "\" ";
335  os << "iyy=\"" << link->inertial->iyy << "\" ";
336  os << "iyz=\"" << link->inertial->iyz << "\" ";
337  os << "izz=\"" << link->inertial->izz << "\"/>" << endl;
338  os << " </inertial>" << endl;
339  } else {
340  // rotation should be identity
341  os << " <inertial>" << endl;
342  os << " <mass value=\"" << link->inertial->mass << "\" />" << endl;
343  os << " <origin ";
344 
345  Eigen::Quaterniond qt(link->inertial->origin.rotation.w,
346  link->inertial->origin.rotation.x,
347  link->inertial->origin.rotation.y,
348  link->inertial->origin.rotation.z);
349  Eigen::Matrix3d mat(qt);
350  Eigen::Matrix3d tmat(mat.transpose());
351  Eigen::Matrix3d imat;
352  imat(0, 0) = link->inertial->ixx;
353  imat(0, 1) = link->inertial->ixy;
354  imat(0, 2) = link->inertial->ixz;
355  imat(1, 0) = link->inertial->ixy;
356  imat(1, 1) = link->inertial->iyy;
357  imat(1, 2) = link->inertial->iyz;
358  imat(2, 0) = link->inertial->ixz;
359  imat(2, 1) = link->inertial->iyz;
360  imat(2, 2) = link->inertial->izz;
361 
362 #define DEBUG_MAT(mat) \
363  cout << "#2f((" << mat(0, 0) << " " << mat(0, 1) << " " << mat(0, 2) << ")"; \
364  cout << "(" << mat(1, 0) << " " << mat(1, 1) << " " << mat(1, 2) << ")"; \
365  cout << "(" << mat(2, 0) << " " << mat(2, 1) << " " << mat(2, 2) << "))" << endl;
366 
367 #if DEBUG
368  DEBUG_MAT(mat);
369  DEBUG_MAT(tmat);
370  DEBUG_MAT(imat);
371 #endif
372 
373  imat = ( mat * imat * tmat );
374 
375 #if DEBUG
376  DEBUG_MAT(imat);
377 #endif
378 
379  urdf::Pose t_pose (link->inertial->origin);
380  t_pose.rotation.clear();
381 
382  PRINT_ORIGIN_XML(os, t_pose);
383  os << "/>" << endl;
384 
385  os << " <inertia ixx=\"" << imat(0, 0) << "\" ";
386  os << "ixy=\"" << imat(0, 1) << "\" ";
387  os << "ixz=\"" << imat(0, 2) << "\" ";
388  os << "iyy=\"" << imat(1, 1) << "\" ";
389  os << "iyz=\"" << imat(1, 2) << "\" ";
390  os << "izz=\"" << imat(2, 2) << "\"/>" << endl;
391  os << " </inertial>" << endl;
392  }
393  }
394  os << " </link>" << endl;
395 
396 #ifdef GAZEBO_1_3
397  if ( add_gazebo_description ) {
398  os << " <gazebo reference=\"" << link->name << "\">" << endl;
399  os << " <mu1>0.9</mu1>" << endl;
400  os << " <mu2>0.9</mu2>" << endl;
401  os << " </gazebo>" << endl;
402  }
403 #endif
404 
405  for (std::vector<urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
406  addChildLinkNamesXML(*child, os);
407 }
408 
409 void addChildJointNamesXML(urdf::LinkConstSharedPtr link, ofstream& os)
410 {
411  double r, p, y;
412  for (std::vector<urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
413  (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y);
414  std::string jtype;
415  if ( (*child)->parent_joint->type == urdf::Joint::UNKNOWN ) {
416  jtype = std::string("unknown");
417  } else if ( (*child)->parent_joint->type == urdf::Joint::REVOLUTE ) {
418  jtype = std::string("revolute");
419  } else if ( (*child)->parent_joint->type == urdf::Joint::CONTINUOUS ) {
420  jtype = std::string("continuous");
421  } else if ( (*child)->parent_joint->type == urdf::Joint::PRISMATIC ) {
422  jtype = std::string("prismatic");
423  } else if ( (*child)->parent_joint->type == urdf::Joint::FLOATING ) {
424  jtype = std::string("floating");
425  } else if ( (*child)->parent_joint->type == urdf::Joint::PLANAR ) {
426  jtype = std::string("planar");
427  } else if ( (*child)->parent_joint->type == urdf::Joint::FIXED ) {
428  jtype = std::string("fixed");
429  } else {
431  }
432 
433  os << " <joint name=\"" << (*child)->parent_joint->name << "\" type=\"" << jtype << "\">" << endl;
434  os << " <parent link=\"" << link->name << "\"/>" << endl;
435  os << " <child link=\"" << (*child)->name << "\"/>" << endl;
436  os << " <origin xyz=\"" << (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " ";
437  os << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " ";
438  os << (*child)->parent_joint->parent_to_joint_origin_transform.position.z;
439  os << "\" rpy=\"" << r << " " << p << " " << y << " " << "\"/>" << endl;
440  os << " <axis xyz=\"" << (*child)->parent_joint->axis.x << " ";
441  os << (*child)->parent_joint->axis.y << " " << (*child)->parent_joint->axis.z << "\"/>" << endl;
442  {
443  urdf::JointSharedPtr jt((*child)->parent_joint);
444 
445  if ( !!jt->limits ) {
446  os << " <limit ";
447  os << "lower=\"" << jt->limits->lower << "\"";
448  os << " upper=\"" << jt->limits->upper << "\"";
449  if (jt->limits->effort == 0.0) {
450  os << " effort=\"100\"";
451  } else {
452  os << " effort=\"" << jt->limits->effort << "\"";
453  }
454  os << " velocity=\"" << jt->limits->velocity << "\"";
455  os << " />" << endl;
456  }
457  if ( !!jt->dynamics ) {
458  os << " <dynamics ";
459  os << "damping=\"" << jt->dynamics->damping << "\"";
460  os << " friction=\"" << jt->dynamics->friction << "\"";
461  os << " />" << endl;
462  } else {
463  os << " <dynamics ";
464  os << "damping=\"0.2\"";
465  os << " friction=\"0\"";
466  os << " />" << endl;
467  }
468 #ifdef GAZEBO_1_3
469 #if 0
470  os << " <safety_controller";
471  os << " k_position=\"10\"";
472  os << " k_velocity=\"10\"";
473  os << " soft_lower_limit=\"-10\"";
474  os << " soft_upper_limit=\"10\"";
475  os << "/>" << endl;
476 #endif
477 #endif
478  }
479 
480  os << " </joint>" << endl;
481 
482  if ( add_gazebo_description ) {
483  os << " <transmission type=\"pr2_mechanism_model/SimpleTransmission\" name=\"";
484  os << (*child)->parent_joint->name << "_trans\" >" << endl;
485  os << " <actuator name=\"" << (*child)->parent_joint->name << "_motor\" />" << endl;
486  os << " <joint name=\"" << (*child)->parent_joint->name << "\" />" << endl;
487  os << " <mechanicalReduction>1</mechanicalReduction>" << endl;
488  //os << " <motorTorqueConstant>1</motorTorqueConstant>" << endl;
489  //os << " <pulsesPerRevolution>90000</pulsesPerRevolution>" << endl;
490  os << " </transmission>" << endl;
491 #ifdef GAZEBO_1_3
492  os << " <gazebo reference=\"" << (*child)->parent_joint->name << "\">" << endl;
493  os << " <cfmDamping>0.4</cfmDamping>" << endl;
494  os << " </gazebo>" << endl;
495 #endif
496  }
497  addChildJointNamesXML(*child, os);
498  }
499 }
500 
501 void printTreeXML(urdf::LinkConstSharedPtr link, string name, string file)
502 {
503  std::ofstream os;
504  os.open(file.c_str());
505  os << "<?xml version=\"1.0\"?>" << endl;
506  os << "<robot name=\"" << name << "\"" << endl;
507  os << " xmlns:xi=\"http://www.w3.org/2001/XInclude\">" << endl;
508 
509  addChildLinkNamesXML(link, os);
510 
511  addChildJointNamesXML(link, os);
512 
513  os << "</robot>" << endl;
514  os.close();
515 }
516 
517 namespace po = boost::program_options;
518 // using namespace std;
519 
520 int main(int argc, char** argv)
521 {
522  string inputfile;
523 
524  po::options_description desc("Usage: collada_to_urdf input.dae [options]\n Options for collada_to_urdf");
525  desc.add_options()
526  ("help", "produce help message")
527  ("simple_visual,V", "use bounding box for visual")
528  ("simple_collision,C", "use bounding box for collision")
529  ("export_collision_mesh", "export collision mesh as STL")
530  ("add_gazebo_description,G", "add description for using on gazebo")
531  ("use_assimp_export,A", "use assimp library for exporting mesh")
532  ("use_collision,U", "use collision geometry (default collision is the same as visual)")
533  ("original_inertia_rotation,R", "does not rotate inertia frame")
534  ("robot_name,N", po::value< vector<string> >(), "output robot name")
535  ("mesh_output_dir", po::value< vector<string> >(), "directory for outputing")
536  ("mesh_prefix", po::value< vector<string> >(), "prefix of mesh files")
537  ("output_file,O", po::value< vector<string> >(), "output file")
538  ("input_file", po::value< vector<string> >(), "input file")
539  ;
540 
541  po::positional_options_description p;
542  p.add("input_file", -1);
543 
544  po::variables_map vm;
545  try {
546  po::store(po::command_line_parser(argc, argv).
547  options(desc).positional(p).run(), vm);
548  po::notify(vm);
549  }
550  catch (po::error e) {
551  cerr << ";; option parse error / " << e.what() << endl;
552  return 1;
553  }
554 
555  if (vm.count("help")) {
556  cout << desc << "\n";
557  return 1;
558  }
559  if (vm.count("simple_visual")) {
560  use_simple_visual = true;
561  cerr << ";; Using simple_visual" << endl;
562  }
563  if (vm.count("simple_collision")) {
564  use_simple_collision = true;
565  cerr << ";; Using simple_collision" << endl;
566  }
567  if (vm.count("add_gazebo_description")) {
568  add_gazebo_description = true;
569  cerr << ";; Adding gazebo description" << endl;
570  }
571  if (vm.count("use_assimp_export")) {
572 #if defined(ASSIMP_EXPORT_API)
573  use_assimp_export = true;
574 #endif
575  cerr << ";; Use assimp export" << endl;
576  }
577  if (vm.count("original_inertia_rotation")) {
578  rotate_inertia_frame = false;
579  cerr << ";; Does not rotate inertia frame" << endl;
580  }
581  if (vm.count("export_collision_mesh")) {
582  export_collision_mesh = true;
583  cerr << ";; erxport collision mesh as STL" << endl;
584  }
585  if (vm.count("output_file")) {
586  vector<string> aa = vm["output_file"].as< vector<string> >();
587  cerr << ";; output file is: "
588  << aa[0] << endl;
589  output_file = aa[0];
590  }
591  if (vm.count("robot_name")) {
592  vector<string> aa = vm["robot_name"].as< vector<string> >();
593  cerr << ";; robot_name is: "
594  << aa[0] << endl;
595  arobot_name = aa[0];
596  }
597  if (vm.count("mesh_prefix")) {
598  vector<string> aa = vm["mesh_prefix"].as< vector<string> >();
599  cerr << ";; mesh_prefix is: "
600  << aa[0] << endl;
601  mesh_prefix = aa[0];
602  }
603  if (vm.count("mesh_output_dir")) {
604  vector<string> aa = vm["mesh_output_dir"].as< vector<string> >();
605  cerr << ";; Mesh output directory is: "
606  << aa[0] << endl;
607  mesh_dir = aa[0];
608  // check directory existence
609  boost::filesystem::path mpath( mesh_dir );
610  try {
611  if ( ! boost::filesystem::is_directory(mpath) ) {
612  boost::filesystem::create_directory ( mpath );
613  }
614  }
615  catch ( boost::filesystem::filesystem_error e ) {
616  cerr << ";; mesh output directory error / " << e.what() << endl;
617  return 1;
618  }
619  }
620  if (vm.count("input_file")) {
621  vector<string> aa = vm["input_file"].as< vector<string> >();
622  cerr << ";; Input file is: "
623  << aa[0] << endl;
624  inputfile = aa[0];
625  }
626 
627  if(inputfile == "") {
628  cerr << desc << endl;
629  return 1;
630  }
631 
632  std::string xml_string;
633  std::fstream xml_file(inputfile.c_str(), std::fstream::in);
634  while ( xml_file.good() )
635  {
636  std::string line;
637  std::getline( xml_file, line);
638  xml_string += (line + "\n");
639  }
640  xml_file.close();
641 
642  urdf::ModelInterfaceSharedPtr robot;
643  if( xml_string.find("<COLLADA") != std::string::npos )
644  {
645  ROS_DEBUG("Parsing robot collada xml string");
646  robot = parseCollada(xml_string);
647  }
648  else
649  {
650  ROS_DEBUG("Parsing robot urdf xml string");
651  robot = parseURDF(xml_string);
652  }
653  if (!robot){
654  std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
655  return -1;
656  }
657 
658  if (arobot_name == "") {
659  arobot_name = robot->getName();
660  }
661  if (output_file == "") {
662  output_file = arobot_name + ".urdf";
663  }
664  printTreeXML (robot->getRoot(), arobot_name, output_file);
665 
666  return 0;
667 }
string arobot_name
int main(int argc, char **argv)
bool export_collision_mesh
string mesh_prefix
void printTreeXML(urdf::LinkConstSharedPtr link, string name, string file)
void assimp_file_export(std::string fname, std::string ofname, std::string mesh_type="collada")
string mesh_dir
bool use_same_collision_as_visual
void addChildJointNamesXML(urdf::LinkConstSharedPtr link, ofstream &os)
bool rotate_inertia_frame
double y
void addChildLinkNamesXML(urdf::LinkConstSharedPtr link, ofstream &os)
urdf::ModelInterfaceSharedPtr parseCollada(const std::string &xml_string)
#define PRINT_ORIGIN_XML(os, origin)
bool use_simple_collision
bool use_assimp_export
string output_file
bool use_simple_visual
#define DEBUG_MAT(mat)
r
void run(ClassLoader *loader)
void assimp_calc_bbox(string fname, float &minx, float &miny, float &minz, float &maxx, float &maxy, float &maxz)
bool add_gazebo_description
#define ROS_DEBUG(...)


collada_urdf
Author(s): Tim Field, Rosen Diankov, Ioan Sucan , Jackie Kay
autogenerated on Fri Jun 7 2019 21:50:26