Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	bindings/python/parsers/urdf/geometry.cpp	Lines:	51	69	73.9 %
Date:	2024-04-26 13:14:21	Branches:	65	188	34.6 %


//
// Copyright (c) 2015-2020 CNRS INRIA
//

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/bindings/python/parsers/urdf.hpp"
#include "pinocchio/bindings/python/utils/list.hpp"

#include <boost/python.hpp>

namespace pinocchio
{
  namespace python
  {

    namespace bp = boost::python;

#ifdef PINOCCHIO_WITH_URDFDOM
    typedef ::hpp::fcl::MeshLoaderPtr MeshLoaderPtr;

    void
    buildGeomFromUrdf_existing(const Model & model,
                               const std::istream & stream,
                               const GeometryType type,
                               GeometryModel & geometry_model,
                               bp::object py_pkg_dirs,
                               bp::object py_mesh_loader)
    {
      MeshLoaderPtr mesh_loader = MeshLoaderPtr();
      if (!py_mesh_loader.is_none()) {
#ifdef PINOCCHIO_WITH_HPP_FCL
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
        mesh_loader = bp::extract<::hpp::fcl::MeshLoaderPtr>(py_mesh_loader);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
#else
        PyErr_WarnEx(PyExc_UserWarning, "Mesh loader is ignored because Pinocchio is not built with hpp-fcl", 1);
#endif
      }

      std::vector<std::string> pkg_dirs;

      bp::extract<std::string> pkg_dir_extract(py_pkg_dirs);
      bp::extract<bp::list> pkg_dirs_list_extract(py_pkg_dirs);
      bp::extract<const std::vector<std::string>&> pkg_dirs_vect_extract(py_pkg_dirs);
      if (py_pkg_dirs.is_none()) {} // Provided None
      else if (pkg_dir_extract.check()) // Provided a string
        pkg_dirs.push_back(pkg_dir_extract());
      else if (pkg_dirs_list_extract.check()) // Provided a list of string
        extract(pkg_dirs_list_extract(), pkg_dirs);
      else if (pkg_dirs_vect_extract.check()) // Provided a vector of string
        pkg_dirs = pkg_dirs_vect_extract();
      else { // Did not understand the provided argument
        std::string what = bp::extract<std::string>(py_pkg_dirs.attr("__str__")())();
        throw std::invalid_argument("pkg_dirs must be either None, a string or a list of strings. Provided " + what);
      }

      pinocchio::urdf::buildGeom(model,stream,type,geometry_model,pkg_dirs,mesh_loader);
    }

    // This function is complex in order to keep backward compatibility.
    GeometryModel*
    buildGeomFromUrdfStream(const Model & model,
                            const std::istream & stream,
                            const GeometryType type,
                            bp::object py_geom_model,
                            bp::object package_dirs,
                            bp::object mesh_loader)
    {
      GeometryModel* geom_model;
      if (py_geom_model.is_none())
        geom_model = new GeometryModel;
      else {
        bp::extract<GeometryModel*> geom_model_extract(py_geom_model);
        if (geom_model_extract.check())
          geom_model = geom_model_extract();
        else {
          // When backward compat is removed, the code in this `else` section
          // can be removed and the argument py_geom_model changed into a GeometryModel*
          PyErr_WarnEx(PyExc_UserWarning,
              "You passed package dir(s) via argument geometry_model and provided package_dirs.",1);

          // At this stage, py_geom_model contains the package dir(s). mesh_loader can
          // be passed either by package_dirs or mesh_loader
          bp::object new_pkg_dirs = py_geom_model;
          if (!package_dirs.is_none() && !mesh_loader.is_none())
            throw std::invalid_argument("package_dirs and mesh_loader cannot be both provided since you passed the package dirs via argument geometry_model.");
          if (mesh_loader.is_none())
            mesh_loader = package_dirs;
          try {
            // If geom_model is not a valid package_dir(s), then rethrow with clearer message
            geom_model = new GeometryModel;
            buildGeomFromUrdf_existing(model, stream, type, *geom_model, new_pkg_dirs, mesh_loader);
            return geom_model;
          } catch (std::invalid_argument const& e) {
            std::cout << "Caught: " << e.what() << std::endl;
            throw std::invalid_argument("Argument geometry_model should be a GeometryModel");
          }
        }
      }
      buildGeomFromUrdf_existing(model, stream, type, *geom_model, package_dirs, mesh_loader);
      return geom_model;
    }

