Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/parsers/urdf/geometry.cpp	Lines:	146	163	89.6 %
Date:	2024-01-23 21:41:47	Branches:	198	397	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
      /**
       * @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.
          hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale);
          bool convex = tree.isMeshConvex (linkName, geomName);
          if (convex) {
            bvh->buildConvexRepresentation (false);
            geometry = shared_ptr<fcl::CollisionGeometry>(bvh->convex.get(), [bvh](...) mutable { bvh->convex.reset(); });
          } else {
            geometry = shared_ptr<fcl::CollisionGeometry>(bvh.get(), [bvh](...) mutable { bvh.reset(); });
          }
        }

        // 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			/**
114			* @brief Get a fcl::CollisionObject from a URDF geometry, searching
115			* for it in specified package directories
116			*
117			* @param[in] urdf_geometry A shared pointer on the input urdf Geometry
118			* @param[in] package_dirs A vector containing the different directories where to search for packages
119			* @param[out] meshPath The Absolute path of the mesh currently read
120			* @param[out] meshScale Scale of transformation currently applied to the mesh
121			*
122			* @return A shared pointer on the geometry converted as a fcl::CollisionGeometry
123			*/
124			shared_ptr<fcl::CollisionGeometry>
125		763	inline retrieveCollisionGeometry(const UrdfTree& tree,
126			fcl::MeshLoaderPtr& meshLoader,
127			const std::string& linkName,
128			const std::string& geomName,
129			const ::urdf::GeometrySharedPtr urdf_geometry,
130			const std::vector<std::string> & package_dirs,
131			std::string & meshPath,
132			Eigen::Vector3d & meshScale)
133			{
134		763	shared_ptr<fcl::CollisionGeometry> geometry;
135
136			// Handle the case where collision geometry is a mesh
137	✓✓	763	if (urdf_geometry->type == ::urdf::Geometry::MESH)
138			{
139		1460	const ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
140	✓✗	1460	std::string collisionFilename = urdf_mesh->filename;
141
142	✓✗	730	meshPath = retrieveResourcePath(collisionFilename, package_dirs);
143	✗✓	730	if (meshPath == "") {
144			std::stringstream ss;
145			ss << "Mesh " << collisionFilename << " could not be found.";
146			throw std::invalid_argument (ss.str());
147			}
148
149		1460	fcl::Vec3f scale = fcl::Vec3f(urdf_mesh->scale.x,
150		730	urdf_mesh->scale.y,
151		730	urdf_mesh->scale.z
152	✓✗	730	);
153
154	✓✗	730	retrieveMeshScale(urdf_mesh, meshScale);
155
156			// Create FCL mesh by parsing Collada file.
157	✓✗	1460	hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale);
158	✓✗	730	bool convex = tree.isMeshConvex (linkName, geomName);
159	✓✓	730	if (convex) {
160	✓✗	3	bvh->buildConvexRepresentation (false);
161	✓✗	6	geometry = shared_ptr<fcl::CollisionGeometry>(bvh->convex.get(), [bvh](...) mutable { bvh->convex.reset(); });
162			} else {
163	✓✗	1434	geometry = shared_ptr<fcl::CollisionGeometry>(bvh.get(), [bvh](...) mutable { bvh.reset(); });
164			}
165			}
166
167			// Handle the case where collision geometry is a cylinder
168			// Use FCL capsules for cylinders
169	✓✓	33	else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
170			{
171	✓✗	23	const bool is_capsule = tree.isCapsule(linkName, geomName);
172	✓✗✓✗ ✓✗	23	meshScale << 1,1,1;
173		46	const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
174
175		23	double radius = collisionGeometry->radius;
176		23	double length = collisionGeometry->length;
177
178			// Create fcl capsule geometry.
179	✓✓	23	if (is_capsule) {
180	✓✗	3	meshPath = "CAPSULE";
181	✓✗✓✗ ✓✗	3	geometry = shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length));
182			} else {
183	✓✗	20	meshPath = "CYLINDER";
184	✓✗✓✗ ✓✗	20	geometry = shared_ptr < fcl::CollisionGeometry >(new fcl::Cylinder (radius, length));
185			}
186			}
187			// Handle the case where collision geometry is a box.
188	✓✗	10	else if (urdf_geometry->type == ::urdf::Geometry::BOX)
189			{
190	✓✗	10	meshPath = "BOX";
191	✓✗✓✗ ✓✗	10	meshScale << 1,1,1;
192		10	const ::urdf::BoxSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
193
194		10	double x = collisionGeometry->dim.x;
195		10	double y = collisionGeometry->dim.y;
196		10	double z = collisionGeometry->dim.z;
197
198	✓✗✓✗ ✓✗	10	geometry = shared_ptr < fcl::CollisionGeometry > (new fcl::Box (x, y, z));
199			}
200			// Handle the case where collision geometry is a sphere.
201			else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
202			{
203			meshPath = "SPHERE";
204			meshScale << 1,1,1;
205			const ::urdf::SphereSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
206
207			double radius = collisionGeometry->radius;
208
209			geometry = shared_ptr < fcl::CollisionGeometry > (new fcl::Sphere (radius));
210			}
211			else throw std::invalid_argument("Unknown geometry type :");
212
213	✗✓	763	if (!geometry)
214			{
215			throw std::invalid_argument("The polyhedron retrieved is empty");
216			}
217
218		763	return geometry;
219			}
220			#endif // PINOCCHIO_WITH_HPP_FCL
221
222			/**
223			* @brief Get the first geometry attached to a link
224			*
225			* @param[in] link The URDF link
226			*
227			* @return Either the first collision or visual
228			*/
229			template<typename T>
230			inline PINOCCHIO_URDF_SHARED_PTR(const T)
231			getLinkGeometry(const ::urdf::LinkConstSharedPtr link);
232
233			template<>
234			inline ::urdf::CollisionConstSharedPtr
235		1237	getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
236			{
237		1237	return link->collision;
238			}
239
240			template<>
241			inline ::urdf::VisualConstSharedPtr
242		333	getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
243			{
244		333	return link->visual;
245			}
246
247
248			/**
249			* @brief Get the material values from the link visual object
250			*
251			* @param[in] Visual/Collision The Visual or the Collision object.
252			* @param[out] meshTexturePath The absolute file path containing the texture description.
253			* @param[out] meshColor The mesh RGBA vector.
254			* @param[in] package_dirs A vector containing the different directories where to search for packages
255			*
256			*/
257			template<typename urdfObject>
258			inline bool getVisualMaterial(const PINOCCHIO_URDF_SHARED_PTR(urdfObject) urdf_object,std::string & meshTexturePath,
259			Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs);
260
261			template<>
262		543	inline bool getVisualMaterial< ::urdf::Collision>
263			(const ::urdf::CollisionSharedPtr, std::string& meshTexturePath,
264			Eigen::Vector4d & meshColor, const std::vector<std::string> &)
265			{
266	✓✗✓✗ ✓✗✓✗	543	meshColor << 0.9, 0.9, 0.9, 1.;
267		543	meshTexturePath = "";
268		543	return false;
269			}
270
271			template<>
272		220	inline bool getVisualMaterial< ::urdf::Visual>
273			(const ::urdf::VisualSharedPtr urdf_visual, std::string& meshTexturePath,
274			Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs)
275			{
276	✓✗✓✗ ✓✗✓✗	220	meshColor << 0.9, 0.9, 0.9, 1.;
277		220	meshTexturePath = "";
278		220	bool overrideMaterial = false;
279	✓✓	220	if(urdf_visual->material) {
280		167	overrideMaterial = true;
281	✓✗✓✗	167	meshColor << urdf_visual->material->color.r, urdf_visual->material->color.g,
282	✓✗✓✗	167	urdf_visual->material->color.b, urdf_visual->material->color.a;
283	✗✓	167	if(urdf_visual->material->texture_filename!="")
284			meshTexturePath = retrieveResourcePath((urdf_visual)->material->texture_filename, package_dirs);
285			}
286		220	return overrideMaterial;
287			}
288
289			/**
290			* @brief Get the array of geometries attached to a link
291			*
292			* @param[in] link The URDF link
293			*
294			* @return the array of either collisions or visuals
295			*/
296			template<typename T>
297			inline const std::vector< PINOCCHIO_URDF_SHARED_PTR(T) > &
298			getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link);
299
300			template<>
301			inline const std::vector< ::urdf::CollisionSharedPtr> &
302		540	getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
303			{
304		540	return link->collision_array;
305			}
306
307			template<>
308			inline const std::vector< ::urdf::VisualSharedPtr> &
309		220	getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
310			{
311		220	return link->visual_array;
312			}
313
314			/**
315			* @brief Add the geometries attached to a URDF link to a GeometryModel, looking
316			* either for collisions or visuals
317			*
318			* @param[in] tree The URDF kinematic tree
319			* @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded geometries
320			* @param[in] link The current URDF link
321			* @param model The model to which is the GeometryModel associated
322			* @param geomModel The GeometryModel where the Collision Objects must be added
323			* @param[in] package_dirs A vector containing the different directories where to search for packages
324			*
325			*/
326			template<typename GeometryType>
327		3140	static void addLinkGeometryToGeomModel(const UrdfTree & tree,
328			::hpp::fcl::MeshLoaderPtr & meshLoader,
329			::urdf::LinkConstSharedPtr link,
330			UrdfGeomVisitorBase& visitor,
331			GeometryModel & geomModel,
332			const std::vector<std::string> & package_dirs)
333			{
334			#ifndef PINOCCHIO_WITH_HPP_FCL
335			PINOCCHIO_UNUSED_VARIABLE(tree);
336			PINOCCHIO_UNUSED_VARIABLE(meshLoader);
337			#endif // PINOCCHIO_WITH_HPP_FCL
338
339			typedef std::vector< PINOCCHIO_URDF_SHARED_PTR(GeometryType) > VectorSharedT;
340			typedef GeometryModel::SE3 SE3;
341
342	✓✓	3140	if(getLinkGeometry<GeometryType>(link))
343			{
344	✓✗	3040	std::string meshPath = "";
345
346	✓✗✓✗	1520	Eigen::Vector3d meshScale(Eigen::Vector3d::Ones());
347
348		1520	const std::string & link_name = link->name;
349
350	✓✗	3040	VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link);
351
352			FrameIndex frame_id;
353	✓✗	3040	UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame (link_name, frame_id);
354		1520	const SE3 & body_placement = frame.placement;
355
356		1520	std::size_t objectId = 0;
357	✓✓	3046	for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
358			{
359		1526	meshPath.clear();
360			#ifdef PINOCCHIO_WITH_HPP_FCL
361
362			#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
363			const std::string & geom_name = (*i)->group_name;
364			#else
365		1526	const std::string & geom_name = (*i)->name;
366			#endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
367	✓✗	3052	const GeometryObject::CollisionGeometryPtr geometry =
368			retrieveCollisionGeometry(tree, meshLoader, link_name, geom_name,
369		1526	(*i)->geometry, package_dirs, meshPath, meshScale);
370			#else
371			::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry);
372			if (urdf_mesh)
373			{
374			meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs);
375			retrieveMeshScale(urdf_mesh, meshScale);
376			}
377
378			const shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
379			#endif // PINOCCHIO_WITH_HPP_FCL
380
381	✓✗	1526	Eigen::Vector4d meshColor;
382		3052	std::string meshTexturePath;
383	✓✗	1526	bool overrideMaterial = getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs);
384
385	✓✗✓✗	1526	const SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
386	✓✗	3052	std::ostringstream geometry_object_suffix;
387	✓✗✓✗	1526	geometry_object_suffix << "_" << objectId;
388	✓✗✓✗	3052	const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
389	✓✗✓✗	1526	GeometryObject geometry_object(geometry_object_name,
390			frame_id, frame.parent,
391			geometry,
392			geomPlacement, meshPath, meshScale,
393			overrideMaterial, meshColor, meshTexturePath);
394	✓✗	1526	geomModel.addGeometryObject(geometry_object);
395		1526	++objectId;
396			}
397			}
398		3140	}
399
400			/**
401			* @brief Recursive procedure for reading the URDF tree, looking for geometries
402			* This function fill the geometric model with geometry objects retrieved from the URDF tree
403			*
404			* @param[in] tree The URDF kinematic tree
405			* @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded geometries
406			* @param[in] link The current URDF link
407			* @param model The model to which is the GeometryModel associated
408			* @param geomModel The GeometryModel where the Collision Objects must be added
409			* @param[in] package_dirs A vector containing the different directories where to search for packages
410			* @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION)
411			*
412			*/
413		1570	void recursiveParseTreeForGeom(const UrdfTree& tree,
414			::hpp::fcl::MeshLoaderPtr& meshLoader,
415			::urdf::LinkConstSharedPtr link,
416			UrdfGeomVisitorBase& visitor,
417			GeometryModel & geomModel,
418			const std::vector<std::string> & package_dirs,
419			const GeometryType type)
420			{
421
422	✓✓✗	1570	switch(type)
423			{
424		1237	case COLLISION:
425	✓✗	1237	addLinkGeometryToGeomModel< ::urdf::Collision >(tree, meshLoader, link, visitor, geomModel, package_dirs);
426		1237	break;
427		333	case VISUAL:
428	✓✗	333	addLinkGeometryToGeomModel< ::urdf::Visual >(tree, meshLoader, link, visitor, geomModel, package_dirs);
429		333	break;
430			default:
431			break;
432			}
433
434	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗	4628	BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links)
435			{
436	✓✗	1529	recursiveParseTreeForGeom(tree, meshLoader, child, visitor, geomModel, package_dirs,type);
437			}
438
439		1570	}
440
441		41	void parseTreeForGeom(UrdfGeomVisitorBase& visitor,
442			const std::istream& xmlStream,
443			const GeometryType type,
444			GeometryModel & geomModel,
445			const std::vector<std::string> & package_dirs,
446			::hpp::fcl::MeshLoaderPtr meshLoader)
447			{
448		82	std::string xmlStr;
449			{
450	✓✗	41	std::ostringstream os;
451	✓✗✓✗	41	os << xmlStream.rdbuf();
452	✓✗	41	xmlStr = os.str();
453			}
454
455	✓✗	82	details::UrdfTree tree;
456	✓✗	41	tree.parse (xmlStr);
457
458	✓✗	82	std::vector<std::string> hint_directories(package_dirs);
459
460			// Append the ROS_PACKAGE_PATH
461	✓✗	41	std::vector<std::string> ros_pkg_paths = rosPaths();
462	✓✗	41	hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
463
464			#ifdef PINOCCHIO_WITH_HPP_FCL
465	✓✗✓✗ ✓✗	41	if (!meshLoader) meshLoader = fcl::MeshLoaderPtr(new fcl::MeshLoader);
466			#endif // ifdef PINOCCHIO_WITH_HPP_FCL
467
468	✓✗	41	recursiveParseTreeForGeom(tree, meshLoader, tree.urdf_->getRoot(),
469			visitor, geomModel, hint_directories,type);
470		41	}
471			} // namespace details
472			/// \endinternal
473			} // namespace urdf
474			} // namespace pinocchio