GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/parsers/urdf.hpp" |
||
6 |
#include "pinocchio/parsers/urdf/types.hpp" |
||
7 |
#include "pinocchio/parsers/urdf/utils.hpp" |
||
8 |
#include "pinocchio/parsers/utils.hpp" |
||
9 |
|||
10 |
#include <boost/property_tree/xml_parser.hpp> |
||
11 |
#include <boost/property_tree/ptree.hpp> |
||
12 |
|||
13 |
#include <urdf_model/model.h> |
||
14 |
#include <urdf_parser/urdf_parser.h> |
||
15 |
|||
16 |
namespace pinocchio |
||
17 |
{ |
||
18 |
namespace urdf |
||
19 |
{ |
||
20 |
/// \internal |
||
21 |
namespace details |
||
22 |
{ |
||
23 |
struct UrdfTree |
||
24 |
{ |
||
25 |
typedef boost::property_tree::ptree ptree; |
||
26 |
typedef std::map<std::string, const ptree&> LinkMap_t; |
||
27 |
|||
28 |
41 |
void parse (const std::string & xmlStr) |
|
29 |
{ |
||
30 |
✓✗ | 41 |
urdf_ = ::urdf::parseURDF(xmlStr); |
31 |
✗✓ | 41 |
if (!urdf_) { |
32 |
throw std::invalid_argument("Unable to parse URDF"); |
||
33 |
} |
||
34 |
|||
35 |
✓✗ | 82 |
std::istringstream iss(xmlStr); |
36 |
using namespace boost::property_tree; |
||
37 |
✓✗ | 41 |
read_xml(iss, tree_, xml_parser::no_comments); |
38 |
|||
39 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗ |
8071 |
BOOST_FOREACH(const ptree::value_type & link, tree_.get_child("robot")) { |
40 |
✓✓ | 4015 |
if (link.first == "link") { |
41 |
✓✗✓✗ |
1570 |
std::string name = link.second.get<std::string>("<xmlattr>.name"); |
42 |
✓✗✓✗ |
1570 |
links_.insert(std::pair<std::string,const ptree&>(name, link.second)); |
43 |
} |
||
44 |
} // BOOST_FOREACH |
||
45 |
41 |
} |
|
46 |
|||
47 |
23 |
bool isCapsule(const std::string & linkName, |
|
48 |
const std::string & geomName) const |
||
49 |
{ |
||
50 |
✓✗ | 23 |
LinkMap_t::const_iterator _link = links_.find(linkName); |
51 |
✗✓ | 23 |
assert (_link != links_.end()); |
52 |
23 |
const ptree& link = _link->second; |
|
53 |
✓✗✓✗ ✓✓ |
23 |
if (link.count ("collision_checking") == 0) |
54 |
20 |
return false; |
|
55 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
3 |
BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) { |
56 |
✓✗ | 3 |
if (cc.first == "capsule") { |
57 |
#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME |
||
58 |
std::cerr << "Warning: support for tag link/collision_checking/capsule" |
||
59 |
" is not available for URDFDOM < 0.3.0" << std::endl; |
||
60 |
#else |
||
61 |
✓✗✓✗ |
3 |
std::string name = cc.second.get<std::string>("<xmlattr>.name"); |
62 |
✓✗ | 3 |
if (geomName == name) return true; |
63 |
#endif |
||
64 |
} |
||
65 |
} // BOOST_FOREACH |
||
66 |
|||
67 |
return false; |
||
68 |
} |
||
69 |
|||
70 |
730 |
bool isMeshConvex(const std::string & linkName, |
|
71 |
const std::string & geomName) const |
||
72 |
{ |
||
73 |
✓✗ | 730 |
LinkMap_t::const_iterator _link = links_.find(linkName); |
74 |
✗✓ | 730 |
assert (_link != links_.end()); |
75 |
730 |
const ptree& link = _link->second; |
|
76 |
✓✗✓✗ ✓✓ |
730 |
if (link.count ("collision_checking") == 0) |
77 |
727 |
return false; |
|
78 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✗ ✓✗ |
9 |
BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) { |
79 |
✓✓ | 6 |
if (cc.first == "convex") { |
80 |
#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME |
||
81 |
std::cerr << "Warning: support for tag link/collision_checking/convex" |
||
82 |
" is not available for URDFDOM < 0.3.0" << std::endl; |
||
83 |
#else |
||
84 |
✓✗✓✗ |
3 |
std::string name = cc.second.get<std::string>("<xmlattr>.name"); |
85 |
✓✗ | 3 |
if (geomName == name) return true; |
86 |
#endif |
||
87 |
} |
||
88 |
} // BOOST_FOREACH |
||
89 |
|||
90 |
return false; |
||
91 |
} |
||
92 |
|||
93 |
// For standard URDF tags |
||
94 |
::urdf::ModelInterfaceSharedPtr urdf_; |
||
95 |
// For other tags |
||
96 |
ptree tree_; |
||
97 |
// A mapping from link name to corresponding child of tree_ |
||
98 |
LinkMap_t links_; |
||
99 |
}; |
||
100 |
|||
101 |
template<typename Vector3> |
||
102 |
730 |
static void retrieveMeshScale(const ::urdf::MeshSharedPtr & mesh, |
|
103 |
const Eigen::MatrixBase<Vector3> & scale) |
||
104 |
{ |
||
105 |
730 |
Vector3 & scale_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,scale); |
|
106 |
scale_ << |
||
107 |
1460 |
mesh->scale.x, |
|
108 |
✓✗ | 730 |
mesh->scale.y, |
109 |
✓✗ | 730 |
mesh->scale.z; |
110 |
730 |
} |
|
111 |
|||
112 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
113 |
# if ( HPP_FCL_MAJOR_VERSION>1 || ( HPP_FCL_MAJOR_VERSION==1 && \ |
||
114 |
( HPP_FCL_MINOR_VERSION>1 || ( HPP_FCL_MINOR_VERSION==1 && \ |
||
115 |
HPP_FCL_PATCH_VERSION>3)))) |
||
116 |
# define PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 |
||
117 |
# endif |
||
118 |
|||
119 |
/** |
||
120 |
* @brief Get a fcl::CollisionObject from a URDF geometry, searching |
||
121 |
* for it in specified package directories |
||
122 |
* |
||
123 |
* @param[in] urdf_geometry A shared pointer on the input urdf Geometry |
||
124 |
* @param[in] package_dirs A vector containing the different directories where to search for packages |
||
125 |
* @param[out] meshPath The Absolute path of the mesh currently read |
||
126 |
* @param[out] meshScale Scale of transformation currently applied to the mesh |
||
127 |
* |
||
128 |
* @return A shared pointer on the geometry converted as a fcl::CollisionGeometry |
||
129 |
*/ |
||
130 |
shared_ptr<fcl::CollisionGeometry> |
||
131 |
763 |
inline retrieveCollisionGeometry(const UrdfTree& tree, |
|
132 |
fcl::MeshLoaderPtr& meshLoader, |
||
133 |
const std::string& linkName, |
||
134 |
const std::string& geomName, |
||
135 |
const ::urdf::GeometrySharedPtr urdf_geometry, |
||
136 |
const std::vector<std::string> & package_dirs, |
||
137 |
std::string & meshPath, |
||
138 |
Eigen::Vector3d & meshScale) |
||
139 |
{ |
||
140 |
763 |
shared_ptr<fcl::CollisionGeometry> geometry; |
|
141 |
|||
142 |
// Handle the case where collision geometry is a mesh |
||
143 |
✓✓ | 763 |
if (urdf_geometry->type == ::urdf::Geometry::MESH) |
144 |
{ |
||
145 |
1460 |
const ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry); |
|
146 |
✓✗ | 1460 |
std::string collisionFilename = urdf_mesh->filename; |
147 |
|||
148 |
✓✗ | 730 |
meshPath = retrieveResourcePath(collisionFilename, package_dirs); |
149 |
✗✓ | 730 |
if (meshPath == "") { |
150 |
std::stringstream ss; |
||
151 |
ss << "Mesh " << collisionFilename << " could not be found."; |
||
152 |
throw std::invalid_argument (ss.str()); |
||
153 |
} |
||
154 |
|||
155 |
1460 |
fcl::Vec3f scale = fcl::Vec3f(urdf_mesh->scale.x, |
|
156 |
730 |
urdf_mesh->scale.y, |
|
157 |
730 |
urdf_mesh->scale.z |
|
158 |
✓✗ | 730 |
); |
159 |
|||
160 |
✓✗ | 730 |
retrieveMeshScale(urdf_mesh, meshScale); |
161 |
|||
162 |
// Create FCL mesh by parsing Collada file. |
||
163 |
#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 |
||
164 |
✓✗ | 1460 |
hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale); |
165 |
✓✗ | 730 |
bool convex = tree.isMeshConvex (linkName, geomName); |
166 |
✓✓ | 730 |
if (convex) { |
167 |
✓✗ | 3 |
bvh->buildConvexRepresentation (false); |
168 |
#if HPP_FCL_MAJOR_VERSION < 2 |
||
169 |
geometry = bvh->convex; |
||
170 |
#else |
||
171 |
✓✗ | 6 |
geometry = shared_ptr<fcl::CollisionGeometry>(bvh->convex.get(), [bvh](...) mutable { bvh->convex.reset(); }); |
172 |
#endif // HPP_FCL_MAJOR_VERSION |
||
173 |
} else { |
||
174 |
#if HPP_FCL_MAJOR_VERSION < 2 |
||
175 |
geometry = bvh; |
||
176 |
#else |
||
177 |
✓✗ | 1069 |
geometry = shared_ptr<fcl::CollisionGeometry>(bvh.get(), [bvh](...) mutable { bvh.reset(); }); |
178 |
#endif // HPP_FCL_MAJOR_VERSION |
||
179 |
} |
||
180 |
#else |
||
181 |
geometry = meshLoader->load (meshPath, scale); |
||
182 |
#endif |
||
183 |
} |
||
184 |
|||
185 |
// Handle the case where collision geometry is a cylinder |
||
186 |
// Use FCL capsules for cylinders |
||
187 |
✓✓ | 33 |
else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER) |
188 |
{ |
||
189 |
✓✗ | 23 |
const bool is_capsule = tree.isCapsule(linkName, geomName); |
190 |
✓✗✓✗ ✓✗ |
23 |
meshScale << 1,1,1; |
191 |
46 |
const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry); |
|
192 |
|||
193 |
23 |
double radius = collisionGeometry->radius; |
|
194 |
23 |
double length = collisionGeometry->length; |
|
195 |
|||
196 |
// Create fcl capsule geometry. |
||
197 |
✓✓ | 23 |
if (is_capsule) { |
198 |
✓✗ | 3 |
meshPath = "CAPSULE"; |
199 |
✓✗✓✗ ✓✗ |
3 |
geometry = shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length)); |
200 |
} else { |
||
201 |
✓✗ | 20 |
meshPath = "CYLINDER"; |
202 |
✓✗✓✗ ✓✗ |
20 |
geometry = shared_ptr < fcl::CollisionGeometry >(new fcl::Cylinder (radius, length)); |
203 |
} |
||
204 |
} |
||
205 |
// Handle the case where collision geometry is a box. |
||
206 |
✓✗ | 10 |
else if (urdf_geometry->type == ::urdf::Geometry::BOX) |
207 |
{ |
||
208 |
✓✗ | 10 |
meshPath = "BOX"; |
209 |
✓✗✓✗ ✓✗ |
10 |
meshScale << 1,1,1; |
210 |
10 |
const ::urdf::BoxSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry); |
|
211 |
|||
212 |
10 |
double x = collisionGeometry->dim.x; |
|
213 |
10 |
double y = collisionGeometry->dim.y; |
|
214 |
10 |
double z = collisionGeometry->dim.z; |
|
215 |
|||
216 |
✓✗✓✗ ✓✗ |
10 |
geometry = shared_ptr < fcl::CollisionGeometry > (new fcl::Box (x, y, z)); |
217 |
} |
||
218 |
// Handle the case where collision geometry is a sphere. |
||
219 |
else if (urdf_geometry->type == ::urdf::Geometry::SPHERE) |
||
220 |
{ |
||
221 |
meshPath = "SPHERE"; |
||
222 |
meshScale << 1,1,1; |
||
223 |
const ::urdf::SphereSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry); |
||
224 |
|||
225 |
double radius = collisionGeometry->radius; |
||
226 |
|||
227 |
geometry = shared_ptr < fcl::CollisionGeometry > (new fcl::Sphere (radius)); |
||
228 |
} |
||
229 |
else throw std::invalid_argument("Unknown geometry type :"); |
||
230 |
|||
231 |
✗✓ | 763 |
if (!geometry) |
232 |
{ |
||
233 |
throw std::invalid_argument("The polyhedron retrieved is empty"); |
||
234 |
} |
||
235 |
|||
236 |
763 |
return geometry; |
|
237 |
} |
||
238 |
#endif // PINOCCHIO_WITH_HPP_FCL |
||
239 |
|||
240 |
/** |
||
241 |
* @brief Get the first geometry attached to a link |
||
242 |
* |
||
243 |
* @param[in] link The URDF link |
||
244 |
* |
||
245 |
* @return Either the first collision or visual |
||
246 |
*/ |
||
247 |
template<typename T> |
||
248 |
inline PINOCCHIO_URDF_SHARED_PTR(const T) |
||
249 |
getLinkGeometry(const ::urdf::LinkConstSharedPtr link); |
||
250 |
|||
251 |
template<> |
||
252 |
inline ::urdf::CollisionConstSharedPtr |
||
253 |
1237 |
getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link) |
|
254 |
{ |
||
255 |
1237 |
return link->collision; |
|
256 |
} |
||
257 |
|||
258 |
template<> |
||
259 |
inline ::urdf::VisualConstSharedPtr |
||
260 |
333 |
getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link) |
|
261 |
{ |
||
262 |
333 |
return link->visual; |
|
263 |
} |
||
264 |
|||
265 |
|||
266 |
/** |
||
267 |
* @brief Get the material values from the link visual object |
||
268 |
* |
||
269 |
* @param[in] Visual/Collision The Visual or the Collision object. |
||
270 |
* @param[out] meshTexturePath The absolute file path containing the texture description. |
||
271 |
* @param[out] meshColor The mesh RGBA vector. |
||
272 |
* @param[in] package_dirs A vector containing the different directories where to search for packages |
||
273 |
* |
||
274 |
*/ |
||
275 |
template<typename urdfObject> |
||
276 |
inline bool getVisualMaterial(const PINOCCHIO_URDF_SHARED_PTR(urdfObject) urdf_object,std::string & meshTexturePath, |
||
277 |
Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs); |
||
278 |
|||
279 |
template<> |
||
280 |
543 |
inline bool getVisualMaterial< ::urdf::Collision> |
|
281 |
(const ::urdf::CollisionSharedPtr, std::string& meshTexturePath, |
||
282 |
Eigen::Vector4d & meshColor, const std::vector<std::string> &) |
||
283 |
{ |
||
284 |
✓✗✓✗ ✓✗✓✗ |
543 |
meshColor << 0.9, 0.9, 0.9, 1.; |
285 |
543 |
meshTexturePath = ""; |
|
286 |
543 |
return false; |
|
287 |
} |
||
288 |
|||
289 |
template<> |
||
290 |
220 |
inline bool getVisualMaterial< ::urdf::Visual> |
|
291 |
(const ::urdf::VisualSharedPtr urdf_visual, std::string& meshTexturePath, |
||
292 |
Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs) |
||
293 |
{ |
||
294 |
✓✗✓✗ ✓✗✓✗ |
220 |
meshColor << 0.9, 0.9, 0.9, 1.; |
295 |
220 |
meshTexturePath = ""; |
|
296 |
220 |
bool overrideMaterial = false; |
|
297 |
✓✓ | 220 |
if(urdf_visual->material) { |
298 |
167 |
overrideMaterial = true; |
|
299 |
✓✗✓✗ |
167 |
meshColor << urdf_visual->material->color.r, urdf_visual->material->color.g, |
300 |
✓✗✓✗ |
167 |
urdf_visual->material->color.b, urdf_visual->material->color.a; |
301 |
✗✓ | 167 |
if(urdf_visual->material->texture_filename!="") |
302 |
meshTexturePath = retrieveResourcePath((urdf_visual)->material->texture_filename, package_dirs); |
||
303 |
} |
||
304 |
220 |
return overrideMaterial; |
|
305 |
} |
||
306 |
|||
307 |
/** |
||
308 |
* @brief Get the array of geometries attached to a link |
||
309 |
* |
||
310 |
* @param[in] link The URDF link |
||
311 |
* |
||
312 |
* @return the array of either collisions or visuals |
||
313 |
*/ |
||
314 |
template<typename T> |
||
315 |
inline const std::vector< PINOCCHIO_URDF_SHARED_PTR(T) > & |
||
316 |
getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link); |
||
317 |
|||
318 |
template<> |
||
319 |
inline const std::vector< ::urdf::CollisionSharedPtr> & |
||
320 |
540 |
getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link) |
|
321 |
{ |
||
322 |
540 |
return link->collision_array; |
|
323 |
} |
||
324 |
|||
325 |
template<> |
||
326 |
inline const std::vector< ::urdf::VisualSharedPtr> & |
||
327 |
220 |
getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link) |
|
328 |
{ |
||
329 |
220 |
return link->visual_array; |
|
330 |
} |
||
331 |
|||
332 |
/** |
||
333 |
* @brief Add the geometries attached to a URDF link to a GeometryModel, looking |
||
334 |
* either for collisions or visuals |
||
335 |
* |
||
336 |
* @param[in] tree The URDF kinematic tree |
||
337 |
* @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded geometries |
||
338 |
* @param[in] link The current URDF link |
||
339 |
* @param model The model to which is the GeometryModel associated |
||
340 |
* @param geomModel The GeometryModel where the Collision Objects must be added |
||
341 |
* @param[in] package_dirs A vector containing the different directories where to search for packages |
||
342 |
* |
||
343 |
*/ |
||
344 |
template<typename GeometryType> |
||
345 |
3140 |
static void addLinkGeometryToGeomModel(const UrdfTree & tree, |
|
346 |
::hpp::fcl::MeshLoaderPtr & meshLoader, |
||
347 |
::urdf::LinkConstSharedPtr link, |
||
348 |
UrdfGeomVisitorBase& visitor, |
||
349 |
GeometryModel & geomModel, |
||
350 |
const std::vector<std::string> & package_dirs) |
||
351 |
{ |
||
352 |
#ifndef PINOCCHIO_WITH_HPP_FCL |
||
353 |
PINOCCHIO_UNUSED_VARIABLE(tree); |
||
354 |
PINOCCHIO_UNUSED_VARIABLE(meshLoader); |
||
355 |
#endif // PINOCCHIO_WITH_HPP_FCL |
||
356 |
|||
357 |
typedef std::vector< PINOCCHIO_URDF_SHARED_PTR(GeometryType) > VectorSharedT; |
||
358 |
typedef GeometryModel::SE3 SE3; |
||
359 |
|||
360 |
✓✓ | 3140 |
if(getLinkGeometry<GeometryType>(link)) |
361 |
{ |
||
362 |
✓✗ | 3040 |
std::string meshPath = ""; |
363 |
|||
364 |
✓✗✓✗ |
1520 |
Eigen::Vector3d meshScale(Eigen::Vector3d::Ones()); |
365 |
|||
366 |
1520 |
const std::string & link_name = link->name; |
|
367 |
|||
368 |
✓✗ | 3040 |
VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link); |
369 |
|||
370 |
FrameIndex frame_id; |
||
371 |
✓✗ | 3040 |
UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame (link_name, frame_id); |
372 |
1520 |
const SE3 & body_placement = frame.placement; |
|
373 |
|||
374 |
1520 |
std::size_t objectId = 0; |
|
375 |
✓✓ | 3046 |
for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i) |
376 |
{ |
||
377 |
1526 |
meshPath.clear(); |
|
378 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
379 |
|||
380 |
#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME |
||
381 |
const std::string & geom_name = (*i)->group_name; |
||
382 |
#else |
||
383 |
1526 |
const std::string & geom_name = (*i)->name; |
|
384 |
#endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME |
||
385 |
✓✗ | 3052 |
const GeometryObject::CollisionGeometryPtr geometry = |
386 |
retrieveCollisionGeometry(tree, meshLoader, link_name, geom_name, |
||
387 |
1526 |
(*i)->geometry, package_dirs, meshPath, meshScale); |
|
388 |
#else |
||
389 |
::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry); |
||
390 |
if (urdf_mesh) |
||
391 |
{ |
||
392 |
meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs); |
||
393 |
retrieveMeshScale(urdf_mesh, meshScale); |
||
394 |
} |
||
395 |
|||
396 |
const shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry()); |
||
397 |
#endif // PINOCCHIO_WITH_HPP_FCL |
||
398 |
|||
399 |
✓✗ | 1526 |
Eigen::Vector4d meshColor; |
400 |
3052 |
std::string meshTexturePath; |
|
401 |
✓✗ | 1526 |
bool overrideMaterial = getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs); |
402 |
|||
403 |
✓✗✓✗ |
1526 |
const SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin); |
404 |
✓✗ | 3052 |
std::ostringstream geometry_object_suffix; |
405 |
✓✗✓✗ |
1526 |
geometry_object_suffix << "_" << objectId; |
406 |
✓✗✓✗ |
3052 |
const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str()); |
407 |
✓✗ | 1526 |
GeometryObject geometry_object(geometry_object_name, |
408 |
frame_id, frame.parent, |
||
409 |
geometry, |
||
410 |
geomPlacement, meshPath, meshScale, |
||
411 |
overrideMaterial, meshColor, meshTexturePath); |
||
412 |
✓✗ | 1526 |
geomModel.addGeometryObject(geometry_object); |
413 |
1526 |
++objectId; |
|
414 |
} |
||
415 |
} |
||
416 |
3140 |
} |
|
417 |
|||
418 |
/** |
||
419 |
* @brief Recursive procedure for reading the URDF tree, looking for geometries |
||
420 |
* This function fill the geometric model with geometry objects retrieved from the URDF tree |
||
421 |
* |
||
422 |
* @param[in] tree The URDF kinematic tree |
||
423 |
* @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded geometries |
||
424 |
* @param[in] link The current URDF link |
||
425 |
* @param model The model to which is the GeometryModel associated |
||
426 |
* @param geomModel The GeometryModel where the Collision Objects must be added |
||
427 |
* @param[in] package_dirs A vector containing the different directories where to search for packages |
||
428 |
* @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION) |
||
429 |
* |
||
430 |
*/ |
||
431 |
1570 |
void recursiveParseTreeForGeom(const UrdfTree& tree, |
|
432 |
::hpp::fcl::MeshLoaderPtr& meshLoader, |
||
433 |
::urdf::LinkConstSharedPtr link, |
||
434 |
UrdfGeomVisitorBase& visitor, |
||
435 |
GeometryModel & geomModel, |
||
436 |
const std::vector<std::string> & package_dirs, |
||
437 |
const GeometryType type) |
||
438 |
{ |
||
439 |
|||
440 |
✓✓✗ | 1570 |
switch(type) |
441 |
{ |
||
442 |
1237 |
case COLLISION: |
|
443 |
✓✗ | 1237 |
addLinkGeometryToGeomModel< ::urdf::Collision >(tree, meshLoader, link, visitor, geomModel, package_dirs); |
444 |
1237 |
break; |
|
445 |
333 |
case VISUAL: |
|
446 |
✓✗ | 333 |
addLinkGeometryToGeomModel< ::urdf::Visual >(tree, meshLoader, link, visitor, geomModel, package_dirs); |
447 |
333 |
break; |
|
448 |
default: |
||
449 |
break; |
||
450 |
} |
||
451 |
|||
452 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗ |
4628 |
BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links) |
453 |
{ |
||
454 |
✓✗ | 1529 |
recursiveParseTreeForGeom(tree, meshLoader, child, visitor, geomModel, package_dirs,type); |
455 |
} |
||
456 |
|||
457 |
1570 |
} |
|
458 |
|||
459 |
41 |
void parseTreeForGeom(UrdfGeomVisitorBase& visitor, |
|
460 |
const std::istream& xmlStream, |
||
461 |
const GeometryType type, |
||
462 |
GeometryModel & geomModel, |
||
463 |
const std::vector<std::string> & package_dirs, |
||
464 |
::hpp::fcl::MeshLoaderPtr meshLoader) |
||
465 |
{ |
||
466 |
82 |
std::string xmlStr; |
|
467 |
{ |
||
468 |
✓✗ | 41 |
std::ostringstream os; |
469 |
✓✗✓✗ |
41 |
os << xmlStream.rdbuf(); |
470 |
✓✗ | 41 |
xmlStr = os.str(); |
471 |
} |
||
472 |
|||
473 |
✓✗ | 82 |
details::UrdfTree tree; |
474 |
✓✗ | 41 |
tree.parse (xmlStr); |
475 |
|||
476 |
✓✗ | 82 |
std::vector<std::string> hint_directories(package_dirs); |
477 |
|||
478 |
// Append the ROS_PACKAGE_PATH |
||
479 |
✓✗ | 41 |
std::vector<std::string> ros_pkg_paths = rosPaths(); |
480 |
✓✗ | 41 |
hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end()); |
481 |
|||
482 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
483 |
✓✗✓✗ ✓✗ |
41 |
if (!meshLoader) meshLoader = fcl::MeshLoaderPtr(new fcl::MeshLoader); |
484 |
#endif // ifdef PINOCCHIO_WITH_HPP_FCL |
||
485 |
|||
486 |
✓✗ | 41 |
recursiveParseTreeForGeom(tree, meshLoader, tree.urdf_->getRoot(), |
487 |
visitor, geomModel, hint_directories,type); |
||
488 |
41 |
} |
|
489 |
} // namespace details |
||
490 |
/// \endinternal |
||
491 |
} // namespace urdf |
||
492 |
} // namespace pinocchio |
Generated by: GCOVR (Version 4.2) |