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 |
474 |
|
|
|