Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/parsers/urdf/geometry.cpp	Lines:	146	163	89.6 %
Date:	2023-08-09 08:43:58	Branches:	197	395	49.9 %


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

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/urdf/types.hpp"
#include "pinocchio/parsers/urdf/utils.hpp"
#include "pinocchio/parsers/utils.hpp"

#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>

namespace pinocchio
{
  namespace urdf
  {
    /// \internal
    namespace details
    {
      struct UrdfTree
      {
        typedef boost::property_tree::ptree ptree;
        typedef std::map<std::string, const ptree&> LinkMap_t;

        void parse (const std::string & xmlStr)
        {
          urdf_ = ::urdf::parseURDF(xmlStr);
          if (!urdf_) {
            throw std::invalid_argument("Unable to parse URDF");
          }

          std::istringstream iss(xmlStr);
          using namespace boost::property_tree;
          read_xml(iss, tree_, xml_parser::no_comments);

          BOOST_FOREACH(const ptree::value_type & link, tree_.get_child("robot")) {
            if (link.first == "link") {
              std::string name = link.second.get<std::string>("<xmlattr>.name");
              links_.insert(std::pair<std::string,const ptree&>(name, link.second));
            }
          } // BOOST_FOREACH
        }

        bool isCapsule(const std::string & linkName,
                       const std::string & geomName) const
        {
          LinkMap_t::const_iterator _link = links_.find(linkName);
          assert (_link != links_.end());
          const ptree& link = _link->second;
          if (link.count ("collision_checking") == 0)
            return false;
          BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
            if (cc.first == "capsule") {
#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
              std::cerr << "Warning: support for tag link/collision_checking/capsule"
                " is not available for URDFDOM < 0.3.0" << std::endl;
#else
              std::string name = cc.second.get<std::string>("<xmlattr>.name");
              if (geomName == name) return true;
#endif
            }
          } // BOOST_FOREACH

          return false;
        }

        bool isMeshConvex(const std::string & linkName,
                          const std::string & geomName) const
        {
          LinkMap_t::const_iterator _link = links_.find(linkName);
          assert (_link != links_.end());
          const ptree& link = _link->second;
          if (link.count ("collision_checking") == 0)
            return false;
          BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
            if (cc.first == "convex") {
#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
              std::cerr << "Warning: support for tag link/collision_checking/convex"
                " is not available for URDFDOM < 0.3.0" << std::endl;
#else
              std::string name = cc.second.get<std::string>("<xmlattr>.name");
              if (geomName == name) return true;
#endif
            }
          } // BOOST_FOREACH

          return false;
        }

        // For standard URDF tags
        ::urdf::ModelInterfaceSharedPtr urdf_;
        // For other tags
        ptree tree_;
        // A mapping from link name to corresponding child of tree_
        LinkMap_t links_;
      };

      template<typename Vector3>
      static void retrieveMeshScale(const ::urdf::MeshSharedPtr & mesh,
                                    const Eigen::MatrixBase<Vector3> & scale)
      {
        Vector3 & scale_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,scale);
        scale_ <<
        mesh->scale.x,
        mesh->scale.y,
        mesh->scale.z;
      }

#ifdef PINOCCHIO_WITH_HPP_FCL
# if ( HPP_FCL_MAJOR_VERSION>1 || ( HPP_FCL_MAJOR_VERSION==1 && \
      ( HPP_FCL_MINOR_VERSION>1 || ( HPP_FCL_MINOR_VERSION==1 && \
                                     HPP_FCL_PATCH_VERSION>3))))
