Skip to content
GitLab
Commit c6a6dec4 authored by jcarpent's avatar jcarpent
Browse files

[C++][CMake] Move URDF parser to the lib

parent 3dab6308
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ c6a6dec4
......@@ -238,13 +238,11 @@ ENDIF(BUILD_PYTHON_INTERFACE)
IF(URDFDOM_FOUND)
LIST(APPEND ${PROJECT_NAME}_PARSERS_HEADERS
parsers/urdf.hpp
parsers/urdf/model.hxx
parsers/urdf/utils.hpp
)
IF(HPP_FCL_FOUND)
LIST(APPEND ${PROJECT_NAME}_PARSERS_HEADERS
parsers/urdf/geometry.hxx
)
ENDIF(HPP_FCL_FOUND)
......
src/CMakeLists.txt
View file @ c6a6dec4
......@@ -57,6 +57,18 @@ IF(BUILD_PYTHON_INTERFACE)
)
ENDIF(BUILD_PYTHON_INTERFACE)
IF(URDFDOM_FOUND)
LIST(APPEND ${PROJECT_NAME}_PARSERS_SOURCES
parsers/urdf/model.cpp
)
IF(HPP_FCL_FOUND)
LIST(APPEND ${PROJECT_NAME}_PARSERS_SOURCES
parsers/urdf/geometry.cpp
)
ENDIF(HPP_FCL_FOUND)
ENDIF(URDFDOM_FOUND)
IF(LUA5_1_FOUND)
LIST(APPEND ${PROJECT_NAME}_PARSERS_SOURCES
parsers/lua/lua_tables.cpp
......
src/parsers/urdf.hpp
View file @ c6a6dec4
......@@ -31,6 +31,7 @@
#include <string>
#include <exception>
#include <boost/shared_ptr.hpp>
namespace urdf
{
......@@ -55,10 +56,9 @@ namespace se3
///
/// \return The se3::Model of the URDF file.
///
template <typename D>
Model buildModel (const std::string & filename,
const JointModelBase<D> & root_joint,
bool verbose = false) throw (std::invalid_argument);
const JointModelVariant & root_joint,
const bool verbose = false) throw (std::invalid_argument);
///
/// \brief Build the model from a URDF file with a fixed joint as root of the model tree.
......@@ -68,8 +68,8 @@ namespace se3
///
/// \return The se3::Model of the URDF file.
///
inline Model buildModel (const std::string & filename,
const bool verbose = false) throw (std::invalid_argument);
Model buildModel (const std::string & filename,
const bool verbose = false) throw (std::invalid_argument);
#ifdef WITH_HPP_FCL
......@@ -90,22 +90,14 @@ namespace se3
* @return The GeometryModel associated to the urdf file and the given Model.
*
*/
inline GeometryModel buildGeom(const Model & model,
const std::string & filename,
const std::vector<std::string> & package_dirs = std::vector<std::string> (),
const GeometryType type = NONE) throw (std::invalid_argument);
GeometryModel buildGeom(const Model & model,
const std::string & filename,
const std::vector<std::string> & package_dirs = std::vector<std::string> (),
const GeometryType type = NONE) throw (std::invalid_argument);
#endif
} // namespace urdf
} // namespace se3
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/parsers/urdf/model.hxx"
#ifdef WITH_HPP_FCL
#include "pinocchio/parsers/urdf/geometry.hxx"
#endif
#endif // ifndef __se3_parsers_urdf_hpp__
src/parsers/urdf/geometry.hxx → src/parsers/urdf/geometry.cpp
View file @ c6a6dec4
......@@ -15,8 +15,9 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_parsers_urdf_geometry_hxx__
#define __se3_parsers_urdf_geometry_hxx__
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/urdf/utils.hpp"
#include "pinocchio/parsers/utils.hpp"
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
......@@ -26,14 +27,8 @@
#include <iomanip>
#include <boost/foreach.hpp>
#include "pinocchio/parsers/urdf/utils.hpp"
#include "pinocchio/parsers/utils.hpp"
#include "pinocchio/multibody/model.hpp"
#include <hpp/fcl/mesh_loader/assimp.h>
#include <exception>
namespace se3
{
namespace urdf
......@@ -53,11 +48,11 @@ namespace se3
*
* @return A shared pointer on the he geometry converted as a fcl::CollisionGeometry
*/
inline boost::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry(const boost::shared_ptr < ::urdf::Geometry> urdf_geometry,
const std::vector < std::string > & package_dirs,
std::string & mesh_path)
boost::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry(const boost::shared_ptr< ::urdf::Geometry> urdf_geometry,
const std::vector<std::string> & package_dirs,
std::string & mesh_path)
{
boost::shared_ptr < fcl::CollisionGeometry > geometry;
boost::shared_ptr<fcl::CollisionGeometry> geometry;
// Handle the case where collision geometry is a mesh
if (urdf_geometry->type == ::urdf::Geometry::MESH)
......@@ -224,11 +219,11 @@ namespace se3
* @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)
*/
inline void parseTreeForGeom(::urdf::LinkConstPtr link,
const Model & model,
GeometryModel & geom_model,
const std::vector<std::string> & package_dirs,
const GeometryType type)
void parseTreeForGeom(::urdf::LinkConstPtr link,
const Model & model,
GeometryModel & geom_model,
const std::vector<std::string> & package_dirs,
const GeometryType type) throw (std::invalid_argument)
{
switch(type)
......@@ -254,10 +249,10 @@ namespace se3
inline GeometryModel buildGeom(const Model & model,
const std::string & filename,
const std::vector<std::string> & package_dirs,
const GeometryType type) throw(std::invalid_argument)
GeometryModel buildGeom(const Model & model,
const std::string & filename,
const std::vector<std::string> & package_dirs,
const GeometryType type) throw(std::invalid_argument)
{
if (type == NONE)
{
......@@ -285,6 +280,3 @@ namespace se3
} // namespace urdf
} // namespace se3
#endif // __se3_parsers_urdf_geometry_hxx__
src/parsers/urdf/model.hxx → src/parsers/urdf/model.cpp
View file @ c6a6dec4
......@@ -16,9 +16,7 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_parsers_urdf_model_hxx__
#define __se3_parsers_urdf_model_hxx__
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/urdf/utils.hpp"
#include "pinocchio/multibody/model.hpp"
......@@ -80,7 +78,7 @@ namespace se3
/// \param[in] model The model where the link must be added.
/// \param[in] placementOffset The relative placement of the link relative to the closer non fixed joint in the tree.
///
inline void parseTree (::urdf::LinkConstPtr link, Model & model, const SE3 & placementOffset, bool verbose) throw (std::invalid_argument)
void parseTree(::urdf::LinkConstPtr link, Model & model, const SE3 & placementOffset, bool verbose) throw (std::invalid_argument)
{
// Parent joint of the current body
......@@ -442,8 +440,6 @@ namespace se3
}
}
///
/// \brief Parse a tree with a specific root joint linking the model to the environment.
/// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
......@@ -572,15 +568,41 @@ namespace se3
}
} // namespace details
///
/// \brief Call parse root tree templated function
///
struct ParseRootTreeVisitor : public boost::static_visitor<>
{
::urdf::LinkConstPtr m_root_link;
Model & m_model;
const bool m_verbose;
ParseRootTreeVisitor(::urdf::LinkConstPtr root_link, Model & model, const bool verbose)
: m_root_link(root_link)
, m_model(model)
, m_verbose(verbose)
{}
template<typename Derived>
void operator()(const JointModelBase<Derived> & root_joint) const
{
details::parseRootTree(m_root_link,m_model,root_joint,m_verbose);
}
static void run(::urdf::LinkConstPtr root_link, Model & model, const JointModelVariant & root_joint, const bool verbose)
{
boost::apply_visitor(ParseRootTreeVisitor(root_link,model,verbose),root_joint);
}
}; // struct ParseRootTreeVisitor
template <typename D>
Model buildModel (const std::string & filename, const JointModelBase<D> & root_joint, bool verbose) throw (std::invalid_argument)
Model buildModel(const std::string & filename, const JointModelVariant & root_joint, const bool verbose) throw (std::invalid_argument)
{
Model model;
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
if (urdfTree)
details::parseRootTree(urdfTree->getRoot(), model, root_joint, verbose);
ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose);
else
{
const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
......@@ -590,7 +612,7 @@ namespace se3
return model;
}
inline Model buildModel (const std::string & filename, const bool verbose) throw (std::invalid_argument)
Model buildModel(const std::string & filename, const bool verbose) throw (std::invalid_argument)
{
Model model;
......@@ -608,5 +630,3 @@ namespace se3
} // namespace urdf
} // namespace se3
#endif // __se3_parsers_urdf_model_hxx__
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment