| 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 |
|
✗ |
if (rosPathVar.toStdString() != "") return rosPathVar.split(':'); |
| 120 |
|
✗ |
const QString amentPrefixPath(qgetenv("AMENT_PREFIX_PATH")); |
| 121 |
|
✗ |
if (amentPrefixPath.toStdString() != "") { |
| 122 |
|
✗ |
QStringList paths(amentPrefixPath.split(':')); |
| 123 |
|
✗ |
QStringList res; |
| 124 |
|
✗ |
for (const auto& path : paths) { |
| 125 |
|
✗ |
res.append(path + QString("/share")); |
| 126 |
|
|
} |
| 127 |
|
✗ |
return res; |
| 128 |
|
|
} |
| 129 |
|
✗ |
throw std::invalid_argument( |
| 130 |
|
|
"neither ROS_PACKAGE_PATH nor AMENT_PREFIX_PATH environment variables is " |
| 131 |
|
✗ |
"defined."); |
| 132 |
|
|
} |
| 133 |
|
|
|
| 134 |
|
✗ |
std::string getFilename(const QString& input) { |
| 135 |
|
✗ |
if (input.startsWith("package://")) { |
| 136 |
|
✗ |
QStringList rosPaths; |
| 137 |
|
|
try { |
| 138 |
|
✗ |
rosPaths = rosPackagePath(); |
| 139 |
|
✗ |
} catch (const std::invalid_argument& exc) { |
| 140 |
|
✗ |
throw std::invalid_argument( |
| 141 |
|
✗ |
std::string("Input path: \"") + input.toStdString() + |
| 142 |
|
✗ |
std::string("\" starts with \"package://\", but ") + exc.what()); |
| 143 |
|
|
} |
| 144 |
|
✗ |
for (int i = 0; i < rosPaths.size(); ++i) { |
| 145 |
|
✗ |
QFileInfo fileInfo(rosPaths[i] + '/' + input.right(input.size() - 10)); |
| 146 |
|
✗ |
if (fileInfo.exists() && fileInfo.isFile()) |
| 147 |
|
✗ |
return fileInfo.filePath().toStdString(); |
| 148 |
|
|
} |
| 149 |
|
✗ |
throw std::invalid_argument( |
| 150 |
|
✗ |
("File not found: " + input + |
| 151 |
|
|
". Check ROS_PACKAGE_PATH environment variable.") |
| 152 |
|
✗ |
.toStdString()); |
| 153 |
|
|
} |
| 154 |
|
✗ |
return input.toStdString(); |
| 155 |
|
|
} |
| 156 |
|
|
|
| 157 |
|
|
template <typename ReturnType> |
| 158 |
|
|
void toDoubles(const QString& s, ReturnType& vect) { |
| 159 |
|
|
#if QT_VERSION >= QT_VERSION_CHECK(5, 14, 0) |
| 160 |
|
|
QStringList nums = s.split(' ', Qt::SkipEmptyParts); |
| 161 |
|
|
#else |
| 162 |
|
|
QStringList nums = s.split(' ', QString::SkipEmptyParts); |
| 163 |
|
|
#endif |
| 164 |
|
|
if (ReturnType::num_components != nums.size()) |
| 165 |
|
|
throw std::logic_error("Could not parse " + s.toStdString()); |
| 166 |
|
|
bool ok; |
| 167 |
|
|
for (int i = 0; i < nums.size(); ++i) { |
| 168 |
|
|
double d = nums[i].toDouble(&ok); |
| 169 |
|
|
if (!ok) throw std::logic_error("Could not parse " + s.toStdString()); |
| 170 |
|
|
vect[i] = d; |
| 171 |
|
|
} |
| 172 |
|
|
} |
| 173 |
|
|
|
| 174 |
|
|
template <typename ReturnType> |
| 175 |
|
✗ |
void toFloats(const QString& s, ReturnType& vect) { |
| 176 |
|
|
#if QT_VERSION >= QT_VERSION_CHECK(5, 14, 0) |
| 177 |
|
✗ |
QStringList nums = s.split(' ', Qt::SkipEmptyParts); |
| 178 |
|
|
#else |
| 179 |
|
|
QStringList nums = s.split(' ', QString::SkipEmptyParts); |
| 180 |
|
|
#endif |
| 181 |
|
✗ |
if (ReturnType::num_components != nums.size()) |
| 182 |
|
✗ |
throw std::logic_error("Could not parse " + s.toStdString()); |
| 183 |
|
|
bool ok; |
| 184 |
|
✗ |
for (int i = 0; i < nums.size(); ++i) { |
| 185 |
|
✗ |
float d = nums[i].toFloat(&ok); |
| 186 |
|
✗ |
if (!ok) throw std::logic_error("Could not parse " + s.toStdString()); |
| 187 |
|
✗ |
vect[i] = d; |
| 188 |
|
|
} |
| 189 |
|
|
} |
| 190 |
|
|
|
| 191 |
|
✗ |
void parseOrigin(const QDomElement element, osgVector3& T, osgQuat& R) { |
| 192 |
|
✗ |
QDomElement origin = element.firstChildElement("origin"); |
| 193 |
|
✗ |
if (!origin.nextSiblingElement("origin").isNull()) |
| 194 |
|
✗ |
throw std::logic_error("URDF contains two origins."); |
| 195 |
|
|
|
| 196 |
|
✗ |
T = osgVector3(0, 0, 0); |
| 197 |
|
✗ |
R = osgQuat(0, 0, 0, 1); |
| 198 |
|
✗ |
if (origin.isNull()) return; |
| 199 |
|
✗ |
QString xyz = origin.attribute("xyz"); |
| 200 |
|
✗ |
if (!xyz.isNull()) toFloats(xyz, T); |
| 201 |
|
✗ |
QString rpy = origin.attribute("rpy"); |
| 202 |
|
✗ |
if (!rpy.isNull()) { |
| 203 |
|
✗ |
osgVector3 rpy2; |
| 204 |
|
✗ |
toFloats(rpy, rpy2); |
| 205 |
|
|
|
| 206 |
|
|
double phi, the, psi; |
| 207 |
|
✗ |
phi = rpy2[0] / 2.0; |
| 208 |
|
✗ |
the = rpy2[1] / 2.0; |
| 209 |
|
✗ |
psi = rpy2[2] / 2.0; |
| 210 |
|
|
|
| 211 |
|
✗ |
R.x() = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); |
| 212 |
|
✗ |
R.y() = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); |
| 213 |
|
✗ |
R.z() = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); |
| 214 |
|
✗ |
R.w() = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); |
| 215 |
|
|
} |
| 216 |
|
|
} |
| 217 |
|
|
|
| 218 |
|
✗ |
NodePtr_t createMesh(const QString& name, const QDomElement mesh, |
| 219 |
|
|
Cache_t& cache) { |
| 220 |
|
✗ |
std::string mesh_path = getFilename(mesh.attribute("filename")); |
| 221 |
|
|
|
| 222 |
|
✗ |
Cache_t::const_iterator _cache = cache.find(mesh_path); |
| 223 |
|
✗ |
LeafNodeColladaPtr_t meshNode; |
| 224 |
|
✗ |
if (_cache == cache.end()) { |
| 225 |
|
✗ |
meshNode = LeafNodeCollada::create(name.toStdString(), mesh_path); |
| 226 |
|
✗ |
cache.insert(std::make_pair(mesh_path, meshNode->getColladaPtr())); |
| 227 |
|
|
} else |
| 228 |
|
|
meshNode = |
| 229 |
|
✗ |
LeafNodeCollada::create(name.toStdString(), _cache->second, mesh_path); |
| 230 |
|
|
|
| 231 |
|
✗ |
if (mesh.hasAttribute("scale")) { |
| 232 |
|
✗ |
osgVector3 scale(1, 1, 1); |
| 233 |
|
✗ |
toFloats(mesh.attribute("scale"), scale); |
| 234 |
|
✗ |
if (scale != osgVector3(1, 1, 1)) { |
| 235 |
|
✗ |
meshNode->setScale(scale); |
| 236 |
|
✗ |
meshNode->applyScale(); |
| 237 |
|
|
} |
| 238 |
|
|
} |
| 239 |
|
|
|
| 240 |
|
✗ |
return meshNode; |
| 241 |
|
|
} |
| 242 |
|
|
|
| 243 |
|
|
/// \param collision collision or visual tag |
| 244 |
|
✗ |
bool isCapsule(const QDomElement& link, const QDomElement& collision) { |
| 245 |
|
✗ |
QString gname = collision.attribute("name"); |
| 246 |
|
✗ |
if (gname.isNull()) return false; |
| 247 |
|
✗ |
QDomElement cc = link.firstChildElement("collision_checking"); |
| 248 |
|
✗ |
if (cc.isNull()) return false; |
| 249 |
|
|
|
| 250 |
|
✗ |
for (QDomElement element = cc.firstChildElement("capsule"); !element.isNull(); |
| 251 |
|
✗ |
element = element.nextSiblingElement("capsule")) |
| 252 |
|
✗ |
if (element.attribute("name") == gname) return true; |
| 253 |
|
|
|
| 254 |
|
✗ |
return false; |
| 255 |
|
|
} |
| 256 |
|
|
|
| 257 |
|
✗ |
Material parseMaterial(const QDomElement material) { |
| 258 |
|
✗ |
Material mat; |
| 259 |
|
|
|
| 260 |
|
|
// Set color |
| 261 |
|
✗ |
QDomElement color = material.firstChildElement("color"); |
| 262 |
|
✗ |
mat.hasColor = !color.isNull(); |
| 263 |
|
✗ |
if (mat.hasColor) { |
| 264 |
|
✗ |
toFloats(color.attribute("rgba"), mat.color); |
| 265 |
|
|
} |
| 266 |
|
|
|
| 267 |
|
|
// Set texture |
| 268 |
|
✗ |
QDomElement texture = material.firstChildElement("texture"); |
| 269 |
|
✗ |
mat.hasTexture = !texture.isNull(); |
| 270 |
|
✗ |
if (mat.hasTexture) { |
| 271 |
|
✗ |
if (!texture.hasAttribute("filename")) |
| 272 |
|
✗ |
throw std::logic_error("texture tag must have a filename attribute."); |
| 273 |
|
✗ |
mat.texture = getFilename(texture.attribute("filename")); |
| 274 |
|
|
} |
| 275 |
|
✗ |
return mat; |
| 276 |
|
|
} |
| 277 |
|
|
|
| 278 |
|
✗ |
void setMaterial(const QDomElement material, NodePtr_t node, |
| 279 |
|
|
MaterialMap_t& materials) { |
| 280 |
|
✗ |
if (material.isNull()) return; |
| 281 |
|
✗ |
Material mat; |
| 282 |
|
|
|
| 283 |
|
✗ |
if (!material.hasAttribute("name")) |
| 284 |
|
✗ |
throw std::logic_error("material tag must have a name attribute."); |
| 285 |
|
|
|
| 286 |
|
✗ |
QString name = material.attribute("name"); |
| 287 |
|
✗ |
if (materials.contains(name)) { |
| 288 |
|
✗ |
mat = materials[name]; |
| 289 |
|
|
} else { |
| 290 |
|
|
// Parse material |
| 291 |
|
✗ |
mat = parseMaterial(material); |
| 292 |
|
✗ |
if (!mat.hasColor && !mat.hasTexture) { |
| 293 |
|
✗ |
log() << "material tag " << name |
| 294 |
|
|
<< " must have either a color or a " |
| 295 |
|
✗ |
"texture.\n"; |
| 296 |
|
✗ |
mat.color = osgVector4(0.9f, 0.9f, 0.9f, 1.f); |
| 297 |
|
✗ |
mat.hasColor = true; |
| 298 |
|
|
} |
| 299 |
|
✗ |
materials[name] = mat; |
| 300 |
|
|
} |
| 301 |
|
|
|
| 302 |
|
|
// Set color |
| 303 |
|
✗ |
if (mat.hasColor) node->setColor(mat.color); |
| 304 |
|
|
|
| 305 |
|
|
// Set texture |
| 306 |
|
✗ |
if (mat.hasTexture) { |
| 307 |
|
✗ |
NodeDrawablePtr_t nd = dynamic_pointer_cast<NodeDrawable>(node); |
| 308 |
|
✗ |
LeafNodeColladaPtr_t lc = dynamic_pointer_cast<LeafNodeCollada>(node); |
| 309 |
|
✗ |
assert(nd || lc); |
| 310 |
|
✗ |
if (nd) |
| 311 |
|
✗ |
nd->setTexture(mat.texture); |
| 312 |
|
✗ |
else if (lc) |
| 313 |
|
✗ |
lc->setTexture(mat.texture); |
| 314 |
|
|
} |
| 315 |
|
|
} |
| 316 |
|
|
|
| 317 |
|
|
template <bool visual> |
| 318 |
|
✗ |
void addGeoms(const std::string& robotName, const QString& namePrefix, |
| 319 |
|
|
const QDomElement& link, LinkNodePtr_t& linkNode, bool linkFrame, |
| 320 |
|
|
Cache_t& cache, MaterialMap_t& materials) { |
| 321 |
|
✗ |
static const QString tagName(visual ? "visual" : "collision"); |
| 322 |
|
✗ |
static const QString nameFormat("%1/%2_%3"); |
| 323 |
|
✗ |
QString name = nameFormat.arg(robotName.c_str()).arg(namePrefix); |
| 324 |
|
|
|
| 325 |
|
✗ |
int index = 0; |
| 326 |
|
✗ |
for (QDomElement element = link.firstChildElement(tagName); !element.isNull(); |
| 327 |
|
✗ |
element = element.nextSiblingElement(tagName)) { |
| 328 |
|
✗ |
NodePtr_t node; |
| 329 |
|
✗ |
QString name_i = name.arg(index); |
| 330 |
|
|
|
| 331 |
|
|
// Parse tag geometry |
| 332 |
|
✗ |
QDomElement geometry = element.firstChildElement("geometry"); |
| 333 |
|
✗ |
if (!geometry.nextSiblingElement("geometry").isNull()) |
| 334 |
|
✗ |
throw std::logic_error( |
| 335 |
|
|
"Visual or collision tag contains two geometries."); |
| 336 |
|
✗ |
int N = 0; |
| 337 |
|
|
bool ok; |
| 338 |
|
✗ |
for (QDomElement type = geometry.firstChildElement(); !type.isNull(); |
| 339 |
|
✗ |
type = type.nextSiblingElement()) { |
| 340 |
|
✗ |
if (type.tagName() == "box") { |
| 341 |
|
|
// Create box |
| 342 |
|
✗ |
osgVector3 sideLengths; |
| 343 |
|
✗ |
toFloats(type.attribute("size"), sideLengths); |
| 344 |
|
✗ |
node = LeafNodeBox::create(name_i.toStdString(), sideLengths * .5f); |
| 345 |
|
✗ |
++N; |
| 346 |
|
✗ |
} else if (type.tagName() == "cylinder") { |
| 347 |
|
✗ |
float radius = type.attribute("radius").toFloat(&ok); |
| 348 |
|
✗ |
if (!ok) throw std::logic_error("Could not parse cylinder radius."); |
| 349 |
|
✗ |
float length = type.attribute("length").toFloat(&ok); |
| 350 |
|
✗ |
if (!ok) throw std::logic_error("Could not parse cylinder length."); |
| 351 |
|
✗ |
if (isCapsule(link, element)) |
| 352 |
|
✗ |
node = LeafNodeCapsule::create(name_i.toStdString(), radius, length); |
| 353 |
|
|
else |
| 354 |
|
✗ |
node = LeafNodeCylinder::create(name_i.toStdString(), radius, length); |
| 355 |
|
✗ |
++N; |
| 356 |
|
✗ |
} else if (type.tagName() == "sphere") { |
| 357 |
|
✗ |
float radius = type.attribute("radius").toFloat(&ok); |
| 358 |
|
✗ |
if (!ok) throw std::logic_error("Could not parse sphere radius."); |
| 359 |
|
✗ |
node = LeafNodeSphere::create(name_i.toStdString(), radius); |
| 360 |
|
✗ |
++N; |
| 361 |
|
✗ |
} else if (type.tagName() == "mesh") { |
| 362 |
|
✗ |
node = createMesh(name_i, type, cache); |
| 363 |
|
✗ |
++N; |
| 364 |
|
|
} |
| 365 |
|
|
} |
| 366 |
|
✗ |
if (N != 1) |
| 367 |
|
✗ |
throw std::logic_error( |
| 368 |
|
|
"geometry tag must contain only one of box, cylinder, sphere or " |
| 369 |
|
|
"mesh."); |
| 370 |
|
|
|
| 371 |
|
|
// Parse tag origin |
| 372 |
|
✗ |
if (linkFrame) { |
| 373 |
|
✗ |
osgVector3 static_pos; |
| 374 |
|
✗ |
osgQuat static_quat; |
| 375 |
|
✗ |
parseOrigin(element, static_pos, static_quat); |
| 376 |
|
✗ |
node->setStaticTransform(static_pos, static_quat); |
| 377 |
|
|
} |
| 378 |
|
|
|
| 379 |
|
|
// Parse tag meterial |
| 380 |
|
✗ |
setMaterial(element.firstChildElement("material"), node, materials); |
| 381 |
|
|
|
| 382 |
|
✗ |
linkNode->add(node, visual); |
| 383 |
|
✗ |
++index; |
| 384 |
|
|
} |
| 385 |
|
|
} |
| 386 |
|
|
|
| 387 |
|
✗ |
void parseXML(const std::string& urdf_file, QDomDocument& model) { |
| 388 |
|
|
bool ok; |
| 389 |
|
✗ |
QString errorPrefix, errorMsg; |
| 390 |
|
|
int errorRow, errorCol; |
| 391 |
|
|
|
| 392 |
|
✗ |
if (urdf_file.compare(urdf_file.length() - 5, 5, ".urdf") == 0) { |
| 393 |
|
✗ |
std::string urdf_file2 = getFilename(QString::fromStdString(urdf_file)); |
| 394 |
|
✗ |
QFile file(urdf_file2.c_str()); |
| 395 |
|
✗ |
ok = model.setContent(&file, false, &errorMsg, &errorRow, &errorCol); |
| 396 |
|
✗ |
errorPrefix = "Failed to parse "; |
| 397 |
|
✗ |
errorPrefix += urdf_file.c_str(); |
| 398 |
|
✗ |
} else { |
| 399 |
|
✗ |
QBuffer buffer; |
| 400 |
|
✗ |
buffer.setData(urdf_file.c_str(), (int)urdf_file.length()); |
| 401 |
|
✗ |
ok = model.setContent(&buffer, false, &errorMsg, &errorRow, &errorCol); |
| 402 |
|
✗ |
errorPrefix = "Failed to parse XML string"; |
| 403 |
|
|
} |
| 404 |
|
|
|
| 405 |
|
✗ |
if (!ok) { |
| 406 |
|
✗ |
throw std::invalid_argument(QString("%1: %2 at %3:%4") |
| 407 |
|
✗ |
.arg(errorPrefix) |
| 408 |
|
✗ |
.arg(errorMsg) |
| 409 |
|
✗ |
.arg(errorRow) |
| 410 |
|
✗ |
.arg(errorCol) |
| 411 |
|
✗ |
.toStdString()); |
| 412 |
|
|
} |
| 413 |
|
|
} |
| 414 |
|
|
} // namespace details |
| 415 |
|
|
|
| 416 |
|
✗ |
std::string getFilename(const std::string& input) { |
| 417 |
|
✗ |
return details::getFilename(QString::fromStdString(input)); |
| 418 |
|
|
} |
| 419 |
|
|
|
| 420 |
|
✗ |
GroupNodePtr_t parse(const std::string& robotName, const std::string& urdf_file, |
| 421 |
|
|
const bool& visual, const bool& linkFrame) { |
| 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 = |
| 430 |
|
✗ |
model.firstChildElement("robot").firstChildElement("material"); |
| 431 |
|
✗ |
!material.isNull(); 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( |
| 439 |
|
✗ |
"material tag must have either a color or a texture."); |
| 440 |
|
✗ |
QString name = material.attribute("name"); |
| 441 |
|
✗ |
materials[name] = mat; |
| 442 |
|
|
} |
| 443 |
|
|
|
| 444 |
|
✗ |
GroupNodePtr_t robot = GroupNode::create(robotName); |
| 445 |
|
✗ |
QDomNodeList links = model.elementsByTagName("link"); |
| 446 |
|
|
|
| 447 |
|
✗ |
robot->addProperty(BoolProperty::create( |
| 448 |
|
|
"ShowVisual", |
| 449 |
|
✗ |
BoolProperty::Getter_t(boost::bind(details::getShowVisuals, robot.get())), |
| 450 |
|
✗ |
BoolProperty::Setter_t( |
| 451 |
|
|
boost::bind(details::setShowVisuals, robot.get(), _1)))); |
| 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 |
|
✗ |
log() << name << '\n'; |
| 462 |
|
|
|
| 463 |
|
|
details::LinkNodePtr_t linkNode( |
| 464 |
|
✗ |
details::LinkNode ::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, |
| 471 |
|
|
materials); |
| 472 |
|
✗ |
if (linkFrame) { |
| 473 |
|
|
try { |
| 474 |
|
✗ |
details::addGeoms<false>(robotName, "collision_" + name, link, |
| 475 |
|
✗ |
linkNode, linkFrame, cache, materials); |
| 476 |
|
✗ |
} catch (const std::invalid_argument& e) { |
| 477 |
|
|
std::cerr << "Could not load collision geometries of " << robotName |
| 478 |
|
✗ |
<< ":" << e.what() << std::endl; |
| 479 |
|
|
} |
| 480 |
|
|
} |
| 481 |
|
|
} else { |
| 482 |
|
✗ |
details::addGeoms<false>(robotName, name, link, linkNode, linkFrame, |
| 483 |
|
|
cache, materials); |
| 484 |
|
✗ |
if (linkFrame) { |
| 485 |
|
|
try { |
| 486 |
|
✗ |
details::addGeoms<true>(robotName, "visual_" + name, link, linkNode, |
| 487 |
|
✗ |
linkFrame, cache, materials); |
| 488 |
|
✗ |
} catch (const std::invalid_argument& e) { |
| 489 |
|
|
std::cerr << "Could not load visual geometries of " << robotName |
| 490 |
|
✗ |
<< ":" << e.what() << std::endl; |
| 491 |
|
|
} |
| 492 |
|
|
} |
| 493 |
|
|
} |
| 494 |
|
|
} |
| 495 |
|
✗ |
return robot; |
| 496 |
|
|
} |
| 497 |
|
|
} // namespace urdfParser |
| 498 |
|
|
} /* namespace viewer */ |
| 499 |
|
|
|
| 500 |
|
|
} // namespace gepetto |
| 501 |
|
|
|