Skip to content

GitLab

Commit 91286173 authored by jcarpent's avatar jcarpent
Browse files

[Parsers] Use right shared_ptr type according to the URDFDOM version

parent 5c8e1b15
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 91286173
......@@ -92,13 +92,24 @@ IF(EIGEN3_FOUND)
ENDIF(${EIGEN3_VERSION} VERSION_GREATER "3.2.10")
ENDIF(EIGEN3_FOUND)
# Special care of urdfdom less than 0.3.0
# Special care of urdfdom version
IF(URDFDOM_FOUND)
IF(${URDFDOM_VERSION} VERSION_LESS "0.3.0")
ADD_DEFINITIONS(-DURDFDOM_COLLISION_WITH_GROUP_NAME)
SET(URDFDOM_COLLISION_WITH_GROUP_NAME TRUE)
PKG_CONFIG_APPEND_CFLAGS("-DURDFDOM_COLLISION_WITH_GROUP_NAME")
ENDIF(${URDFDOM_VERSION} VERSION_LESS "0.3.0")
# defines types from version 0.4.0
IF(NOT ${URDFDOM_VERSION} VERSION_LESS "0.4.0")
ADD_DEFINITIONS(-DURDFDOM_TYPEDEF_SHARED_PTR)
PKG_CONFIG_APPEND_CFLAGS("-DURDFDOM_TYPEDEF_SHARED_PTR")
ENDIF(NOT ${URDFDOM_VERSION} VERSION_LESS "0.4.0")
# std::shared_ptr appears from version 1.0.0
IF(${URDFDOM_VERSION} VERSION_GREATER "0.4.2")
ADD_DEFINITIONS(-DURDFDOM_USE_STD_SHARED_PTR)
PKG_CONFIG_APPEND_CFLAGS("-DURDFDOM_USE_STD_SHARED_PTR")
ENDIF(${URDFDOM_VERSION} VERSION_GREATER "0.4.2")
ENDIF(URDFDOM_FOUND)
# Special care of lua which can be of versions 5.1 or 5.2
......
src/parsers/urdf.hpp
View file @ 91286173
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
......@@ -27,17 +27,73 @@
#include <string>
#include <exception>
#include <boost/shared_ptr.hpp>
#ifdef URDFDOM_USE_STD_SHARED_PTR
#include <memory>
#define URDF_SHARED_PTR(type) std::shared_ptr<type>
#define URDF_WEAK_PTR(type) std::weak_ptr<type>
#else
#include <boost/shared_ptr.hpp>
#define URDF_SHARED_PTR(type) boost::shared_ptr<type>
#define URDF_WEAK_PTR(type) boost::weak_ptr<type>
#endif
#ifndef URDFDOM_TYPEDEF_SHARED_PTR
#define URDF_TYPEDEF_CLASS_POINTER(Class) \
typedef URDF_SHARED_PTR(Class) Class##SharedPtr; \
typedef URDF_SHARED_PTR(const Class) Class##ConstSharedPtr; \
typedef URDF_WEAK_PTR(Class) Class##WeakPtr
namespace urdf
{
typedef boost::shared_ptr<ModelInterface> ModelInterfacePtr;
typedef boost::shared_ptr<const Joint> JointConstPtr;
typedef boost::shared_ptr<const Link> LinkConstPtr;
typedef boost::shared_ptr<Link> LinkPtr;
typedef boost::shared_ptr<const Inertial> InertialConstPtr;
URDF_TYPEDEF_CLASS_POINTER(Box);
URDF_TYPEDEF_CLASS_POINTER(Collision);
URDF_TYPEDEF_CLASS_POINTER(Cylinder);
URDF_TYPEDEF_CLASS_POINTER(Geometry);
URDF_TYPEDEF_CLASS_POINTER(Inertial);
URDF_TYPEDEF_CLASS_POINTER(Joint);
URDF_TYPEDEF_CLASS_POINTER(Link);
URDF_TYPEDEF_CLASS_POINTER(Material);
URDF_TYPEDEF_CLASS_POINTER(Mesh);
URDF_TYPEDEF_CLASS_POINTER(ModelInterface);
URDF_TYPEDEF_CLASS_POINTER(Sphere);
URDF_TYPEDEF_CLASS_POINTER(Visual);
template<class T, class U>
URDF_SHARED_PTR(T) const_pointer_cast(URDF_SHARED_PTR(U) const & r)
{
#ifdef URDFDOM_USE_STD_SHARED_PTR
return std::const_pointer_cast<T>(r);
#else
return boost::const_pointer_cast<T>(r);
#endif
}
template<class T, class U>
URDF_SHARED_PTR(T) dynamic_pointer_cast(URDF_SHARED_PTR(U) const & r)
{
#ifdef URDFDOM_USE_STD_SHARED_PTR
return std::dynamic_pointer_cast<T>(r);
#else
return boost::dynamic_pointer_cast<T>(r);
#endif
}
template<class T, class U>
URDF_SHARED_PTR(T) static_pointer_cast(URDF_SHARED_PTR(U) const & r)
{
#ifdef URDFDOM_USE_STD_SHARED_PTR
return std:static_pointer_cast<T>(r);
#else
return boost::static_pointer_cast<T>(r);
#endif
}
}
#undef URDF_TYPEDEF_CLASS_POINTER
#endif
namespace se3
{
namespace urdf
......
src/parsers/urdf/geometry.cpp
View file @ 91286173
......@@ -27,6 +27,7 @@
#include <sstream>
#include <iomanip>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#ifdef WITH_HPP_FCL
#include <hpp/fcl/mesh_loader/assimp.h>
......@@ -53,7 +54,7 @@ namespace se3
*
* @return A shared pointer on the he geometry converted as a fcl::CollisionGeometry
*/
boost::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry(const boost::shared_ptr< ::urdf::Geometry> urdf_geometry,
boost::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry(const ::urdf::GeometrySharedPtr urdf_geometry,
const std::vector<std::string> & package_dirs,
std::string & meshPath,
Eigen::Vector3d & meshScale)
......@@ -63,7 +64,7 @@ namespace se3
// Handle the case where collision geometry is a mesh
if (urdf_geometry->type == ::urdf::Geometry::MESH)
{
boost::shared_ptr < ::urdf::Mesh> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
const ::urdf::MeshSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
std::string collisionFilename = collisionGeometry->filename;
meshPath = retrieveResourcePath(collisionFilename, package_dirs);
......@@ -95,7 +96,7 @@ namespace se3
{
meshPath = "CYLINDER";
meshScale << 1,1,1;
boost::shared_ptr < ::urdf::Cylinder> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
double radius = collisionGeometry->radius;
double length = collisionGeometry->length;
......@@ -108,7 +109,7 @@ namespace se3
{
meshPath = "BOX";
meshScale << 1,1,1;
boost::shared_ptr < ::urdf::Box> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
const ::urdf::BoxSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
double x = collisionGeometry->dim.x;
double y = collisionGeometry->dim.y;
......@@ -121,7 +122,7 @@ namespace se3
{
meshPath = "SPHERE";
meshScale << 1,1,1;
boost::shared_ptr < ::urdf::Sphere> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
const ::urdf::SphereSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
double radius = collisionGeometry->radius;
......@@ -146,16 +147,19 @@ namespace se3
* @return Either the first collision or visual
*/
template<typename T>
inline boost::shared_ptr<T> getLinkGeometry(::urdf::LinkConstPtr link);
inline const URDF_SHARED_PTR(T)
getLinkGeometry(const ::urdf::LinkConstSharedPtr link);
template<>
inline boost::shared_ptr< ::urdf::Collision> getLinkGeometry< ::urdf::Collision>(::urdf::LinkConstPtr link)
inline const ::urdf::CollisionSharedPtr
getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
{
return link->collision;
}
template<>
inline boost::shared_ptr< ::urdf::Visual> getLinkGeometry< ::urdf::Visual>(::urdf::LinkConstPtr link)
inline const ::urdf::VisualSharedPtr
getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
{
return link->visual;
}
......@@ -171,11 +175,11 @@ namespace se3
*
*/
template<typename urdfObject>
inline bool getVisualMaterial(const boost::shared_ptr< urdfObject > urdf_object,std::string& meshTexturePath,
inline bool getVisualMaterial(const 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 boost::shared_ptr< ::urdf::Collision >, std::string& meshTexturePath,
inline bool getVisualMaterial< ::urdf::Collision>(const ::urdf::CollisionSharedPtr, std::string& meshTexturePath,
Eigen::Vector4d & meshColor, const std::vector<std::string> &)
{
meshColor.setZero();
......@@ -184,7 +188,7 @@ namespace se3
}
template<>
inline bool getVisualMaterial< ::urdf::Visual>(const boost::shared_ptr< ::urdf::Visual > urdf_visual, std::string& meshTexturePath,
inline bool getVisualMaterial< ::urdf::Visual>(const ::urdf::VisualSharedPtr urdf_visual, std::string& meshTexturePath,
Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs)
{
meshColor.setZero();
......@@ -211,16 +215,19 @@ namespace se3
* @return the array of either collisions or visuals
*/
template<typename T>
inline std::vector<boost::shared_ptr<T> > getLinkGeometryArray(::urdf::LinkConstPtr link);
inline const std::vector< URDF_SHARED_PTR(T) > &
getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link);
template<>
inline std::vector< boost::shared_ptr< ::urdf::Collision> > getLinkGeometryArray< ::urdf::Collision>(::urdf::LinkConstPtr link)
inline const std::vector< ::urdf::CollisionSharedPtr> &
getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
{
return link->collision_array;
}
template<>
inline std::vector< boost::shared_ptr< ::urdf::Visual> > getLinkGeometryArray< ::urdf::Visual>(::urdf::LinkConstPtr link)
inline const std::vector< ::urdf::VisualSharedPtr> &
getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
{
return link->visual_array;
}
......@@ -236,20 +243,21 @@ namespace se3
* @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION)
*/
template<typename T>
inline void addLinkGeometryToGeomModel(::urdf::LinkConstPtr link,
inline void addLinkGeometryToGeomModel(::urdf::LinkConstSharedPtr link,
const Model & model,
GeometryModel & geomModel,
const std::vector<std::string> & package_dirs) throw (std::invalid_argument)
{
typedef std::vector< URDF_SHARED_PTR(T) > VectorSharedT;
if(getLinkGeometry<T>(link))
{
std::string meshPath = "";
Eigen::Vector3d meshScale;
std::string link_name = link->name;
const std::string & link_name = link->name;
std::vector< boost::shared_ptr< T > > geometries_array = getLinkGeometryArray<T>(link);
VectorSharedT geometries_array = getLinkGeometryArray<T>(link);
if (!model.existFrame(link_name, BODY))
throw std::invalid_argument("No link " + link_name + " in model");
......@@ -258,7 +266,7 @@ namespace se3
assert(model.frames[frame_id].type == BODY);
std::size_t objectId = 0;
for (typename std::vector< boost::shared_ptr< T > >::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
{
meshPath.clear();
#ifdef WITH_HPP_FCL
......@@ -299,7 +307,7 @@ 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)
*/
void parseTreeForGeom(::urdf::LinkConstPtr link,
void parseTreeForGeom(::urdf::LinkConstSharedPtr link,
const Model & model,
GeometryModel & geomModel,
const std::vector<std::string> & package_dirs,
......@@ -318,7 +326,7 @@ namespace se3
break;
}
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links)
{
parseTreeForGeom(child, model, geomModel, package_dirs,type);
}
......@@ -347,7 +355,7 @@ namespace se3
throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash");
}
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
details::parseTreeForGeom(urdfTree->getRoot(), model, geomModel, hint_directories,type);
return geomModel;
}
......
src/parsers/urdf/model.cpp
View file @ 91286173
......@@ -37,7 +37,7 @@ namespace se3
const FrameType JOINT_OR_FIXED_JOINT = (FrameType) (JOINT | FIXED_JOINT);
const double infty = std::numeric_limits<double>::infinity();
FrameIndex getParentJointFrame(::urdf::LinkConstPtr link, Model & model)
FrameIndex getParentJointFrame(const ::urdf::LinkConstSharedPtr link, const Model & model)
{
assert(link && link->getParent());
......@@ -55,23 +55,23 @@ namespace se3
+ link->getParent()->parent_joint->name);
}
Frame& f = model.frames[id];
const Frame & f = model.frames[id];
if (f.type == JOINT || f.type == FIXED_JOINT)
return id;
throw std::invalid_argument ("Parent frame is not a JOINT neither a FIXED_JOINT");
}
void appendBodyToJoint(Model& model, const FrameIndex fid,
const boost::shared_ptr< ::urdf::Inertial> Y,
void appendBodyToJoint(Model & model, const FrameIndex fid,
const ::urdf::InertialConstSharedPtr Y_ptr,
const SE3 & placement,
const std::string & body_name)
{
const Frame& frame = model.frames[fid];
const SE3& p = frame.placement * placement;
if (frame.parent > 0
&& Y
&& Y->mass > Eigen::NumTraits<double>::epsilon()) {
model.appendBodyToJoint(frame.parent, convertFromUrdf(*Y), p);
&& Y_ptr
&& Y_ptr->mass > Eigen::NumTraits<double>::epsilon()) {
model.appendBodyToJoint(frame.parent, convertFromUrdf(*Y_ptr), p);
}
model.addBodyFrame(body_name, frame.parent, p, (int)fid);
// Reference to model.frames[fid] can has changed because the vector
......@@ -86,9 +86,10 @@ namespace se3
/// \brief Shortcut for adding a joint and directly append a body to it.
///
template<typename JointModel>
void addJointAndBody(Model & model, const JointModelBase<JointModel> & jmodel, const FrameIndex& parentFrameId,
void addJointAndBody(Model & model, const JointModelBase<JointModel> & jmodel,
const FrameIndex & parentFrameId,
const SE3 & joint_placement, const std::string & joint_name,
const boost::shared_ptr< ::urdf::Inertial> Y,
const ::urdf::InertialConstSharedPtr Y,
const std::string & body_name,
const typename JointModel::TangentVector_t & max_effort = JointModel::TangentVector_t::Constant( infty),
const typename JointModel::TangentVector_t & max_velocity = JointModel::TangentVector_t::Constant( infty),
......@@ -96,7 +97,7 @@ namespace se3
const typename JointModel::ConfigVector_t & max_config = JointModel::ConfigVector_t ::Constant( infty))
{
Model::JointIndex idx;
Frame& frame = model.frames[parentFrameId];
const Frame & frame = model.frames[parentFrameId];
idx = model.addJoint(frame.parent,jmodel,
frame.placement * joint_placement,
......@@ -112,10 +113,10 @@ namespace se3
///
void addFixedJointAndBody(Model & model, const FrameIndex& parentFrameId,
const SE3 & joint_placement, const std::string & joint_name,
const boost::shared_ptr< ::urdf::Inertial> Y,
const ::urdf::InertialConstSharedPtr Y,
const std::string & body_name)
{
Frame& frame = model.frames[parentFrameId];
const Frame & frame = model.frames[parentFrameId];
int fid = model.addFrame(
Frame (joint_name, frame.parent, parentFrameId,
......@@ -129,9 +130,12 @@ namespace se3
///
/// \brief Handle the case of JointModelComposite which is dynamic.
///
void addJointAndBody(Model & model, const JointModelBase< JointModelComposite > & jmodel, const Model::JointIndex parent_id,
void addJointAndBody(Model & model,
const JointModelBase< JointModelComposite > & jmodel,
const Model::JointIndex parent_id,
const SE3 & joint_placement, const std::string & joint_name,
const boost::shared_ptr< ::urdf::Inertial> Y, const std::string & body_name)
const ::urdf::InertialConstSharedPtr Y,
const std::string & body_name)
{
Model::JointIndex idx;
......@@ -148,11 +152,12 @@ namespace se3
/// \param[in] link The current URDF link.
/// \param[in] model The model where the link must be added.
///
void parseTree(::urdf::LinkConstPtr link, Model & model, bool verbose) throw (std::invalid_argument)
void parseTree(::urdf::LinkConstSharedPtr link, Model & model, bool verbose) throw (std::invalid_argument)
{
// Parent joint of the current body
::urdf::JointConstPtr joint = link->parent_joint;
const ::urdf::JointConstSharedPtr joint =
::urdf::const_pointer_cast< ::urdf::Joint>(link->parent_joint);
if(joint) // if the link is not the root of the tree
{
......@@ -168,7 +173,8 @@ namespace se3
// Transformation from the parent link to the joint origin
const SE3 jointPlacement = convertFromUrdf(joint->parent_to_joint_origin_transform);
const boost::shared_ptr< ::urdf::Inertial> Y = link->inertial;
const ::urdf::InertialSharedPtr Y =
::urdf::const_pointer_cast< ::urdf::Inertial>(link->inertial);
switch(joint->type)
{
......@@ -477,7 +483,7 @@ namespace se3
}
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
BOOST_FOREACH(::urdf::LinkConstSharedPtr child, link->child_links)
{
parseTree(child, model, verbose);
}
......@@ -491,12 +497,12 @@ namespace se3
/// \param[in] model The model where the link must be added.
/// \param[in] verbose Print parsing info.
///
void parseRootTree (::urdf::LinkConstPtr root_link, Model & model, const bool verbose) throw (std::invalid_argument)
void parseRootTree (::urdf::LinkConstSharedPtr root_link, Model & model, const bool verbose) throw (std::invalid_argument)
{
addFixedJointAndBody(model, 0, SE3::Identity(), "root_joint",
root_link->inertial, root_link->name);
BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links)
BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links)
{
parseTree(child, model, verbose);
}
......@@ -517,13 +523,13 @@ namespace se3
/// \param[in] verbose Print parsing info.
///
template <typename D>
void parseRootTree (::urdf::LinkConstPtr root_link, Model & model, const JointModelBase<D> & root_joint, const bool verbose) throw (std::invalid_argument)
void parseRootTree (::urdf::LinkConstSharedPtr root_link, Model & model, const JointModelBase<D> & root_joint, const bool verbose) throw (std::invalid_argument)
{
addJointAndBody(model,root_joint,
0,SE3::Identity(),"root_joint",
root_link->inertial,root_link->name);
BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links)
BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links)
{
parseTree(child, model, verbose);
}
......@@ -537,11 +543,11 @@ namespace se3
///
struct ParseRootTreeVisitor : public boost::static_visitor<>
{
::urdf::LinkConstPtr m_root_link;
::urdf::LinkConstSharedPtr m_root_link;
Model & m_model;
const bool m_verbose;
ParseRootTreeVisitor(::urdf::LinkConstPtr root_link, Model & model, const bool verbose)
ParseRootTreeVisitor(::urdf::LinkConstSharedPtr root_link, Model & model, const bool verbose)
: m_root_link(root_link)
, m_model(model)
, m_verbose(verbose)
......@@ -553,7 +559,7 @@ namespace se3
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)
static void run(::urdf::LinkConstSharedPtr root_link, Model & model, const JointModelVariant & root_joint, const bool verbose)
{
boost::apply_visitor(ParseRootTreeVisitor(root_link,model,verbose),root_joint);
}
......@@ -565,7 +571,7 @@ namespace se3
const bool verbose)
throw (std::invalid_argument)
{
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
if (urdfTree)
ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose);
else
......@@ -579,7 +585,7 @@ namespace se3
Model& buildModel(const std::string & filename, Model& model,const bool verbose)
throw (std::invalid_argument)
{
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
if (urdfTree)
details::parseRootTree(urdfTree->getRoot(),model,verbose);
else
......
Markdown is supported
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