    GeometryModel*
    buildGeomFromUrdfFile(const Model & model,
                          const std::string & filename,
                          const GeometryType type,
                          bp::object geom_model,
                          bp::object package_dirs,
                          bp::object mesh_loader)
    {
      std::ifstream stream(filename.c_str());
      if (!stream.is_open())
      {
        throw std::invalid_argument(filename + " does not seem to be a valid file.");
      }
      return buildGeomFromUrdfStream(model, stream, type, geom_model, package_dirs, mesh_loader);
    }

    GeometryModel*
    buildGeomFromUrdfString(const Model & model,
                            const std::string & xmlString,
                            const GeometryType type,
                            bp::object geom_model,
                            bp::object package_dirs,
                            bp::object mesh_loader)
    {
      std::istringstream stream(xmlString);
      return buildGeomFromUrdfStream(model, stream, type, geom_model, package_dirs, mesh_loader);
    }

#ifdef PINOCCHIO_WITH_HPP_FCL
# define MESH_LOADER_DOC "\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries).\n"
#else // #ifdef PINOCCHIO_WITH_HPP_FCL
# define MESH_LOADER_DOC "\tmesh_loader: unused because the Pinocchio is built without hpp-fcl\n"
#endif // #ifdef PINOCCHIO_WITH_HPP_FCL
    template <std::size_t owner_arg = 1>
    struct return_value_policy : bp::return_internal_reference<owner_arg>
    {
     public:
      template <class ArgumentPackage>
      static PyObject* postcall(ArgumentPackage const& args_, PyObject* result)
      {
        // If owner_arg exists, we run bp::return_internal_reference postcall
        // result lifetime will be tied to the owner_arg lifetime
        PyObject* patient = bp::detail::get_prev<owner_arg>::execute(args_, result);
        if (patient != Py_None)
          return bp::return_internal_reference<owner_arg>::postcall(args_, result);
        // If owner_arg doesn't exist, then Python will have to manage the result lifetime
        bp::extract<GeometryModel*> geom_model_extract(result);
        if (geom_model_extract.check())
        {
          return bp::to_python_indirect<GeometryModel, bp::detail::make_owning_holder>()(geom_model_extract());
        }
        // If returned value is not a GeometryModel*, then raise an error
        PyErr_SetString(PyExc_RuntimeError, "pinocchio::python::return_value_policy only works on GeometryModel* data type");
        return 0;
      }
    };


    template<typename F>
    void defBuildUrdf(const char* name, F f, const char* urdf_arg, const char* urdf_doc)
    {
      std::ostringstream doc;
      doc << "Parse the URDF file given as input looking for the geometry of the given input model and\n"
             "and store either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL) in a GeometryModel object.\n"
             "Parameters:\n"
             "\tmodel: model of the robot\n"
             "\n" << urdf_arg << ": " << urdf_doc << "\n"
             "\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n"
             "\tgeometry_model: if provided, this geometry model will be used to store the parsed information instead of creating a new one\n"
             "\tpackage_dirs: either a single path or a vector of paths pointing to folders containing the model of the robot\n"
             MESH_LOADER_DOC
             "\n"
             "Retuns:\n"
             "\ta new GeometryModel if `geometry_model` is None else `geometry_model` (that has been updated).\n";

      bp::def(name, f,
          (bp::arg("model"),
           bp::arg(urdf_arg),
           bp::arg("geom_type"),
           bp::arg("geometry_model") = static_cast<GeometryModel*>(NULL),
           bp::arg("package_dirs") = bp::object(),
           bp::arg("mesh_loader") = bp::object()),
          doc.str().c_str(), return_value_policy<4>());
    }

#endif

    void exposeURDFGeometry()
    {
#ifdef PINOCCHIO_WITH_URDFDOM
      defBuildUrdf("buildGeomFromUrdf", buildGeomFromUrdfFile, "urdf_filename",
              "path to the URDF file containing the model of the robot");
      defBuildUrdf("buildGeomFromUrdfString", buildGeomFromUrdfString, "urdf_string",
              "a string containing the URDF model of the robot");
#endif // #ifdef PINOCCHIO_WITH_URDFDOM
    }
  }
}



Generated by: GCOVR (Version 4.2)

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