Head

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 %


//
// 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
        mesh_loader = bp::extract<::hpp::fcl::MeshLoaderPtr>(py_mesh_loader);
#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)
      {
        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);
        return result;
      }
    };


    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			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