#  define PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
# endif

      /**
       * @brief      Get a fcl::CollisionObject from a URDF geometry, searching
       *             for it in specified package directories
       *
       * @param[in]  urdf_geometry  A shared pointer on the input urdf Geometry
       * @param[in]  package_dirs   A vector containing the different directories where to search for packages
       * @param[out] meshPath      The Absolute path of the mesh currently read
       * @param[out] meshScale     Scale of transformation currently applied to the mesh
       *
       * @return     A shared pointer on the geometry converted as a fcl::CollisionGeometry
       */
      shared_ptr<fcl::CollisionGeometry>
      inline retrieveCollisionGeometry(const UrdfTree& tree,
                                       fcl::MeshLoaderPtr& meshLoader,
                                       const std::string& linkName,
                                       const std::string& geomName,
                                       const ::urdf::GeometrySharedPtr urdf_geometry,
                                       const std::vector<std::string> & package_dirs,
                                       std::string & meshPath,
                                       Eigen::Vector3d & meshScale)
      {
        shared_ptr<fcl::CollisionGeometry> geometry;

        // Handle the case where collision geometry is a mesh
        if (urdf_geometry->type == ::urdf::Geometry::MESH)
        {
          const ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
          std::string collisionFilename = urdf_mesh->filename;

          meshPath = retrieveResourcePath(collisionFilename, package_dirs);
          if (meshPath == "") {
            std::stringstream ss;
            ss << "Mesh " << collisionFilename << " could not be found.";
            throw std::invalid_argument (ss.str());
          }

          fcl::Vec3f scale = fcl::Vec3f(urdf_mesh->scale.x,
                                        urdf_mesh->scale.y,
                                        urdf_mesh->scale.z
                                        );

          retrieveMeshScale(urdf_mesh, meshScale);

          // Create FCL mesh by parsing Collada file.
#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
          hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale);
          bool convex = tree.isMeshConvex (linkName, geomName);
          if (convex) {
            bvh->buildConvexRepresentation (false);
#if HPP_FCL_MAJOR_VERSION < 2
            geometry = bvh->convex;
#else
            geometry = shared_ptr<fcl::CollisionGeometry>(bvh->convex.get(), [bvh](...) mutable { bvh->convex.reset(); });
#endif  // HPP_FCL_MAJOR_VERSION
          } else {
#if HPP_FCL_MAJOR_VERSION < 2
            geometry = bvh;
#else
            geometry = shared_ptr<fcl::CollisionGeometry>(bvh.get(), [bvh](...) mutable { bvh.reset(); });
#endif  // HPP_FCL_MAJOR_VERSION
          }
#else
          geometry = meshLoader->load (meshPath, scale);
#endif
        }

        // Handle the case where collision geometry is a cylinder
        // Use FCL capsules for cylinders
        else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
        {
          const bool is_capsule = tree.isCapsule(linkName, geomName);
          meshScale << 1,1,1;
          const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);

          double radius = collisionGeometry->radius;
          double length = collisionGeometry->length;

          // Create fcl capsule geometry.
          if (is_capsule) {
            meshPath = "CAPSULE";
            geometry = shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length));
          } else {
            meshPath = "CYLINDER";
            geometry = shared_ptr < fcl::CollisionGeometry >(new fcl::Cylinder (radius, length));
          }
        }
        // Handle the case where collision geometry is a box.
        else if (urdf_geometry->type == ::urdf::Geometry::BOX)
        {
          meshPath = "BOX";
          meshScale << 1,1,1;
          const ::urdf::BoxSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);

          double x = collisionGeometry->dim.x;
          double y = collisionGeometry->dim.y;
          double z = collisionGeometry->dim.z;

          geometry = shared_ptr < fcl::CollisionGeometry > (new fcl::Box (x, y, z));
        }
        // Handle the case where collision geometry is a sphere.
        else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
        {
          meshPath = "SPHERE";
          meshScale << 1,1,1;
          const ::urdf::SphereSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);

          double radius = collisionGeometry->radius;

          geometry = shared_ptr < fcl::CollisionGeometry > (new fcl::Sphere (radius));
        }
        else throw std::invalid_argument("Unknown geometry type :");

        if (!geometry)
        {
          throw std::invalid_argument("The polyhedron retrieved is empty");
        }

        return geometry;
      }
