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