Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/urdf-parser.cpp	Lines:	0	254	0.0 %
Date:	2024-04-14 11:13:22	Branches:	0	704	0.0 %


//
//  urdf-parser.cpp
//  gepetto-viewer
//
//  Created by Anthony Couret, Mathieu Geisert in November 2014.
//  Updated by Joseph Mirabel in February 2019.
//  Copyright (c) 2014 LAAS-CNRS. All rights reserved.
//

#include <gepetto/viewer/urdf-parser.h>

#include <QBuffer>
#include <QDebug>
#include <QDomDocument>
#include <QDomElement>
#include <QFile>
#include <QFileInfo>
#include <QMap>
#include <QString>
#include <QStringList>
#include <QtGlobal>
#include <iostream>
#include <osg/Version>
#include <string>
#include <vector>
#if OSG_VERSION_GREATER_OR_EQUAL(3, 3, 9) && OSG_VERSION_LESS_THAN(3, 5, 7)
#include <osgQt/Version>
#endif

#include <gepetto/viewer/leaf-node-box.h>
#include <gepetto/viewer/leaf-node-capsule.h>
#include <gepetto/viewer/leaf-node-cylinder.h>
#include <gepetto/viewer/leaf-node-sphere.h>

#include "log.hh"

namespace gepetto {
namespace viewer {
namespace urdfParser {

namespace details {
typedef std::map<std::string, ::osg::NodeRefPtr> Cache_t;
struct Material {
  bool hasColor, hasTexture;
  osgVector4 color;
  std::string texture;
  Material() : hasColor(false), hasTexture(false) {}
};
typedef QMap<QString, Material> MaterialMap_t;
DEF_CLASS_SMART_PTR(LinkNode)

class LinkNode : public GroupNode {
 private:
  bool currentIsVisual_;
  std::vector<NodePtr_t> visuals_, collisions_;

 public:
  virtual ~LinkNode() {}

  LinkNode(const std::string& name) : GroupNode(name) {
    addProperty(BoolProperty::create(
        "ShowVisual",
        BoolProperty::getterFromMemberFunction(this, &LinkNode::showVisual),
        BoolProperty::setterFromMemberFunction(this, &LinkNode::showVisual)));
  }

  const bool& showVisual() const { return currentIsVisual_; }

  void showVisual(const bool& visual) {
    if (currentIsVisual_ == visual) return;
    VisibilityMode m = (visual ? VISIBILITY_ON : VISIBILITY_OFF);
    for (std::size_t i = 0; i < visuals_.size(); ++i)
      visuals_[i]->setVisibilityMode(m);
    m = (visual ? VISIBILITY_OFF : VISIBILITY_ON);
    for (std::size_t i = 0; i < collisions_.size(); ++i)
      collisions_[i]->setVisibilityMode(m);
    currentIsVisual_ = visual;
  }

  void add(const NodePtr_t& child_ptr, bool vis) {
    VisibilityMode m =
        (currentIsVisual_ == vis ? VISIBILITY_ON : VISIBILITY_OFF);
    addChild(child_ptr);
    if (vis)
      visuals_.push_back(child_ptr);
    else
      collisions_.push_back(child_ptr);
    child_ptr->setVisibilityMode(m);
  }