#endif // PINOCCHIO_WITH_HPP_FCL

     /**
      * @brief Get the first geometry attached to a link
      *
      * @param[in] link   The URDF link
      *
      * @return Either the first collision or visual
      */
      template<typename T>
      inline PINOCCHIO_URDF_SHARED_PTR(const T)
      getLinkGeometry(const ::urdf::LinkConstSharedPtr link);

      template<>
      inline ::urdf::CollisionConstSharedPtr
      getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
      {
        return link->collision;
      }

      template<>
      inline ::urdf::VisualConstSharedPtr
      getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
      {
        return link->visual;
      }


     /**
      * @brief Get the material values from the link visual object
      *
      * @param[in]  Visual/Collision The Visual or the Collision object.
      * @param[out] meshTexturePath  The absolute file path containing the texture description.
      * @param[out] meshColor        The mesh RGBA vector.
      * @param[in]  package_dirs     A vector containing the different directories where to search for packages
      *
      */
      template<typename urdfObject>
      inline bool getVisualMaterial(const PINOCCHIO_URDF_SHARED_PTR(urdfObject) urdf_object,std::string & meshTexturePath,
                                    Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs);

      template<>
      inline bool getVisualMaterial< ::urdf::Collision>
      (const ::urdf::CollisionSharedPtr, std::string& meshTexturePath,
       Eigen::Vector4d & meshColor, const std::vector<std::string> &)
      {
        meshColor << 0.9, 0.9, 0.9, 1.;
        meshTexturePath = "";
        return false;
      }

      template<>
      inline bool getVisualMaterial< ::urdf::Visual>
      (const ::urdf::VisualSharedPtr urdf_visual, std::string& meshTexturePath,
       Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs)
      {
        meshColor << 0.9, 0.9, 0.9, 1.;
        meshTexturePath = "";
        bool overrideMaterial = false;
        if(urdf_visual->material) {
          overrideMaterial = true;
          meshColor << urdf_visual->material->color.r, urdf_visual->material->color.g,
          urdf_visual->material->color.b, urdf_visual->material->color.a;
          if(urdf_visual->material->texture_filename!="")
            meshTexturePath = retrieveResourcePath((urdf_visual)->material->texture_filename, package_dirs);
        }
        return overrideMaterial;
      }

     /**
      * @brief Get the array of geometries attached to a link
      *
      * @param[in] link   The URDF link
      *
      * @return the array of either collisions or visuals
      */
      template<typename T>
      inline const std::vector< PINOCCHIO_URDF_SHARED_PTR(T) > &
      getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link);

      template<>
      inline const std::vector< ::urdf::CollisionSharedPtr> &
      getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
      {
        return link->collision_array;
      }

      template<>
      inline const std::vector< ::urdf::VisualSharedPtr> &
      getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
      {
        return link->visual_array;
      }

      /**
       * @brief      Add the geometries attached to a URDF link to a GeometryModel, looking
       *             either for collisions or visuals
       *
       * @param[in]  tree           The URDF kinematic tree
       * @param[in]  meshLoader     The FCL mesh loader to avoid duplications of already loaded geometries
       * @param[in]  link            The current URDF link
       * @param      model           The model to which is the GeometryModel associated
       * @param      geomModel      The GeometryModel where the Collision Objects must be added
       * @param[in]  package_dirs    A vector containing the different directories where to search for packages
       *
       */
      template<typename GeometryType>
      static void addLinkGeometryToGeomModel(const UrdfTree & tree,
                                             ::hpp::fcl::MeshLoaderPtr & meshLoader,
                                             ::urdf::LinkConstSharedPtr link,
                                             UrdfGeomVisitorBase& visitor,
                                             GeometryModel & geomModel,
                                             const std::vector<std::string> & package_dirs)
      {
#ifndef PINOCCHIO_WITH_HPP_FCL
        PINOCCHIO_UNUSED_VARIABLE(tree);
        PINOCCHIO_UNUSED_VARIABLE(meshLoader);
#endif // PINOCCHIO_WITH_HPP_FCL

        typedef std::vector< PINOCCHIO_URDF_SHARED_PTR(GeometryType) > VectorSharedT;
        typedef GeometryModel::SE3 SE3;

        if(getLinkGeometry<GeometryType>(link))
        {
          std::string meshPath = "";

          Eigen::Vector3d meshScale(Eigen::Vector3d::Ones());

          const std::string & link_name = link->name;

          VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link);

          FrameIndex frame_id;
          UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame (link_name, frame_id);
          const SE3 & body_placement = frame.placement;

          std::size_t objectId = 0;
          for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
          {
            meshPath.clear();
#ifdef PINOCCHIO_WITH_HPP_FCL

#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
            const std::string & geom_name = (*i)->group_name;
#else
            const std::string & geom_name = (*i)->name;
#endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
            const GeometryObject::CollisionGeometryPtr geometry =
            retrieveCollisionGeometry(tree, meshLoader, link_name, geom_name,
                                      (*i)->geometry, package_dirs, meshPath, meshScale);
#else
            ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry);
            if (urdf_mesh)
            {
              meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs);
              retrieveMeshScale(urdf_mesh, meshScale);
            }

            const shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
