geometry.hxx 4.43 KB
Newer Older
1
//
2
// Copyright (c) 2015-2019 CNRS INRIA
3
4
//

5
6
#ifndef __pinocchio_multibody_parsers_urdf_geometry_hxx__
#define __pinocchio_multibody_parsers_urdf_geometry_hxx__
7

8
#include "pinocchio/parsers/urdf.hpp"
9
10
11
12

#include <iostream>
#include <sstream>
#include <iomanip>
13
#include <boost/shared_ptr.hpp>
14

15
#ifdef PINOCCHIO_WITH_HPP_FCL
16
  #include <hpp/fcl/mesh_loader/loader.h>
17
  #include <hpp/fcl/mesh_loader/assimp.h>
18
#endif // PINOCCHIO_WITH_HPP_FCL
19

20
namespace pinocchio
21
22
23
24
25
{
  namespace urdf
  {
    namespace details
    {
26
      struct UrdfGeomVisitorBase
Justin Carpentier's avatar
Justin Carpentier committed
27
      {
Justin Carpentier's avatar
Justin Carpentier committed
28
        typedef FrameTpl<urdf_scalar_type, 0> Frame;
29

30
        virtual Frame getBodyFrame (const std::string& name, FrameIndex& fid) const = 0;
Justin Carpentier's avatar
Justin Carpentier committed
31
      };
32

33
34
      template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
      struct UrdfGeomVisitor : UrdfGeomVisitorBase
35
      {
36
37
        typedef ModelTpl<_Scalar,_Options,JointCollectionTpl> Model;
        const Model& model;
38

39
        UrdfGeomVisitor (const Model& model) : model(model) {}
40

41
        Frame getBodyFrame (const std::string& link_name, FrameIndex& fid) const
42
        {
43
          if (!model.existFrame(link_name, BODY))
44
          {
Joseph Mirabel's avatar
Joseph Mirabel committed
45
            throw std::invalid_argument("No link " + link_name + " in model");
46
          }
47
48
          fid = model.getFrameId(link_name, BODY);
          PINOCCHIO_CHECK_INPUT_ARGUMENT(model.frames[fid].type == BODY);
Justin Carpentier's avatar
Justin Carpentier committed
49
          return model.frames[fid].template cast<urdf_scalar_type>();
50
        }
51
      };
52

53
54
55
56
      /**
       * @brief      Recursive procedure for reading the URDF tree, looking for geometries
       *             This function fill the geometric model whith geometry objects retrieved from the URDF tree
       *
57
58
59
60
       * @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
61
       * @param      geomModel      The GeometryModel where the Collision Objects must be added
62
63
64
       * @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)
       *
65
       */
Francois Keith's avatar
Francois Keith committed
66
      PINOCCHIO_DLLAPI void parseTreeForGeom(UrdfGeomVisitorBase& visitor,
67
68
                            const std::istream& xmlStream,
                            const GeometryType type,
69
70
                            GeometryModel & geomModel,
                            const std::vector<std::string> & package_dirs,
71
                            ::hpp::fcl::MeshLoaderPtr meshLoader);
72
73
74
75
76
77
78
79
80
      
      } // namespace details
      
      
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
      GeometryModel& buildGeom(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                               const std::string & filename,
                               const GeometryType type,
                               GeometryModel & geomModel,
81
                               const std::vector<std::string> & package_dirs,
82
                               ::hpp::fcl::MeshLoaderPtr meshLoader)
83
      {
84
85
86
87
88
89
        std::ifstream xmlStream(filename.c_str());
        if (! xmlStream.is_open())
        {
          const std::string exception_message (filename + " does not seem to be a valid file.");
          throw std::invalid_argument(exception_message);
        }
90
        return buildGeom (model, xmlStream, type, geomModel, package_dirs, meshLoader);
91
      }
92
93
94
95
96
97
      
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
      GeometryModel& buildGeom(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                               const std::istream& xmlStream,
                               const GeometryType type,
                               GeometryModel & geomModel,
98
                               const std::vector<std::string> & package_dirs,
99
                               ::hpp::fcl::MeshLoaderPtr meshLoader)
100
      {
101
102
103
        details::UrdfGeomVisitor<Scalar, Options, JointCollectionTpl> visitor (model);
        details::parseTreeForGeom (visitor, xmlStream, type, geomModel,
            package_dirs, meshLoader);
104
        return geomModel;
105
106
      }

107
  } // namespace urdf
108
} // namespace pinocchio
109
            
110
#endif // ifndef __pinocchio_multibody_parsers_urdf_geometry_hxx__