GCC Code Coverage Report

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