GCC Code Coverage Report

Directory: ./
File: bindings/python/parsers/urdf/geometry.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 53 66 80.3%
Branches: 67 160 41.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2022 CNRS INRIA
3 //
4
5 #ifdef PINOCCHIO_WITH_URDFDOM
6 #include "pinocchio/parsers/urdf.hpp"
7 #endif
8 #include "pinocchio/bindings/python/parsers/urdf.hpp"
9 #include "pinocchio/bindings/python/utils/list.hpp"
10 #include "pinocchio/bindings/python/utils/path.hpp"
11
12 #include <boost/python.hpp>
13
14 namespace pinocchio
15 {
16 namespace python
17 {
18
19 namespace bp = boost::python;
20
21 #ifdef PINOCCHIO_WITH_URDFDOM
22 typedef ::hpp::fcl::MeshLoaderPtr MeshLoaderPtr;
23
24 50 void buildGeomFromUrdf_existing(
25 const Model & model,
26 const std::istream & stream,
27 const GeometryType type,
28 GeometryModel & geometry_model,
29 bp::object py_pkg_dirs,
30 bp::object py_mesh_loader)
31 {
32 50 MeshLoaderPtr mesh_loader = MeshLoaderPtr();
33
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 50 times.
 50 if (!py_mesh_loader.is_none())
34 {
35 #ifdef PINOCCHIO_WITH_HPP_FCL
36 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
37 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
38 ✗ mesh_loader = bp::extract<::hpp::fcl::MeshLoaderPtr>(py_mesh_loader);
39 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
40 #else
41 PyErr_WarnEx(
42 PyExc_UserWarning, "Mesh loader is ignored because Pinocchio is not built with hpp-fcl",
43 1);
44 #endif
45 }
46
47 50 std::vector<std::string> pkg_dirs;
48
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 if (py_pkg_dirs.ptr() == Py_None)
49 {
50 }
51
2/2
✓ Branch 2 taken 22 times.
✓ Branch 3 taken 28 times.
 50 else if (PyList_Check(py_pkg_dirs.ptr()))
52 {
53
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 pkg_dirs = pathList(py_pkg_dirs);
54 }
55 else
56 {
57
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 28 pkg_dirs.push_back(path(py_pkg_dirs));
58 }
59
60
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 pinocchio::urdf::buildGeom(model, stream, type, geometry_model, pkg_dirs, mesh_loader);
61 50 }
62
63 // This function is complex in order to keep backward compatibility.
64 50 GeometryModel * buildGeomFromUrdfStream(
65 const Model & model,
66 const std::istream & stream,
67 const GeometryType type,
68 bp::object py_geom_model,
69 bp::object package_dirs,
70 bp::object mesh_loader)
71 {
72 GeometryModel * geom_model;
73
2/2
✓ Branch 1 taken 42 times.
✓ Branch 2 taken 8 times.
 50 if (py_geom_model.is_none())
74
1/2
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
 42 geom_model = new GeometryModel;
75 else
76 {
77
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 bp::extract<GeometryModel *> geom_model_extract(py_geom_model);
78
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 5 times.
 8 if (geom_model_extract.check())
79
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 geom_model = geom_model_extract();
80 else
81 {
82 // When backward compat is removed, the code in this `else` section
83 // can be removed and the argument py_geom_model changed into a GeometryModel*
84
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 PyErr_WarnEx(
85 PyExc_UserWarning,
86 "You passed package dir(s) via argument geometry_model and provided package_dirs.", 1);
87
88 // At this stage, py_geom_model contains the package dir(s). mesh_loader can
89 // be passed either by package_dirs or mesh_loader
90
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 bp::object new_pkg_dirs = py_geom_model;
91
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 5 times.
 5 if (!package_dirs.is_none() && !mesh_loader.is_none())
92 ✗ throw std::invalid_argument(
93 "package_dirs and mesh_loader cannot be both provided since you passed the package "
94 ✗ "dirs via argument geometry_model.");
95
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 if (mesh_loader.is_none())
96
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 mesh_loader = package_dirs;
97 try
98 {
99 // If geom_model is not a valid package_dir(s), then rethrow with clearer message
100
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 geom_model = new GeometryModel;
101
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 buildGeomFromUrdf_existing(model, stream, type, *geom_model, new_pkg_dirs, mesh_loader);
102 5 return geom_model;
103 }
104 ✗ catch (std::invalid_argument const & e)
105 {
106 ✗ std::cout << "Caught: " << e.what() << std::endl;
107 ✗ throw std::invalid_argument("Argument geometry_model should be a GeometryModel");
108 }
109 5 }
110 }
111
2/4
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
 45 buildGeomFromUrdf_existing(model, stream, type, *geom_model, package_dirs, mesh_loader);
112 45 return geom_model;
113 }
114
115 50 GeometryModel * buildGeomFromUrdfFile(
116 const Model & model,
117 const bp::object & filename,
118 const GeometryType type,
119 bp::object geom_model,
120 bp::object package_dirs,
121 bp::object mesh_loader)
122 {
123
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 const std::string filename_s = path(filename);
124
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 std::ifstream stream(filename_s.c_str());
125
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 50 times.
 50 if (!stream.is_open())
126 {
127 ✗ throw std::invalid_argument(filename_s + " does not seem to be a valid file.");
128 }
129
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
 100 return buildGeomFromUrdfStream(model, stream, type, geom_model, package_dirs, mesh_loader);
130 50 }
131
132 ✗ GeometryModel * buildGeomFromUrdfString(
133 const Model & model,
134 const std::string & xmlString,
135 const GeometryType type,
136 bp::object geom_model,
137 bp::object package_dirs,
138 bp::object mesh_loader)
139 {
140 ✗ std::istringstream stream(xmlString);
141 ✗ return buildGeomFromUrdfStream(model, stream, type, geom_model, package_dirs, mesh_loader);
142 }
143
144 #ifdef PINOCCHIO_WITH_HPP_FCL
145 #define MESH_LOADER_DOC \
146 "\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries).\n"
147 #else // #ifdef PINOCCHIO_WITH_HPP_FCL
148 #define MESH_LOADER_DOC "\tmesh_loader: unused because the Pinocchio is built without hpp-fcl\n"
149 #endif // #ifdef PINOCCHIO_WITH_HPP_FCL
150 template<std::size_t owner_arg = 1>
151 struct return_value_policy : bp::return_internal_reference<owner_arg>
152 {
153 public:
154 template<class ArgumentPackage>
155 50 static PyObject * postcall(ArgumentPackage const & args_, PyObject * result)
156 {
157 // If owner_arg exists, we run bp::return_internal_reference postcall
158 // result lifetime will be tied to the owner_arg lifetime
159 50 PyObject * patient = bp::detail::get_prev<owner_arg>::execute(args_, result);
160
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 42 times.
 50 if (patient != Py_None)
161
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 return bp::return_internal_reference<owner_arg>::postcall(args_, result);
162 // If owner_arg doesn't exist, then Python will have to manage the result lifetime
163
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 42 bp::extract<GeometryModel *> geom_model_extract(result);
164
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 42 if (geom_model_extract.check())
165 {
166 ✗ return bp::to_python_indirect<GeometryModel, bp::detail::make_owning_holder>()(
167
2/4
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
 42 geom_model_extract());
168 }
169 // If returned value is not a GeometryModel*, then raise an error
170 ✗ PyErr_SetString(
171 PyExc_RuntimeError,
172 "pinocchio::python::return_value_policy only works on GeometryModel* data type");
173 ✗ return 0;
174 }
175 };
176
177 template<typename F>
178 260 void defBuildUrdf(const char * name, F f, const char * urdf_arg, const char * urdf_doc)
179 {
180
1/2
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
 260 std::ostringstream doc;
181 doc << "Parse the URDF file given as input looking for the geometry of the given input model "
182 "and\n"
183 "and store either the collision geometries (GeometryType.COLLISION) or the visual "
184 "geometries (GeometryType.VISUAL) in a GeometryModel object.\n"
185 "Parameters:\n"
186 "\tmodel: model of the robot\n"
187 "\n"
188 << urdf_arg << ": " << urdf_doc
189
5/10
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 130 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 130 times.
✗ Branch 14 not taken.
 260 << "\n"
190 "\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for "
191 "display or the COLLISION for collision detection).\n"
192 "\tgeometry_model: if provided, this geometry model will be used to store the parsed "
193 "information instead of creating a new one\n"
194 "\tpackage_dirs: either a single path or a vector of paths pointing to folders "
195 "containing the model of the robot\n" MESH_LOADER_DOC "\n"
196 "Retuns:\n"
197 "\ta new GeometryModel if `geometry_model` is None else `geometry_model` (that has "
198 "been updated).\n";
199
200
3/6
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
 780 bp::def(
201 name, f,
202
2/4
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
 780 (bp::arg("model"), bp::arg(urdf_arg), bp::arg("geom_type"),
203
3/6
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
 520 bp::arg("geometry_model") = static_cast<GeometryModel *>(NULL),
204
8/16
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 130 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 130 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 130 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 130 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 130 times.
✗ Branch 23 not taken.
 520 bp::arg("package_dirs") = bp::object(), bp::arg("mesh_loader") = bp::object()),
205
2/4
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 130 times.
✗ Branch 6 not taken.
 520 doc.str().c_str(), return_value_policy<4>());
206 260 }
207
208 #endif
209
210 65 void exposeURDFGeometry()
211 {
212 #ifdef PINOCCHIO_WITH_URDFDOM
213 65 defBuildUrdf(
214 "buildGeomFromUrdf", buildGeomFromUrdfFile, "urdf_filename",
215 "path to the URDF file containing the model of the robot");
216 65 defBuildUrdf(
217 "buildGeomFromUrdfString", buildGeomFromUrdfString, "urdf_string",
218 "a string containing the URDF model of the robot");
219 #endif // #ifdef PINOCCHIO_WITH_URDFDOM
220 65 }
221 } // namespace python
222 } // namespace pinocchio
223