Program Listing for File geometry.hxx

↰ Return to documentation for file (include/pinocchio/parsers/urdf/geometry.hxx)

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

#ifndef __pinocchio_multibody_parsers_urdf_geometry_hxx__
#define __pinocchio_multibody_parsers_urdf_geometry_hxx__

#include "pinocchio/parsers/urdf.hpp"

#include <iostream>
#include <sstream>
#include <iomanip>

#ifdef PINOCCHIO_WITH_HPP_FCL
  #include <hpp/fcl/mesh_loader/loader.h>
  #include <hpp/fcl/mesh_loader/assimp.h>
#endif // PINOCCHIO_WITH_HPP_FCL

namespace pinocchio
{
  namespace urdf
  {
    namespace details
    {
      struct UrdfGeomVisitorBase
      {
        typedef FrameTpl<urdf_scalar_type, 0> Frame;

        virtual Frame getBodyFrame (const std::string& name, FrameIndex& fid) const = 0;
      };

      template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
      struct UrdfGeomVisitor : UrdfGeomVisitorBase
      {
        typedef ModelTpl<_Scalar,_Options,JointCollectionTpl> Model;
        const Model& model;

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

        Frame getBodyFrame (const std::string& link_name, FrameIndex& fid) const
        {
          if (!model.existFrame(link_name, BODY))
          {
            throw std::invalid_argument("No link " + link_name + " in model");
          }
          fid = model.getFrameId(link_name, BODY);
          PINOCCHIO_CHECK_INPUT_ARGUMENT(model.frames[fid].type == BODY);
          return model.frames[fid].template cast<urdf_scalar_type>();
        }
      };

      PINOCCHIO_DLLAPI void parseTreeForGeom(UrdfGeomVisitorBase& visitor,
                                             const std::istream& xmlStream,
                                             const GeometryType type,
                                             GeometryModel & geomModel,
                                             const std::vector<std::string> & package_dirs,
                                             ::hpp::fcl::MeshLoaderPtr meshLoader);

      } // 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,
                               const std::vector<std::string> & package_dirs,
                               ::hpp::fcl::MeshLoaderPtr meshLoader)
      {
        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);
        }
        return buildGeom (model, xmlStream, type, geomModel, package_dirs, meshLoader);
      }

      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,
                               const std::vector<std::string> & package_dirs,
                               ::hpp::fcl::MeshLoaderPtr meshLoader)
      {
        details::UrdfGeomVisitor<Scalar, Options, JointCollectionTpl> visitor (model);
        details::parseTreeForGeom (visitor, xmlStream, type, geomModel,
            package_dirs, meshLoader);
        return geomModel;
      }

  } // namespace urdf
} // namespace pinocchio

#endif // ifndef __pinocchio_multibody_parsers_urdf_geometry_hxx__