  static LinkNodePtr_t create(const std::string& name) {
    LinkNodePtr_t robot(new LinkNode(name));
    robot->initWeakPtr(robot);
    return robot;
  }
};

bool getShowVisuals(const GroupNode* gn) {
  bool value = true;
  for (std::size_t i = 0; i < gn->getNumOfChildren(); ++i) {
    NodePtr_t n = gn->getChild(i);
    if (n->hasProperty("ShowVisual")) {
      bool v;
      n->getProperty("ShowVisual", v);
      value = value && v;
    }
  }
  return value;
}
void setShowVisuals(GroupNode* gn, bool visual) {
  for (std::size_t i = 0; i < gn->getNumOfChildren(); ++i) {
    NodePtr_t n = gn->getChild(i);
    if (n->hasProperty("ShowVisual")) n->setProperty("ShowVisual", visual);
  }
}

QStringList rosPackagePath() {
  const QString rosPathVar(qgetenv("ROS_PACKAGE_PATH"));
  return rosPathVar.split(':');
}

std::string getFilename(const QString& input) {
  if (input.startsWith("package://")) {
    QStringList rosPaths = rosPackagePath();
    for (int i = 0; i < rosPaths.size(); ++i) {
      QFileInfo fileInfo(rosPaths[i] + '/' + input.right(input.size() - 10));
      if (fileInfo.exists() && fileInfo.isFile())
        return fileInfo.filePath().toStdString();
    }
    throw std::invalid_argument(
        ("File not found: " + input +
         ". Check ROS_PACKAGE_PATH environment variable.")
            .toStdString());
  }
  return input.toStdString();
}

template <typename ReturnType>
void toDoubles(const QString& s, ReturnType& vect) {
  QStringList nums = s.split(' ', QString::SkipEmptyParts);
  if (ReturnType::num_components != nums.size())
    throw std::logic_error("Could not parse " + s.toStdString());
  bool ok;
  for (int i = 0; i < nums.size(); ++i) {
    double d = nums[i].toDouble(&ok);
    if (!ok) throw std::logic_error("Could not parse " + s.toStdString());
    vect[i] = d;
  }
}

template <typename ReturnType>
void toFloats(const QString& s, ReturnType& vect) {
  QStringList nums = s.split(' ', QString::SkipEmptyParts);
  if (ReturnType::num_components != nums.size())
    throw std::logic_error("Could not parse " + s.toStdString());
  bool ok;
  for (int i = 0; i < nums.size(); ++i) {
    float d = nums[i].toFloat(&ok);
    if (!ok) throw std::logic_error("Could not parse " + s.toStdString());
    vect[i] = d;
  }
}

void parseOrigin(const QDomElement element, osgVector3& T, osgQuat& R) {
  QDomElement origin = element.firstChildElement("origin");
  if (!origin.nextSiblingElement("origin").isNull())
    throw std::logic_error("URDF contains two origins.");

  T = osgVector3(0, 0, 0);
  R = osgQuat(0, 0, 0, 1);
  if (origin.isNull()) return;
  QString xyz = origin.attribute("xyz");
  if (!xyz.isNull()) toFloats(xyz, T);
  QString rpy = origin.attribute("rpy");
  if (!rpy.isNull()) {
    osgVector3 rpy2;
    toFloats(rpy, rpy2);

    double phi, the, psi;
    phi = rpy2[0] / 2.0;
    the = rpy2[1] / 2.0;
    psi = rpy2[2] / 2.0;

    R.x() = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
    R.y() = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
    R.z() = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);
    R.w() = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
  }
}

NodePtr_t createMesh(const QString& name, const QDomElement mesh,
                     Cache_t& cache) {
  std::string mesh_path = getFilename(mesh.attribute("filename"));

  Cache_t::const_iterator _cache = cache.find(mesh_path);
  LeafNodeColladaPtr_t meshNode;
  if (_cache == cache.end()) {
    meshNode = LeafNodeCollada::create(name.toStdString(), mesh_path);
    cache.insert(std::make_pair(mesh_path, meshNode->getColladaPtr()));
  } else
    meshNode =
        LeafNodeCollada::create(name.toStdString(), _cache->second, mesh_path);

  if (mesh.hasAttribute("scale")) {
    osgVector3 scale(1, 1, 1);
    toFloats(mesh.attribute("scale"), scale);
    if (scale != osgVector3(1, 1, 1)) {
      meshNode->setScale(scale);
      meshNode->applyScale();
    }
  }

  return meshNode;
}

/// \param collision collision or visual tag
bool isCapsule(const QDomElement& link, const QDomElement& collision) {
  QString gname = collision.attribute("name");
  if (gname.isNull()) return false;
  QDomElement cc = link.firstChildElement("collision_checking");
  if (cc.isNull()) return false;

  for (QDomElement element = cc.firstChildElement("capsule"); !element.isNull();
       element = element.nextSiblingElement("capsule"))
    if (element.attribute("name") == gname) return true;

  return false;
}

Material parseMaterial(const QDomElement material) {
  Material mat;

  // Set color
  QDomElement color = material.firstChildElement("color");
  mat.hasColor = !color.isNull();
  if (mat.hasColor) {
    toFloats(color.attribute("rgba"), mat.color);
  }

  // Set texture
  QDomElement texture = material.firstChildElement("texture");
  mat.hasTexture = !texture.isNull();
  if (mat.hasTexture) {
    if (!texture.hasAttribute("filename"))
      throw std::logic_error("texture tag must have a filename attribute.");
    mat.texture = getFilename(texture.attribute("filename"));
  }
  return mat;
}

void setMaterial(const QDomElement material, NodePtr_t node,
                 MaterialMap_t& materials) {
  if (material.isNull()) return;
  Material mat;

  if (!material.hasAttribute("name"))
    throw std::logic_error("material tag must have a name attribute.");

  QString name = material.attribute("name");
  if (materials.contains(name)) {
    mat = materials[name];
  } else {
    // Parse material
    mat = parseMaterial(material);
    if (!mat.hasColor && !mat.hasTexture) {
      log() << "material tag " << name
            << " must have either a color or a "
               "texture.\n";
      mat.color = osgVector4(0.9f, 0.9f, 0.9f, 1.f);
      mat.hasColor = true;
    }
    materials[name] = mat;
  }

  // Set color
  if (mat.hasColor) node->setColor(mat.color);

  // Set texture
  if (mat.hasTexture) {
    NodeDrawablePtr_t nd = dynamic_pointer_cast<NodeDrawable>(node);
    LeafNodeColladaPtr_t lc = dynamic_pointer_cast<LeafNodeCollada>(node);
    assert(nd || lc);
    if (nd)
      nd->setTexture(mat.texture);
    else if (lc)
      lc->setTexture(mat.texture);
  }
}

template <bool visual>
void addGeoms(const std::string& robotName, const QString& namePrefix,
              const QDomElement& link, LinkNodePtr_t& linkNode, bool linkFrame,
              Cache_t& cache, MaterialMap_t& materials) {
  static const QString tagName(visual ? "visual" : "collision");
  static const QString nameFormat("%1/%2_%3");
  QString name = nameFormat.arg(robotName.c_str()).arg(namePrefix);

  int index = 0;
  for (QDomElement element = link.firstChildElement(tagName); !element.isNull();
       element = element.nextSiblingElement(tagName)) {
    NodePtr_t node;
    QString name_i = name.arg(index);

    // Parse tag geometry
    QDomElement geometry = element.firstChildElement("geometry");
    if (!geometry.nextSiblingElement("geometry").isNull())
      throw std::logic_error(
          "Visual or collision tag contains two geometries.");
    int N = 0;
    bool ok;
    for (QDomElement type = geometry.firstChildElement(); !type.isNull();
         type = type.nextSiblingElement()) {
      if (type.tagName() == "box") {
        // Create box
        osgVector3 sideLengths;
        toFloats(type.attribute("size"), sideLengths);
        node = LeafNodeBox::create(name_i.toStdString(), sideLengths * .5f);
        ++N;
      } else if (type.tagName() == "cylinder") {
        float radius = type.attribute("radius").toFloat(&ok);
        if (!ok) throw std::logic_error("Could not parse cylinder radius.");
        float length = type.attribute("length").toFloat(&ok);
        if (!ok) throw std::logic_error("Could not parse cylinder length.");
        if (isCapsule(link, element))
          node = LeafNodeCapsule::create(name_i.toStdString(), radius, length);
        else
          node = LeafNodeCylinder::create(name_i.toStdString(), radius, length);
        ++N;
      } else if (type.tagName() == "sphere") {
        float radius = type.attribute("radius").toFloat(&ok);
        if (!ok) throw std::logic_error("Could not parse sphere radius.");
        node = LeafNodeSphere::create(name_i.toStdString(), radius);
        ++N;
      } else if (type.tagName() == "mesh") {
        node = createMesh(name_i, type, cache);
        ++N;
      }
    }
    if (N != 1)
      throw std::logic_error(
          "geometry tag must contain only one of box, cylinder, sphere or "
          "mesh.");

    // Parse tag origin
    if (linkFrame) {
      osgVector3 static_pos;
      osgQuat static_quat;
      parseOrigin(element, static_pos, static_quat);
      node->setStaticTransform(static_pos, static_quat);
    }

    // Parse tag meterial
    setMaterial(element.firstChildElement("material"), node, materials);

    linkNode->add(node, visual);
    ++index;
  }
}

void parseXML(const std::string& urdf_file, QDomDocument& model) {
  bool ok;
  QString errorPrefix, errorMsg;
  int errorRow, errorCol;

  if (urdf_file.compare(urdf_file.length() - 5, 5, ".urdf") == 0) {
    std::string urdf_file2 = getFilename(QString::fromStdString(urdf_file));
    QFile file(urdf_file2.c_str());
    ok = model.setContent(&file, false, &errorMsg, &errorRow, &errorCol);
    errorPrefix = "Failed to parse ";
    errorPrefix += urdf_file.c_str();
  } else {
    QBuffer buffer;
    buffer.setData(urdf_file.c_str(), (int)urdf_file.length());
    ok = model.setContent(&buffer, false, &errorMsg, &errorRow, &errorCol);
    errorPrefix = "Failed to parse XML string";
  }

  if (!ok) {
    throw std::invalid_argument(QString("%1: %2 at %3:%4")
                                    .arg(errorPrefix)
                                    .arg(errorMsg)
                                    .arg(errorRow)
                                    .arg(errorCol)
                                    .toStdString());
  }
}
}  // namespace details

std::string getFilename(const std::string& input) {
  return details::getFilename(QString::fromStdString(input));
}

GroupNodePtr_t parse(const std::string& robotName, const std::string& urdf_file,
                     const bool& visual, const bool& linkFrame) {
  QDomDocument model;

  // Parse the XML document
  details::parseXML(urdf_file, model);

  // Parse the materials
  details::MaterialMap_t materials;
  for (QDomElement material =
           model.firstChildElement("robot").firstChildElement("material");
       !material.isNull(); material = material.nextSiblingElement("material")) {
    if (!material.hasAttribute("name"))
      throw std::logic_error("material tag must have a name attribute.");

    details::Material mat = details::parseMaterial(material);

    if (!mat.hasColor && !mat.hasTexture)
      throw std::logic_error(
          "material tag must have either a color or a texture.");
    QString name = material.attribute("name");
    materials[name] = mat;
  }

  GroupNodePtr_t robot = GroupNode::create(robotName);
  QDomNodeList links = model.elementsByTagName("link");

  robot->addProperty(BoolProperty::create(
      "ShowVisual",
      BoolProperty::Getter_t(boost::bind(details::getShowVisuals, robot.get())),
      BoolProperty::Setter_t(
          boost::bind(details::setShowVisuals, robot.get(), _1))));

  details::Cache_t cache;
  for (int i = 0; i < links.size(); i++) {
    QDomElement link = links.at(i).toElement();
    if (link.isNull()) throw std::logic_error("link must be a tag.");
    QString name = link.attribute("name");
    if (name.isNull()) throw std::logic_error("A link has no name attribute.");

    std::string link_name = name.toStdString();
    log() << name << '\n';

    details::LinkNodePtr_t linkNode(
        details::LinkNode ::create(robotName + "/" + link_name));
    linkNode->showVisual(visual);
    // add link to robot node
    robot->addChild(linkNode);

    if (visual) {
      details::addGeoms<true>(robotName, name, link, linkNode, linkFrame, cache,
                              materials);
      if (linkFrame) {
        try {
          details::addGeoms<false>(robotName, "collision_" + name, link,
                                   linkNode, linkFrame, cache, materials);
        } catch (const std::invalid_argument& e) {
          std::cerr << "Could not load collision geometries of " << robotName
                    << ":" << e.what() << std::endl;
        }
      }
    } else {
      details::addGeoms<false>(robotName, name, link, linkNode, linkFrame,
                               cache, materials);
      if (linkFrame) {
        try {
          details::addGeoms<true>(robotName, "visual_" + name, link, linkNode,
                                  linkFrame, cache, materials);
        } catch (const std::invalid_argument& e) {
          std::cerr << "Could not load visual geometries of " << robotName
                    << ":" << e.what() << std::endl;
        }
      }
    }
  }
  return robot;
}
}  // namespace urdfParser
} /* namespace viewer */

}  // namespace gepetto


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