GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/urdf-parser.cpp Lines: 1 252 0.4 %
Date: 2020-05-14 11:23:33 Branches: 2 966 0.2 %

Line Branch Exec Source
1
//
2
//  urdf-parser.cpp
3
//  gepetto-viewer
4
//
5
//  Created by Anthony Couret, Mathieu Geisert in November 2014.
6
//  Updated by Joseph Mirabel in February 2019.
7
//  Copyright (c) 2014 LAAS-CNRS. All rights reserved.
8
//
9
10
#include <gepetto/viewer/urdf-parser.h>
11
12
#include <iostream>
13
#include <string>
14
#include <vector>
15
16
#include <QBuffer>
17
#include <QDebug>
18
#include <QDomDocument>
19
#include <QDomElement>
20
#include <QFile>
21
#include <QFileInfo>
22
#include <QMap>
23
#include <QString>
24
#include <QStringList>
25
#include <QtGlobal>
26
27
#include <osg/Version>
28
#if OSG_VERSION_GREATER_OR_EQUAL(3,3,9) && OSG_VERSION_LESS_THAN(3,5,7)
29
#include <osgQt/Version>
30
#endif
31
32
#include <gepetto/viewer/leaf-node-box.h>
33
#include <gepetto/viewer/leaf-node-capsule.h>
34
#include <gepetto/viewer/leaf-node-cylinder.h>
35
#include <gepetto/viewer/leaf-node-sphere.h>
36
37
38
#define OUT(x) qDebug() << x;
39
// #define OUT(x) (void)0
40
41
namespace gepetto {
42
namespace viewer {
43
  namespace urdfParser {
44
45
  namespace details {
46
    typedef std::map<std::string, ::osg::NodeRefPtr> Cache_t;
47
    struct Material {
48
      bool hasColor, hasTexture;
49
      osgVector4 color;
50
      std::string texture;
51
      Material () : hasColor (false), hasTexture (false) {}
52
    };
53
    typedef QMap<QString, Material> MaterialMap_t;
54
    DEF_CLASS_SMART_PTR(LinkNode)
55
56
    class LinkNode :  public GroupNode
57
    {
58
      private:
59
        bool currentIsVisual_;
60
        std::vector<NodePtr_t> visuals_, collisions_;
61
62
      public:
63
        virtual ~LinkNode() {}
64
65
        LinkNode(const std::string& name) : GroupNode(name)
66
        {
67
          addProperty(BoolProperty::create("ShowVisual",
68
                                           BoolProperty::getterFromMemberFunction(this, &LinkNode::showVisual),
69
                                           BoolProperty::setterFromMemberFunction(this, &LinkNode::showVisual)));
70
        }
71
72
        const bool& showVisual () const
73
        {
74
          return currentIsVisual_;
75
        }
76
77
        void showVisual (const bool& visual)
78
        {
79
          if (currentIsVisual_ == visual) return;
80
          VisibilityMode m = (visual ? VISIBILITY_ON : VISIBILITY_OFF);
81
          for (std::size_t i = 0; i < visuals_.size(); ++i)
82
            visuals_[i]->setVisibilityMode(m);
83
          m = (visual ? VISIBILITY_OFF : VISIBILITY_ON);
84
          for (std::size_t i = 0; i < collisions_.size(); ++i)
85
            collisions_[i]->setVisibilityMode(m);
86
          currentIsVisual_ = visual;
87
        }
88
89
        void add (const NodePtr_t& child_ptr, bool vis)
90
        {
91
          VisibilityMode m = (currentIsVisual_ == vis ? VISIBILITY_ON : VISIBILITY_OFF);
92
          addChild(child_ptr);
93
          if (vis) visuals_.push_back(child_ptr);
94
          else collisions_.push_back(child_ptr);
95
          child_ptr->setVisibilityMode(m);
96
        }
97
98
        static LinkNodePtr_t create (const std::string& name)
99
        {
100
          LinkNodePtr_t robot (new LinkNode(name));
101
          robot->initWeakPtr(robot);
102
          return robot;
103
        }
104
    };
105
106
    bool getShowVisuals (const GroupNode* gn)
107
    {
108
      bool value = true;
109
      for (std::size_t i = 0; i < gn->getNumOfChildren (); ++i)
110
      {
111
        NodePtr_t n = gn->getChild(i);
112
        if (n->hasProperty("ShowVisual")) {
113
          bool v;
114
          n->getProperty ("ShowVisual", v);
115
          value = value && v;
116
        }
117
      }
118
      return value;
119
    }
120
    void setShowVisuals (GroupNode* gn, bool visual)
121
    {
122
      for (std::size_t i = 0; i < gn->getNumOfChildren (); ++i)
123
      {
124
        NodePtr_t n = gn->getChild(i);
125
        if (n->hasProperty("ShowVisual"))
126
          n->setProperty ("ShowVisual", visual);
127
      }
128
    }
129
130
    QStringList rosPackagePath()
131
    {
132
      const QString rosPathVar (qgetenv("ROS_PACKAGE_PATH"));
133
      return rosPathVar.split(':');
134
    }
135
136
    std::string getFilename (const QString& input)
137
    {
138
      if (input.startsWith("package://")) {
139
        QStringList rosPaths = rosPackagePath();
140
        for (int i = 0; i < rosPaths.size(); ++i) {
141
          QFileInfo fileInfo (rosPaths[i] + '/' + input.right (input.size()-10));
142
          if (fileInfo.exists() && fileInfo.isFile())
143
            return fileInfo.filePath().toStdString();
144
        }
145
        throw std::invalid_argument (("File not found: " + input
146
              + ". Check ROS_PACKAGE_PATH environment variable.").toStdString());
147
      }
148
      return input.toStdString();
149
    }
150
151
    template <typename ReturnType>
152
    void toDoubles (const QString& s, ReturnType& vect)
153
    {
154
      QStringList nums = s.split (' ', QString::SkipEmptyParts);
155
      if (ReturnType::num_components != nums.size())
156
        throw std::logic_error ("Could not parse " + s.toStdString());
157
      bool ok;
158
      for (int i = 0; i < nums.size(); ++i) {
159
        double d = nums[i].toDouble (&ok);
160
        if (!ok) throw std::logic_error ("Could not parse " + s.toStdString());
161
        vect[i] = d;
162
      }
163
    }
164
165
    template <typename ReturnType>
166
    void toFloats (const QString& s, ReturnType& vect)
167
    {
168
      QStringList nums = s.split (' ', QString::SkipEmptyParts);
169
      if (ReturnType::num_components != nums.size())
170
        throw std::logic_error ("Could not parse " + s.toStdString());
171
      bool ok;
172
      for (int i = 0; i < nums.size(); ++i) {
173
        float d = nums[i].toFloat (&ok);
174
        if (!ok) throw std::logic_error ("Could not parse " + s.toStdString());
175
        vect[i] = d;
176
      }
177
    }
178
179
    void parseOrigin (const QDomElement element, osgVector3& T, osgQuat& R)
180
    {
181
      QDomElement origin = element.firstChildElement ("origin");
182
      if (!origin.nextSiblingElement ("origin").isNull())
183
        throw std::logic_error ("URDF contains two origins.");
184
185
      T = osgVector3 (0,0,0);
186
      R = osgQuat    (0,0,0,1);
187
      if (origin.isNull()) return;
188
      QString xyz = origin.attribute ("xyz");
189
      if (!xyz.isNull())
190
        toFloats (xyz, T);
191
      QString rpy = origin.attribute ("rpy");
192
      if (!rpy.isNull()) {
193
        osgVector3 rpy2;
194
        toFloats (rpy, rpy2);
195
196
        double phi, the, psi;
197
        phi = rpy2[0] / 2.0;
198
        the = rpy2[1] / 2.0;
199
        psi = rpy2[2] / 2.0;
200
201
        R.x() = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
202
        R.y() = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
203
        R.z() = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);
204
        R.w() = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
205
      }
206
    }
207
208
    NodePtr_t createMesh (const QString& name,
209
		  const QDomElement mesh,
210
                  Cache_t& cache)
211
    {
212
      std::string mesh_path = getFilename (mesh.attribute("filename"));
213
214
      Cache_t::const_iterator _cache = cache.find (mesh_path);
215
      LeafNodeColladaPtr_t meshNode;
216
      if (_cache == cache.end()) {
217
        meshNode = LeafNodeCollada::create (name.toStdString(), mesh_path);
218
        cache.insert (std::make_pair(mesh_path, meshNode->getColladaPtr()));
219
      } else
220
        meshNode = LeafNodeCollada::create (name.toStdString(), _cache->second, mesh_path);
221
222
      if (mesh.hasAttribute ("scale")) {
223
        osgVector3 scale (1,1,1);
224
        toFloats (mesh.attribute("scale"), scale);
225
        meshNode->setScale(scale);
226
      }
227
228
      return meshNode;
229
    }
230
231
    /// \param collision collision or visual tag
232
    bool isCapsule (const QDomElement& link,
233
        const QDomElement& collision)
234
    {
235
      QString gname = collision.attribute("name");
236
      if (gname.isNull()) return false;
237
      QDomElement cc = link.firstChildElement("collision_checking");
238
      if (cc.isNull()) return false;
239
240
      for (QDomElement element = cc.firstChildElement ("capsule");
241
          !element.isNull();
242
          element = element.nextSiblingElement ("capsule"))
243
        if (element.attribute("name") == gname) return true;
244
245
      return false;
246
    }
247
248
    Material parseMaterial (const QDomElement material)
249
    {
250
      Material mat;
251
252
      // Set color
253
      QDomElement color = material.firstChildElement ("color");
254
      mat.hasColor = !color.isNull();
255
      if (mat.hasColor) {
256
        toFloats (color.attribute ("rgba"), mat.color);
257
      }
258
259
      // Set texture
260
      QDomElement texture = material.firstChildElement ("texture");
261
      mat.hasTexture = !texture.isNull();
262
      if (mat.hasTexture) {
263
        if (!texture.hasAttribute ("filename"))
264
          throw std::logic_error ("texture tag must have a filename attribute.");
265
        mat.texture = getFilename (texture.attribute("filename"));
266
      }
267
      return mat;
268
    }
269
270
    void setMaterial (const QDomElement material,
271
        NodePtr_t node,
272
        MaterialMap_t& materials)
273
    {
274
      if (material.isNull()) return;
275
      Material mat;
276
277
      if (!material.hasAttribute ("name"))
278
        throw std::logic_error ("material tag must have a name attribute.");
279
280
      QString name = material.attribute ("name");
281
      if (materials.contains (name)) {
282
        mat = materials[name];
283
      } else {
284
        // Parse material
285
        mat = parseMaterial (material);
286
        if (!mat.hasColor && !mat.hasTexture) {
287
          qDebug() << "material tag " << name << " must have either a color or a texture.";
288
          mat.color = osgVector4 (0.9f,0.9f,0.9f,1.f);
289
          mat.hasColor = true;
290
        }
291
        materials[name] = mat;
292
      }
293
294
      // Set color
295
      if (mat.hasColor)
296
        node->setColor (mat.color);
297
298
      // Set texture
299
      if (mat.hasTexture) {
300
        NodeDrawablePtr_t    nd = dynamic_pointer_cast<NodeDrawable   > (node);
301
        LeafNodeColladaPtr_t lc = dynamic_pointer_cast<LeafNodeCollada> (node);
302
        assert (nd || lc);
303
        if      (nd) nd->setTexture(mat.texture);
304
        else if (lc) lc->setTexture(mat.texture);
305
      }
306
    }
307
308
    template <bool visual> void addGeoms(const std::string &robotName,
309
                    const QString& namePrefix,
310
		    const QDomElement& link,
311
		    LinkNodePtr_t &linkNode, bool linkFrame,
312
                    Cache_t& cache,
313
                    MaterialMap_t& materials)
314
    {
315
      static const QString tagName (visual ? "visual" : "collision");
316
      static const QString nameFormat ("%1/%2_%3");
317
      QString name = nameFormat
318
        .arg (robotName.c_str())
319
        .arg (namePrefix);
320
321
      int index = 0;
322
      for (QDomElement element = link.firstChildElement (tagName);
323
          !element.isNull();
324
          element = element.nextSiblingElement (tagName)) {
325
        NodePtr_t node;
326
        QString name_i = name.arg (index);
327
328
        // Parse tag geometry
329
        QDomElement geometry = element.firstChildElement("geometry");
330
        if (!geometry.nextSiblingElement ("geometry").isNull())
331
          throw std::logic_error ("Visual or collision tag contains two geometries.");
332
        int N = 0;
333
        bool ok;
334
        for (QDomElement type = geometry.firstChildElement ();
335
            !type.isNull(); type = type.nextSiblingElement ()) {
336
          if        (type.tagName() == "box") {
337
            // Create box
338
            osgVector3 sideLengths;
339
            toFloats (type.attribute("size"), sideLengths);
340
            node = LeafNodeBox::create (name_i.toStdString (), sideLengths * .5f);
341
            ++N;
342
          } else if (type.tagName() == "cylinder") {
343
            float radius = type.attribute("radius").toFloat (&ok);
344
            if (!ok) throw std::logic_error ("Could not parse cylinder radius.");
345
            float length = type.attribute("length").toFloat (&ok);
346
            if (!ok) throw std::logic_error ("Could not parse cylinder length.");
347
            if (isCapsule (link, element))
348
              node = LeafNodeCapsule::create (name_i.toStdString(), radius, length);
349
            else
350
              node = LeafNodeCylinder::create (name_i.toStdString(), radius, length);
351
            ++N;
352
          } else if (type.tagName() == "sphere") {
353
            float radius = type.attribute("radius").toFloat (&ok);
354
            if (!ok) throw std::logic_error ("Could not parse sphere radius.");
355
            node = LeafNodeSphere::create(name_i.toStdString(), radius);
356
            ++N;
357
          } else if (type.tagName() == "mesh") {
358
            node = createMesh (name_i, type, cache);
359
            ++N;
360
          }
361
        }
362
        if (N != 1)
363
          throw std::logic_error ("geometry tag must contain only one of box, cylinder, sphere or mesh.");
364
365
        // Parse tag origin
366
        if (linkFrame) {
367
          osgVector3 static_pos; osgQuat static_quat;
368
          parseOrigin (element, static_pos, static_quat);
369
          node->setStaticTransform(static_pos,static_quat);
370
        }
371
372
        // Parse tag meterial
373
        setMaterial (element.firstChildElement ("material"),
374
            node, materials);
375
376
        linkNode->add(node, visual);
377
        ++index;
378
      }
379
    }
380
381
    void parseXML (const std::string& urdf_file, QDomDocument& model)
382
    {
383
      bool ok;
384
      QString errorPrefix, errorMsg;
385
      int errorRow, errorCol;
386
387
      if (urdf_file.compare(urdf_file.length() - 5, 5, ".urdf") == 0) {
388
        std::string urdf_file2 = getFilename (QString::fromStdString(urdf_file));
389
        QFile file (urdf_file2.c_str());
390
        ok = model.setContent (&file, false, &errorMsg, &errorRow, &errorCol);
391
        errorPrefix = "Failed to parse ";
392
        errorPrefix +=  urdf_file.c_str();
393
      } else {
394
        QBuffer buffer;
395
        buffer.setData (urdf_file.c_str(), (int)urdf_file.length());
396
        ok = model.setContent (&buffer, false, &errorMsg, &errorRow, &errorCol);
397
        errorPrefix = "Failed to parse XML string";
398
      }
399
400
      if (!ok) {
401
        throw std::invalid_argument ( QString ("%1: %2 at %3:%4")
402
            .arg (errorPrefix)
403
            .arg (errorMsg)
404
            .arg (errorRow)
405
            .arg (errorCol)
406
            .toStdString()
407
            );
408
      }
409
    }
410
  }
411
412
  std::string getFilename (const std::string& input)
413
  {
414
    return details::getFilename (QString::fromStdString(input));
415
  }
416
417
  GroupNodePtr_t parse (const std::string& robotName,
418
			const std::string& urdf_file,
419
			const bool& visual,
420
			const bool& linkFrame)
421
  {
422
    QDomDocument model;
423
424
    // Parse the XML document
425
    details::parseXML (urdf_file, model);
426
427
    // Parse the materials
428
    details::MaterialMap_t materials;
429
    for (QDomElement material = model.firstChildElement("robot").firstChildElement("material");
430
        !material.isNull();
431
        material = material.nextSiblingElement ("material")) {
432
      if (!material.hasAttribute ("name"))
433
        throw std::logic_error ("material tag must have a name attribute.");
434
435
      details::Material mat = details::parseMaterial (material);
436
437
      if (!mat.hasColor && !mat.hasTexture)
438
        throw std::logic_error ("material tag must have either a color or a texture.");
439
      QString name = material.attribute ("name");
440
      materials[name] = mat;
441
    }
442
443
444
    GroupNodePtr_t robot = GroupNode::create(robotName);
445
    QDomNodeList links = model.elementsByTagName ("link");
446
447
    robot->addProperty (
448
        BoolProperty::create("ShowVisual",
449
          BoolProperty::Getter_t(boost::bind(details::getShowVisuals, robot.get())),
450
          BoolProperty::Setter_t(boost::bind(details::setShowVisuals, robot.get(), _1))
451
          ));
452
453
    details::Cache_t cache;
454
    for (int i = 0 ; i < links.size() ; i++) {
455
      QDomElement link = links.at(i).toElement();
456
      if (link.isNull()) throw std::logic_error ("link must be a tag.");
457
      QString name = link.attribute ("name");
458
      if (name.isNull()) throw std::logic_error ("A link has no name attribute.");
459
460
      std::string link_name = name.toStdString();
461
      OUT( link_name.c_str() );
462
463
      details::LinkNodePtr_t linkNode (details::LinkNode
464
          ::create (robotName + "/" + link_name));
465
      linkNode->showVisual(visual);
466
      // add link to robot node
467
      robot->addChild (linkNode);
468
469
      if (visual) {
470
        details::addGeoms<true >(robotName, name, link, linkNode, linkFrame, cache, materials);
471
        if (linkFrame) {
472
          try {
473
            details::addGeoms<false>(robotName, "collision_"+name, link, linkNode, linkFrame, cache, materials);
474
          } catch (const std::invalid_argument& e) {
475
            std::cerr << "Could not load collision geometries of " << robotName << ":"
476
              << e.what() << std::endl;
477
          }
478
        }
479
      } else {
480
        details::addGeoms<false >(robotName, name, link, linkNode, linkFrame, cache, materials);
481
        if (linkFrame) {
482
          try {
483
            details::addGeoms<true>(robotName, "visual_"+name, link, linkNode, linkFrame, cache, materials);
484
          } catch (const std::invalid_argument& e) {
485
            std::cerr << "Could not load visual geometries of " << robotName << ":"
486
              << e.what() << std::endl;
487
          }
488
        }
489
      }
490
    }
491
    return robot;
492
  }
493
  } // namespace urdfParser
494
} /* namespace viewer */
495
496

3
}