#endif // PINOCCHIO_WITH_HPP_FCL

            Eigen::Vector4d meshColor;
            std::string meshTexturePath;
            bool overrideMaterial = getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs);

            const SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
            std::ostringstream geometry_object_suffix;
            geometry_object_suffix << "_" << objectId;
            const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
            GeometryObject geometry_object(geometry_object_name,
                                           frame_id, frame.parent,
                                           geometry,
                                           geomPlacement, meshPath, meshScale,
                                           overrideMaterial, meshColor, meshTexturePath);
            geomModel.addGeometryObject(geometry_object);
            ++objectId;
          }
        }
      }

      /**
       * @brief      Recursive procedure for reading the URDF tree, looking for geometries
       *             This function fill the geometric model with geometry objects retrieved from the URDF tree
       *
       * @param[in]  tree           The URDF kinematic tree
       * @param[in]  meshLoader     The FCL mesh loader to avoid duplications of already loaded geometries
       * @param[in]  link           The current URDF link
       * @param      model          The model to which is the GeometryModel associated
       * @param      geomModel      The GeometryModel where the Collision Objects must be added
       * @param[in]  package_dirs   A vector containing the different directories where to search for packages
       * @param[in]  type           The type of objects that must be loaded ( can be VISUAL or COLLISION)
       *
       */
      void recursiveParseTreeForGeom(const UrdfTree& tree,
                                     ::hpp::fcl::MeshLoaderPtr& meshLoader,
                                     ::urdf::LinkConstSharedPtr link,
                                     UrdfGeomVisitorBase& visitor,
                                     GeometryModel & geomModel,
                                     const std::vector<std::string> & package_dirs,
                                     const GeometryType type)
      {

        switch(type)
        {
          case COLLISION:
            addLinkGeometryToGeomModel< ::urdf::Collision >(tree, meshLoader, link, visitor, geomModel, package_dirs);
            break;
          case VISUAL:
            addLinkGeometryToGeomModel< ::urdf::Visual >(tree, meshLoader, link, visitor, geomModel, package_dirs);
            break;
          default:
            break;
        }

        BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links)
        {
          recursiveParseTreeForGeom(tree, meshLoader, child, visitor, geomModel, package_dirs,type);
        }

      }

      void parseTreeForGeom(UrdfGeomVisitorBase& visitor,
                            const std::istream& xmlStream,
                            const GeometryType type,
                            GeometryModel & geomModel,
                            const std::vector<std::string> & package_dirs,
                            ::hpp::fcl::MeshLoaderPtr meshLoader)
      {
        std::string xmlStr;
        {
          std::ostringstream os;
          os << xmlStream.rdbuf();
          xmlStr = os.str();
        }

        details::UrdfTree tree;
        tree.parse (xmlStr);

        std::vector<std::string> hint_directories(package_dirs);

        // Append the ROS_PACKAGE_PATH
        std::vector<std::string> ros_pkg_paths = rosPaths();
        hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());

#ifdef PINOCCHIO_WITH_HPP_FCL
        if (!meshLoader) meshLoader = fcl::MeshLoaderPtr(new fcl::MeshLoader);
