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/bindings/python/parsers/urdf.hpp" |
||
7 |
#include "pinocchio/bindings/python/utils/list.hpp" |
||
8 |
|||
9 |
#include <boost/python.hpp> |
||
10 |
|||
11 |
namespace pinocchio |
||
12 |
{ |
||
13 |
namespace python |
||
14 |
{ |
||
15 |
|||
16 |
namespace bp = boost::python; |
||
17 |
|||
18 |
#ifdef PINOCCHIO_WITH_URDFDOM |
||
19 |
typedef ::hpp::fcl::MeshLoaderPtr MeshLoaderPtr; |
||
20 |
|||
21 |
void |
||
22 |
21 |
buildGeomFromUrdf_existing(const Model & model, |
|
23 |
const std::istream & stream, |
||
24 |
const GeometryType type, |
||
25 |
GeometryModel & geometry_model, |
||
26 |
bp::object py_pkg_dirs, |
||
27 |
bp::object py_mesh_loader) |
||
28 |
{ |
||
29 |
42 |
MeshLoaderPtr mesh_loader = MeshLoaderPtr(); |
|
30 |
✗✓ | 21 |
if (!py_mesh_loader.is_none()) { |
31 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
32 |
mesh_loader = bp::extract<::hpp::fcl::MeshLoaderPtr>(py_mesh_loader); |
||
33 |
#else |
||
34 |
PyErr_WarnEx(PyExc_UserWarning, "Mesh loader is ignored because Pinocchio is not built with hpp-fcl", 1); |
||
35 |
#endif |
||
36 |
} |
||
37 |
|||
38 |
42 |
std::vector<std::string> pkg_dirs; |
|
39 |
|||
40 |
✓✗ | 21 |
bp::extract<std::string> pkg_dir_extract(py_pkg_dirs); |
41 |
✓✗ | 21 |
bp::extract<bp::list> pkg_dirs_list_extract(py_pkg_dirs); |
42 |
✓✗ | 21 |
bp::extract<const std::vector<std::string>&> pkg_dirs_vect_extract(py_pkg_dirs); |
43 |
✓✗ | 21 |
if (py_pkg_dirs.is_none()) {} // Provided None |
44 |
✓✗ | 21 |
else if (pkg_dir_extract.check()) // Provided a string |
45 |
✓✗✓✗ |
21 |
pkg_dirs.push_back(pkg_dir_extract()); |
46 |
else if (pkg_dirs_list_extract.check()) // Provided a list of string |
||
47 |
extract(pkg_dirs_list_extract(), pkg_dirs); |
||
48 |
else if (pkg_dirs_vect_extract.check()) // Provided a vector of string |
||
49 |
pkg_dirs = pkg_dirs_vect_extract(); |
||
50 |
else { // Did not understand the provided argument |
||
51 |
std::string what = bp::extract<std::string>(py_pkg_dirs.attr("__str__")())(); |
||
52 |
throw std::invalid_argument("pkg_dirs must be either None, a string or a list of strings. Provided " + what); |
||
53 |
} |
||
54 |
|||
55 |
✓✗ | 21 |
pinocchio::urdf::buildGeom(model,stream,type,geometry_model,pkg_dirs,mesh_loader); |
56 |
21 |
} |
|
57 |
|||
58 |
// This function is complex in order to keep backward compatibility. |
||
59 |
GeometryModel* |
||
60 |
21 |
buildGeomFromUrdfStream(const Model & model, |
|
61 |
const std::istream & stream, |
||
62 |
const GeometryType type, |
||
63 |
bp::object py_geom_model, |
||
64 |
bp::object package_dirs, |
||
65 |
bp::object mesh_loader) |
||
66 |
{ |
||
67 |
GeometryModel* geom_model; |
||
68 |
✓✓ | 21 |
if (py_geom_model.is_none()) |
69 |
✓✗ | 20 |
geom_model = new GeometryModel; |
70 |
else { |
||
71 |
✓✗ | 1 |
bp::extract<GeometryModel*> geom_model_extract(py_geom_model); |
72 |
✗✓ | 1 |
if (geom_model_extract.check()) |
73 |
geom_model = geom_model_extract(); |
||
74 |
else { |
||
75 |
// When backward compat is removed, the code in this `else` section |
||
76 |
// can be removed and the argument py_geom_model changed into a GeometryModel* |
||
77 |
✓✗ | 1 |
PyErr_WarnEx(PyExc_UserWarning, |
78 |
"You passed package dir(s) via argument geometry_model and provided package_dirs.",1); |
||
79 |
|||
80 |
// At this stage, py_geom_model contains the package dir(s). mesh_loader can |
||
81 |
// be passed either by package_dirs or mesh_loader |
||
82 |
✓✗ | 1 |
bp::object new_pkg_dirs = py_geom_model; |
83 |
✗✓✗✗ ✗✓ |
1 |
if (!package_dirs.is_none() && !mesh_loader.is_none()) |
84 |
throw std::invalid_argument("package_dirs and mesh_loader cannot be both provided since you passed the package dirs via argument geometry_model."); |
||
85 |
✓✗ | 1 |
if (mesh_loader.is_none()) |
86 |
✓✗ | 1 |
mesh_loader = package_dirs; |
87 |
try { |
||
88 |
// If geom_model is not a valid package_dir(s), then rethrow with clearer message |
||
89 |
✓✗✓✗ |
1 |
geom_model = new GeometryModel; |
90 |
✓✗✓✗ ✓✗ |
1 |
buildGeomFromUrdf_existing(model, stream, type, *geom_model, new_pkg_dirs, mesh_loader); |
91 |
1 |
return geom_model; |
|
92 |
} catch (std::invalid_argument const& e) { |
||
93 |
std::cout << "Caught: " << e.what() << std::endl; |
||
94 |
throw std::invalid_argument("Argument geometry_model should be a GeometryModel"); |
||
95 |
} |
||
96 |
} |
||
97 |
} |
||
98 |
✓✗✓✗ |
20 |
buildGeomFromUrdf_existing(model, stream, type, *geom_model, package_dirs, mesh_loader); |
99 |
20 |
return geom_model; |
|
100 |
} |
||
101 |
|||
102 |
GeometryModel* |
||
103 |
21 |
buildGeomFromUrdfFile(const Model & model, |
|
104 |
const std::string & filename, |
||
105 |
const GeometryType type, |
||
106 |
bp::object geom_model, |
||
107 |
bp::object package_dirs, |
||
108 |
bp::object mesh_loader) |
||
109 |
{ |
||
110 |
✓✗ | 21 |
std::ifstream stream(filename.c_str()); |
111 |
✓✗✗✓ |
21 |
if (!stream.is_open()) |
112 |
{ |
||
113 |
throw std::invalid_argument(filename + " does not seem to be a valid file."); |
||
114 |
} |
||
115 |
✓✗✓✗ ✓✗✓✗ |
42 |
return buildGeomFromUrdfStream(model, stream, type, geom_model, package_dirs, mesh_loader); |
116 |
} |
||
117 |
|||
118 |
GeometryModel* |
||
119 |
buildGeomFromUrdfString(const Model & model, |
||
120 |
const std::string & xmlString, |
||
121 |
const GeometryType type, |
||
122 |
bp::object geom_model, |
||
123 |
bp::object package_dirs, |
||
124 |
bp::object mesh_loader) |
||
125 |
{ |
||
126 |
std::istringstream stream(xmlString); |
||
127 |
return buildGeomFromUrdfStream(model, stream, type, geom_model, package_dirs, mesh_loader); |
||
128 |
} |
||
129 |
|||
130 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
131 |
# define MESH_LOADER_DOC "\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries).\n" |
||
132 |
#else // #ifdef PINOCCHIO_WITH_HPP_FCL |
||
133 |
# define MESH_LOADER_DOC "\tmesh_loader: unused because the Pinocchio is built without hpp-fcl\n" |
||
134 |
#endif // #ifdef PINOCCHIO_WITH_HPP_FCL |
||
135 |
template <std::size_t owner_arg = 1> |
||
136 |
struct return_value_policy : bp::return_internal_reference<owner_arg> |
||
137 |
{ |
||
138 |
public: |
||
139 |
template <class ArgumentPackage> |
||
140 |
21 |
static PyObject* postcall(ArgumentPackage const& args_, PyObject* result) |
|
141 |
{ |
||
142 |
21 |
PyObject* patient = bp::detail::get_prev<owner_arg>::execute(args_, result); |
|
143 |
✓✓ | 21 |
if (patient != Py_None) |
144 |
1 |
return bp::return_internal_reference<owner_arg>::postcall(args_, result); |
|
145 |
20 |
return result; |
|
146 |
} |
||
147 |
}; |
||
148 |
|||
149 |
|||
150 |
template<typename F> |
||
151 |
38 |
void defBuildUrdf(const char* name, F f, const char* urdf_arg, const char* urdf_doc) |
|
152 |
{ |
||
153 |
✓✗ | 38 |
std::ostringstream doc; |
154 |
doc << "Parse the URDF file given as input looking for the geometry of the given input model and\n" |
||
155 |
"and store either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL) in a GeometryModel object.\n" |
||
156 |
"Parameters:\n" |
||
157 |
"\tmodel: model of the robot\n" |
||
158 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
38 |
"\n" << urdf_arg << ": " << urdf_doc << "\n" |
159 |
"\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n" |
||
160 |
"\tgeometry_model: if provided, this geometry model will be used to store the parsed information instead of creating a new one\n" |
||
161 |
"\tpackage_dirs: either a single path or a vector of paths pointing to folders containing the model of the robot\n" |
||
162 |
MESH_LOADER_DOC |
||
163 |
"\n" |
||
164 |
"Retuns:\n" |
||
165 |
"\ta new GeometryModel if `geometry_model` is None else `geometry_model` (that has been updated).\n"; |
||
166 |
|||
167 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
114 |
bp::def(name, f, |
168 |
(bp::arg("model"), |
||
169 |
bp::arg(urdf_arg), |
||
170 |
bp::arg("geom_type"), |
||
171 |
✓✗✓✗ ✓✗ |
76 |
bp::arg("geometry_model") = static_cast<GeometryModel*>(NULL), |
172 |
✓✗✓✗ ✓✗✓✗ |
76 |
bp::arg("package_dirs") = bp::object(), |
173 |
✓✗✓✗ ✓✗✓✗ |
76 |
bp::arg("mesh_loader") = bp::object()), |
174 |
✓✗✓✗ |
76 |
doc.str().c_str(), return_value_policy<4>()); |
175 |
38 |
} |
|
176 |
|||
177 |
#endif |
||
178 |
|||
179 |
19 |
void exposeURDFGeometry() |
|
180 |
{ |
||
181 |
#ifdef PINOCCHIO_WITH_URDFDOM |
||
182 |
19 |
defBuildUrdf("buildGeomFromUrdf", buildGeomFromUrdfFile, "urdf_filename", |
|
183 |
"path to the URDF file containing the model of the robot"); |
||
184 |
19 |
defBuildUrdf("buildGeomFromUrdfString", buildGeomFromUrdfString, "urdf_string", |
|
185 |
"a string containing the URDF model of the robot"); |
||
186 |
#endif // #ifdef PINOCCHIO_WITH_URDFDOM |
||
187 |
19 |
} |
|
188 |
} |
||
189 |
} |
||
190 |
Generated by: GCOVR (Version 4.2) |