GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: bindings/python/parsers/urdf/geometry.cpp Lines: 49 65 75.4 %
Date: 2023-08-09 08:43:58 Branches: 60 176 34.1 %

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