#endif // ifdef PINOCCHIO_WITH_HPP_FCL

        recursiveParseTreeForGeom(tree, meshLoader, tree.urdf_->getRoot(),
            visitor, geomModel, hint_directories,type);
      }
    } // namespace details
    /// \endinternal
  } // namespace urdf
} // namespace pinocchio


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/parsers/urdf/types.hpp"
7			#include "pinocchio/parsers/urdf/utils.hpp"
8			#include "pinocchio/parsers/utils.hpp"
9
10			#include <boost/property_tree/xml_parser.hpp>
11			#include <boost/property_tree/ptree.hpp>
12
13			#include <urdf_model/model.h>
14			#include <urdf_parser/urdf_parser.h>
15
16			namespace pinocchio
17			{
18			namespace urdf
19			{
20			/// \internal
21			namespace details
22			{
23			struct UrdfTree
24			{
25			typedef boost::property_tree::ptree ptree;
26			typedef std::map<std::string, const ptree&> LinkMap_t;
27
28		41	void parse (const std::string & xmlStr)
29			{
30	✓✗	41	urdf_ = ::urdf::parseURDF(xmlStr);
31	✗✓	41	if (!urdf_) {
32			throw std::invalid_argument("Unable to parse URDF");
33			}
34
35	✓✗	82	std::istringstream iss(xmlStr);
36			using namespace boost::property_tree;
37	✓✗	41	read_xml(iss, tree_, xml_parser::no_comments);
38
39	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗	8071	BOOST_FOREACH(const ptree::value_type & link, tree_.get_child("robot")) {
40	✓✓	4015	if (link.first == "link") {
41	✓✗✓✗	1570	std::string name = link.second.get<std::string>("<xmlattr>.name");
42	✓✗✓✗	1570	links_.insert(std::pair<std::string,const ptree&>(name, link.second));
43			}
44			} // BOOST_FOREACH
45		41	}
46
47		23	bool isCapsule(const std::string & linkName,
48			const std::string & geomName) const
49			{
50	✓✗	23	LinkMap_t::const_iterator _link = links_.find(linkName);
51	✗✓	23	assert (_link != links_.end());
52		23	const ptree& link = _link->second;
53	✓✗✓✗ ✓✓	23	if (link.count ("collision_checking") == 0)
54		20	return false;
55	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗	3	BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
56	✓✗	3	if (cc.first == "capsule") {
57			#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
58			std::cerr << "Warning: support for tag link/collision_checking/capsule"
59			" is not available for URDFDOM < 0.3.0" << std::endl;
60			#else
61	✓✗✓✗	3	std::string name = cc.second.get<std::string>("<xmlattr>.name");
62	✓✗	3	if (geomName == name) return true;
63			#endif
64			}
65			} // BOOST_FOREACH
66
67			return false;
68			}
69
70		730	bool isMeshConvex(const std::string & linkName,
71			const std::string & geomName) const
72			{
73	✓✗	730	LinkMap_t::const_iterator _link = links_.find(linkName);
74	✗✓	730	assert (_link != links_.end());
75		730	const ptree& link = _link->second;
76	✓✗✓✗ ✓✓	730	if (link.count ("collision_checking") == 0)
77		727	return false;
78	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✗ ✓✗	9	BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
79	✓✓	6	if (cc.first == "convex") {
80			#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
81			std::cerr << "Warning: support for tag link/collision_checking/convex"
82			" is not available for URDFDOM < 0.3.0" << std::endl;
83			#else
84	✓✗✓✗	3	std::string name = cc.second.get<std::string>("<xmlattr>.name");
85	✓✗	3	if (geomName == name) return true;
86			#endif
87			}
88			} // BOOST_FOREACH
89
90			return false;
91			}
92
93			// For standard URDF tags
94			::urdf::ModelInterfaceSharedPtr urdf_;
95			// For other tags
96			ptree tree_;
97			// A mapping from link name to corresponding child of tree_
98			LinkMap_t links_;
99			};
100
101			template<typename Vector3>
102		730	static void retrieveMeshScale(const ::urdf::MeshSharedPtr & mesh,
103			const Eigen::MatrixBase<Vector3> & scale)
104			{
105		730	Vector3 & scale_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,scale);
106			scale_ <<
107		1460	mesh->scale.x,
108	✓✗	730	mesh->scale.y,
109	✓✗	730	mesh->scale.z;
110		730	}
111
112			#ifdef PINOCCHIO_WITH_HPP_FCL
113			# if ( HPP_FCL_MAJOR_VERSION>1 \|\| ( HPP_FCL_MAJOR_VERSION==1 && \
114			( HPP_FCL_MINOR_VERSION>1 \|\| ( HPP_FCL_MINOR_VERSION==1 && \
115			HPP_FCL_PATCH_VERSION>3))))
116			# define PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
117			# endif
118
119			/**
120			* @brief Get a fcl::CollisionObject from a URDF geometry, searching
121			* for it in specified package directories
122			*
123			* @param[in] urdf_geometry A shared pointer on the input urdf Geometry
124			* @param[in] package_dirs A vector containing the different directories where to search for packages
125			* @param[out] meshPath The Absolute path of the mesh currently read
126			* @param[out] meshScale Scale of transformation currently applied to the mesh
127			*
128			* @return A shared pointer on the geometry converted as a fcl::CollisionGeometry
129			*/
130			shared_ptr<fcl::CollisionGeometry>
131		763	inline retrieveCollisionGeometry(const UrdfTree& tree,
132			fcl::MeshLoaderPtr& meshLoader,
133			const std::string& linkName,
134			const std::string& geomName,
135			const ::urdf::GeometrySharedPtr urdf_geometry,
136			const std::vector<std::string> & package_dirs,
137			std::string & meshPath,
138			Eigen::Vector3d & meshScale)
139			{
140		763	shared_ptr<fcl::CollisionGeometry> geometry;
141
142			// Handle the case where collision geometry is a mesh
143	✓✓	763	if (urdf_geometry->type == ::urdf::Geometry::MESH)
144			{
145		1460	const ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
146	✓✗	1460	std::string collisionFilename = urdf_mesh->filename;
147
148	✓✗	730	meshPath = retrieveResourcePath(collisionFilename, package_dirs);
149	✗✓	730	if (meshPath == "") {
150			std::stringstream ss;
151			ss << "Mesh " << collisionFilename << " could not be found.";
152			throw std::invalid_argument (ss.str());
153			}
154
155		1460	fcl::Vec3f scale = fcl::Vec3f(urdf_mesh->scale.x,
156		730	urdf_mesh->scale.y,
157		730	urdf_mesh->scale.z
158	✓✗	730	);
159
160	✓✗	730	retrieveMeshScale(urdf_mesh, meshScale);
161
162			// Create FCL mesh by parsing Collada file.
163			#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
164	✓✗	1460	hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale);
165	✓✗	730	bool convex = tree.isMeshConvex (linkName, geomName);
166	✓✓	730	if (convex) {
167	✓✗	3	bvh->buildConvexRepresentation (false);
168			#if HPP_FCL_MAJOR_VERSION < 2
169			geometry = bvh->convex;
170			#else
171	✓✗	6	geometry = shared_ptr<fcl::CollisionGeometry>(bvh->convex.get(), [bvh](...) mutable { bvh->convex.reset(); });
172			#endif // HPP_FCL_MAJOR_VERSION
173			} else {
174			#if HPP_FCL_MAJOR_VERSION < 2
175			geometry = bvh;
176			#else
177	✓✗	1069	geometry = shared_ptr<fcl::CollisionGeometry>(bvh.get(), [bvh](...) mutable { bvh.reset(); });
178			#endif // HPP_FCL_MAJOR_VERSION
179			}
180			#else
181			geometry = meshLoader->load (meshPath, scale);
182			#endif
183			}
184
185			// Handle the case where collision geometry is a cylinder
186			// Use FCL capsules for cylinders
187	✓✓	33	else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
188			{
189	✓✗	23	const bool is_capsule = tree.isCapsule(linkName, geomName);
190	✓✗✓✗ ✓✗	23	meshScale << 1,1,1;
191		46	const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
192
193		23	double radius = collisionGeometry->radius;
194		23	double length = collisionGeometry->length;
195
196			// Create fcl capsule geometry.
197	✓✓	23	if (is_capsule) {
198	✓✗	3	meshPath = "CAPSULE";
199	✓✗✓✗ ✓✗	3	geometry = shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length));
200			} else {
201	✓✗	20	meshPath = "CYLINDER";
202	✓✗✓✗ ✓✗	20	geometry = shared_ptr < fcl::CollisionGeometry >(new fcl::Cylinder (radius, length));
203			}
204			}
205			// Handle the case where collision geometry is a box.
206	✓✗	10	else if (urdf_geometry->type == ::urdf::Geometry::BOX)
207			{
208	✓✗	10	meshPath = "BOX";
209	✓✗✓✗ ✓✗	10	meshScale << 1,1,1;
210		10	const ::urdf::BoxSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
211
212		10	double x = collisionGeometry->dim.x;
213		10	double y = collisionGeometry->dim.y;
214		10	double z = collisionGeometry->dim.z;
215
216	✓✗✓✗ ✓✗	10	geometry = shared_ptr < fcl::CollisionGeometry > (new fcl::Box (x, y, z));
217			}
218			// Handle the case where collision geometry is a sphere.
219			else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
220			{
221			meshPath = "SPHERE";
222			meshScale << 1,1,1;
223			const ::urdf::SphereSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
224
225			double radius = collisionGeometry->radius;
226
227			geometry = shared_ptr < fcl::CollisionGeometry > (new fcl::Sphere (radius));
228			}
229			else throw std::invalid_argument("Unknown geometry type :");
230
231	✗✓	763	if (!geometry)
232			{
233			throw std::invalid_argument("The polyhedron retrieved is empty");
234			}
235
236		763	return geometry;
237			}
238			#endif // PINOCCHIO_WITH_HPP_FCL
239
240			/**
241			* @brief Get the first geometry attached to a link
242			*
243			* @param[in] link The URDF link
244			*
245			* @return Either the first collision or visual
246			*/
247			template<typename T>
248			inline PINOCCHIO_URDF_SHARED_PTR(const T)
249			getLinkGeometry(const ::urdf::LinkConstSharedPtr link);
250
251			template<>
252			inline ::urdf::CollisionConstSharedPtr
253		1237	getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
254			{
255		1237	return link->collision;
256			}
257
258			template<>
259			inline ::urdf::VisualConstSharedPtr
260		333	getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
261			{
262		333	return link->visual;
263			}
264
265
266			/**
267			* @brief Get the material values from the link visual object
268			*
269			* @param[in] Visual/Collision The Visual or the Collision object.
270			* @param[out] meshTexturePath The absolute file path containing the texture description.
271			* @param[out] meshColor The mesh RGBA vector.
272			* @param[in] package_dirs A vector containing the different directories where to search for packages
273			*
274			*/
275			template<typename urdfObject>
276			inline bool getVisualMaterial(const PINOCCHIO_URDF_SHARED_PTR(urdfObject) urdf_object,std::string & meshTexturePath,
277			Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs);
278
279			template<>
280		543	inline bool getVisualMaterial< ::urdf::Collision>
281			(const ::urdf::CollisionSharedPtr, std::string& meshTexturePath,
282			Eigen::Vector4d & meshColor, const std::vector<std::string> &)
283			{
284	✓✗✓✗ ✓✗✓✗	543	meshColor << 0.9, 0.9, 0.9, 1.;
285		543	meshTexturePath = "";
286		543	return false;
287			}
288
289			template<>
290		220	inline bool getVisualMaterial< ::urdf::Visual>
291			(const ::urdf::VisualSharedPtr urdf_visual, std::string& meshTexturePath,
292			Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs)
293			{
294	✓✗✓✗ ✓✗✓✗	220	meshColor << 0.9, 0.9, 0.9, 1.;
295		220	meshTexturePath = "";
296		220	bool overrideMaterial = false;
297	✓✓	220	if(urdf_visual->material) {
298		167	overrideMaterial = true;
299	✓✗✓✗	167	meshColor << urdf_visual->material->color.r, urdf_visual->material->color.g,
300	✓✗✓✗	167	urdf_visual->material->color.b, urdf_visual->material->color.a;
301	✗✓	167	if(urdf_visual->material->texture_filename!="")
302			meshTexturePath = retrieveResourcePath((urdf_visual)->material->texture_filename, package_dirs);
303			}
304		220	return overrideMaterial;
305			}
306
307			/**
308			* @brief Get the array of geometries attached to a link
309			*
310			* @param[in] link The URDF link
311			*
312			* @return the array of either collisions or visuals
313			*/
314			template<typename T>
315			inline const std::vector< PINOCCHIO_URDF_SHARED_PTR(T) > &
316			getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link);
317
318			template<>
319			inline const std::vector< ::urdf::CollisionSharedPtr> &
320		540	getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
321			{
322		540	return link->collision_array;
323			}
324
325			template<>
326			inline const std::vector< ::urdf::VisualSharedPtr> &
327		220	getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
328			{
329		220	return link->visual_array;
330			}
331
332			/**
333			* @brief Add the geometries attached to a URDF link to a GeometryModel, looking
334			* either for collisions or visuals
335			*
336			* @param[in] tree The URDF kinematic tree
337			* @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded geometries
338			* @param[in] link The current URDF link
339			* @param model The model to which is the GeometryModel associated
340			* @param geomModel The GeometryModel where the Collision Objects must be added
341			* @param[in] package_dirs A vector containing the different directories where to search for packages
342			*
343			*/
344			template<typename GeometryType>
345		3140	static void addLinkGeometryToGeomModel(const UrdfTree & tree,
346			::hpp::fcl::MeshLoaderPtr & meshLoader,
347			::urdf::LinkConstSharedPtr link,
348			UrdfGeomVisitorBase& visitor,
349			GeometryModel & geomModel,
350			const std::vector<std::string> & package_dirs)
351			{
352			#ifndef PINOCCHIO_WITH_HPP_FCL
353			PINOCCHIO_UNUSED_VARIABLE(tree);
354			PINOCCHIO_UNUSED_VARIABLE(meshLoader);
355			#endif // PINOCCHIO_WITH_HPP_FCL
356
357			typedef std::vector< PINOCCHIO_URDF_SHARED_PTR(GeometryType) > VectorSharedT;
358			typedef GeometryModel::SE3 SE3;
359
360	✓✓	3140	if(getLinkGeometry<GeometryType>(link))
361			{
362	✓✗	3040	std::string meshPath = "";
363
364	✓✗✓✗	1520	Eigen::Vector3d meshScale(Eigen::Vector3d::Ones());
365
366		1520	const std::string & link_name = link->name;
367
368	✓✗	3040	VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link);
369
370			FrameIndex frame_id;
371	✓✗	3040	UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame (link_name, frame_id);
372		1520	const SE3 & body_placement = frame.placement;
373
374		1520	std::size_t objectId = 0;
375	✓✓	3046	for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
376			{
377		1526	meshPath.clear();
378			#ifdef PINOCCHIO_WITH_HPP_FCL
379
380			#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
381			const std::string & geom_name = (*i)->group_name;
382			#else
383		1526	const std::string & geom_name = (*i)->name;
384			#endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
385	✓✗	3052	const GeometryObject::CollisionGeometryPtr geometry =
386			retrieveCollisionGeometry(tree, meshLoader, link_name, geom_name,
387		1526	(*i)->geometry, package_dirs, meshPath, meshScale);
388			#else
389			::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry);
390			if (urdf_mesh)
391			{
392			meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs);
393			retrieveMeshScale(urdf_mesh, meshScale);
394			}
395
396			const shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
397			#endif // PINOCCHIO_WITH_HPP_FCL
398
399	✓✗	1526	Eigen::Vector4d meshColor;
400		3052	std::string meshTexturePath;
401	✓✗	1526	bool overrideMaterial = getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs);
402
403	✓✗✓✗	1526	const SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
404	✓✗	3052	std::ostringstream geometry_object_suffix;
405	✓✗✓✗	1526	geometry_object_suffix << "_" << objectId;
406	✓✗✓✗	3052	const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
407	✓✗	1526	GeometryObject geometry_object(geometry_object_name,
408			frame_id, frame.parent,
409			geometry,
410			geomPlacement, meshPath, meshScale,
411			overrideMaterial, meshColor, meshTexturePath);
412	✓✗	1526	geomModel.addGeometryObject(geometry_object);
413		1526	++objectId;
414			}
415			}
416		3140	}
417
418			/**
419			* @brief Recursive procedure for reading the URDF tree, looking for geometries
420			* This function fill the geometric model with geometry objects retrieved from the URDF tree
421			*
422			* @param[in] tree The URDF kinematic tree
423			* @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded geometries
424			* @param[in] link The current URDF link
425			* @param model The model to which is the GeometryModel associated
426			* @param geomModel The GeometryModel where the Collision Objects must be added
427			* @param[in] package_dirs A vector containing the different directories where to search for packages
428			* @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION)
429			*
430			*/
431		1570	void recursiveParseTreeForGeom(const UrdfTree& tree,
432			::hpp::fcl::MeshLoaderPtr& meshLoader,
433			::urdf::LinkConstSharedPtr link,
434			UrdfGeomVisitorBase& visitor,
435			GeometryModel & geomModel,
436			const std::vector<std::string> & package_dirs,
437			const GeometryType type)
438			{
439
440	✓✓✗	1570	switch(type)
441			{
442		1237	case COLLISION:
443	✓✗	1237	addLinkGeometryToGeomModel< ::urdf::Collision >(tree, meshLoader, link, visitor, geomModel, package_dirs);
444		1237	break;
445		333	case VISUAL:
446	✓✗	333	addLinkGeometryToGeomModel< ::urdf::Visual >(tree, meshLoader, link, visitor, geomModel, package_dirs);
447		333	break;
448			default:
449			break;
450			}
451
452	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗	4628	BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links)
453			{
454	✓✗	1529	recursiveParseTreeForGeom(tree, meshLoader, child, visitor, geomModel, package_dirs,type);
455			}
456
457		1570	}
458
459		41	void parseTreeForGeom(UrdfGeomVisitorBase& visitor,
460			const std::istream& xmlStream,
461			const GeometryType type,
462			GeometryModel & geomModel,
463			const std::vector<std::string> & package_dirs,
464			::hpp::fcl::MeshLoaderPtr meshLoader)
465			{
466		82	std::string xmlStr;
467			{
468	✓✗	41	std::ostringstream os;
469	✓✗✓✗	41	os << xmlStream.rdbuf();
470	✓✗	41	xmlStr = os.str();
471			}
472
473	✓✗	82	details::UrdfTree tree;
474	✓✗	41	tree.parse (xmlStr);
475
476	✓✗	82	std::vector<std::string> hint_directories(package_dirs);
477
478			// Append the ROS_PACKAGE_PATH
479	✓✗	41	std::vector<std::string> ros_pkg_paths = rosPaths();
480	✓✗	41	hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
481
482			#ifdef PINOCCHIO_WITH_HPP_FCL
483	✓✗✓✗ ✓✗	41	if (!meshLoader) meshLoader = fcl::MeshLoaderPtr(new fcl::MeshLoader);
484			#endif // ifdef PINOCCHIO_WITH_HPP_FCL
485
486	✓✗	41	recursiveParseTreeForGeom(tree, meshLoader, tree.urdf_->getRoot(),
487			visitor, geomModel, hint_directories,type);
488		41	}
489			} // namespace details
490			/// \endinternal
491			} // namespace urdf
492			} // namespace pinocchio