From 409c4b923c38582ba595c7fc2a0536f0ee60045c Mon Sep 17 00:00:00 2001
From: Jochen Sprickerhof <github@jochen.sprickerhof.de>
Date: Thu, 5 Jan 2017 01:41:49 +0100
Subject: [PATCH] Use urdf::*ShredPtr instead of boost::shared_ptr (#144)

* Use urdf::*SharedPtr instead of boost::shared_ptr

urdfdom_headers uses C++ std::shared_ptr. As it exports it as a custom
*SharedPtr type, we can use them to stay compatible.

* Add compatibility urdf::VoidSharedPtr

* Use urdfdom compatibility header

* Build kinetic for kinetic branch

* Add missing namespace and includes

* Revert "Build kinetic for kinetic branch"

This reverts commit f1c085ccdbe282de8f17b2150aa0ba6ba453db64.

* Add build dependency

* Revert "Add build dependency"

This reverts commit 94175094ffc656e9173c3a46684116471e68279f.

* Export urdfdom version to header

* Add dummy version in case it's not set by urdfdom_headers

* Add missing header
---
 collada_parser/CMakeLists.txt                 |  2 +-
 .../include/collada_parser/collada_parser.h   |  4 +-
 .../collada_parser/collada_parser_plugin.h    |  2 +-
 collada_parser/package.xml                    |  1 +
 collada_parser/src/collada_parser.cpp         | 56 +++++++++++--------
 collada_parser/src/collada_parser_plugin.cpp  |  2 +-
 collada_urdf/src/collada_to_urdf.cpp          | 14 ++---
 collada_urdf/src/collada_urdf.cpp             | 32 +++++------
 kdl_parser/src/kdl_parser.cpp                 |  9 +--
 urdf/CMakeLists.txt                           | 12 ++--
 urdf/src/model.cpp                            |  2 +-
 urdf/test/test_robot_model_parser.cpp         |  8 +--
 urdf/urdfdom_compatibility.h.in               | 12 +++-
 .../include/urdf_parser_plugin/parser.h       |  4 +-
 14 files changed, 90 insertions(+), 70 deletions(-)

diff --git a/collada_parser/CMakeLists.txt b/collada_parser/CMakeLists.txt
index 43ed0c8..e35694e 100644
--- a/collada_parser/CMakeLists.txt
+++ b/collada_parser/CMakeLists.txt
@@ -3,7 +3,7 @@ project(collada_parser)
 
 find_package(Boost REQUIRED system)
 
-find_package(catkin REQUIRED COMPONENTS urdf_parser_plugin roscpp class_loader)
+find_package(catkin REQUIRED COMPONENTS urdf_parser_plugin roscpp class_loader urdf)
 find_package(urdfdom_headers REQUIRED)
 
 catkin_package( 
diff --git a/collada_parser/include/collada_parser/collada_parser.h b/collada_parser/include/collada_parser/collada_parser.h
index da72748..8c1e052 100644
--- a/collada_parser/include/collada_parser/collada_parser.h
+++ b/collada_parser/include/collada_parser/collada_parser.h
@@ -41,13 +41,13 @@
 #include <map>
 #include <boost/function.hpp>
 
-#include <urdf_model/model.h>
+#include <urdf/urdfdom_compatibility.h>
 
 
 namespace urdf {
 
 /// \brief Load Model from string
-boost::shared_ptr<ModelInterface> parseCollada(const std::string &xml_string );
+urdf::ModelInterfaceSharedPtr parseCollada(const std::string &xml_string );
 
 }
 
diff --git a/collada_parser/include/collada_parser/collada_parser_plugin.h b/collada_parser/include/collada_parser/collada_parser_plugin.h
index 32974db..ac699c0 100644
--- a/collada_parser/include/collada_parser/collada_parser_plugin.h
+++ b/collada_parser/include/collada_parser/collada_parser_plugin.h
@@ -46,7 +46,7 @@ class ColladaURDFParser : public URDFParser
 {
 public:
 
-  virtual boost::shared_ptr<ModelInterface> parse(const std::string &xml_string);    
+  virtual urdf::ModelInterfaceSharedPtr parse(const std::string &xml_string);
 };
 
 }
diff --git a/collada_parser/package.xml b/collada_parser/package.xml
index f210ed9..2a2d7b2 100644
--- a/collada_parser/package.xml
+++ b/collada_parser/package.xml
@@ -25,6 +25,7 @@
   <build_depend>roscpp</build_depend>
   <build_depend>urdf_parser_plugin</build_depend>
   <build_depend>class_loader</build_depend>
+  <build_depend>urdf</build_depend>
 
   <run_depend>collada-dom</run_depend>
   <run_depend>liburdfdom-headers-dev</run_depend>
diff --git a/collada_parser/src/collada_parser.cpp b/collada_parser/src/collada_parser.cpp
index 75afeb6..e67f15b 100644
--- a/collada_parser/src/collada_parser.cpp
+++ b/collada_parser/src/collada_parser.cpp
@@ -176,7 +176,11 @@ public:
         USERDATA(double scale) : scale(scale) {
         }
         double scale;
+#if URDFDOM_HEADERS_MAJOR_VERSION < 1
         boost::shared_ptr<void> p; ///< custom managed data
+#else
+        std::shared_ptr<void> p; ///< custom managed data
+#endif
     };
 
     enum GeomType {
@@ -409,7 +413,7 @@ public:
     };
 
 public:
-    ColladaModelReader(boost::shared_ptr<ModelInterface> model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
+    ColladaModelReader(urdf::ModelInterfaceSharedPtr model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
         daeErrorHandler::setErrorHandler(this);
         _resourcedir = ".";
     }
@@ -715,7 +719,7 @@ protected:
             }
 
             // find the target joint
-            boost::shared_ptr<Joint> pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf);
+            urdf::JointSharedPtr pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf);
             if (!pjoint) {
                 continue;
             }
@@ -785,7 +789,7 @@ protected:
                     }
                     BOOST_ASSERT(psymboljoint->hasAttribute("encoding"));
                     BOOST_ASSERT(psymboljoint->getAttribute("encoding")==std::string("COLLADA"));
-                    boost::shared_ptr<Joint> pbasejoint = _getJointFromRef(psymboljoint->getCharData().c_str(),pf);
+                    urdf::JointSharedPtr pbasejoint = _getJointFromRef(psymboljoint->getCharData().c_str(),pf);
                     if( !!pbasejoint ) {
                         // set the mimic properties
                         pjoint->mimic.reset(new JointMimic());
@@ -801,7 +805,7 @@ protected:
     }
 
     ///  \brief Extract Link info and add it to an existing body
-    boost::shared_ptr<Link> _ExtractLink(const domLinkRef pdomlink,const domNodeRef pdomnode, const Pose& tParentWorldLink, const Pose& tParentLink, const std::vector<domJointRef>& vdomjoints, const KinematicsSceneBindings& bindings) {
+    urdf::LinkSharedPtr _ExtractLink(const domLinkRef pdomlink,const domNodeRef pdomnode, const Pose& tParentWorldLink, const Pose& tParentLink, const std::vector<domJointRef>& vdomjoints, const KinematicsSceneBindings& bindings) {
         const std::list<JointAxisBinding>& listAxisBindings = bindings.listAxisBindings;
         //  Set link name with the name of the COLLADA's Link
         std::string linkname = _ExtractLinkName(pdomlink);
@@ -817,7 +821,7 @@ protected:
             }
         }
 
-        boost::shared_ptr<Link> plink;
+        urdf::LinkSharedPtr plink;
         _model->getLink(linkname,plink);
         if( !plink ) {
             plink.reset(new Link());
@@ -921,7 +925,7 @@ protected:
 
                 if (!pdomjoint || pdomjoint->typeID() != domJoint::ID()) {
                     ROS_WARN_STREAM(str(boost::format("could not find attached joint %s!\n")%pattfull->getJoint()));
-                    return boost::shared_ptr<Link>();
+                    return urdf::LinkSharedPtr();
                 }
 
                 // get direct child link
@@ -952,7 +956,7 @@ protected:
                 }
 
                 // create the joints before creating the child links
-                std::vector<boost::shared_ptr<Joint> > vjoints(vdomaxes.getCount());
+                std::vector<urdf::JointSharedPtr > vjoints(vdomaxes.getCount());
                 for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
                     bool joint_active = true; // if not active, put into the passive list
                     FOREACHC(itaxisbinding,listAxisBindings) {
@@ -966,7 +970,7 @@ protected:
                         }
                     }
 
-                    boost::shared_ptr<Joint> pjoint(new Joint());
+                    urdf::JointSharedPtr pjoint(new Joint());
                     pjoint->limits.reset(new JointLimits());
                     pjoint->limits->velocity = 0.0;
                     pjoint->limits->effort = 0.0;
@@ -995,12 +999,16 @@ protected:
                     }
 
                     _getUserData(pdomjoint)->p = pjoint;
+#if URDFDOM_HEADERS_MAJOR_VERSION < 1
                     _getUserData(pdomaxis)->p = boost::shared_ptr<int>(new int(_model->joints_.size()));
+#else
+                    _getUserData(pdomaxis)->p = std::shared_ptr<int>(new int(_model->joints_.size()));
+#endif
                     _model->joints_[pjoint->name] = pjoint;
                     vjoints[ic] = pjoint;
                 }
 
-                boost::shared_ptr<Link> pchildlink = _ExtractLink(pattfull->getLink(), pchildnode, _poseMult(_poseMult(tParentWorldLink,tlink), tatt), tatt, vdomjoints, bindings);
+                urdf::LinkSharedPtr pchildlink = _ExtractLink(pattfull->getLink(), pchildnode, _poseMult(_poseMult(tParentWorldLink,tlink), tatt), tatt, vdomjoints, bindings);
 
                 if (!pchildlink) {
                     ROS_WARN_STREAM(str(boost::format("Link has no child: %s\n")%plink->name));
@@ -1035,7 +1043,7 @@ protected:
                     }
 
                     ROS_DEBUG_STREAM(str(boost::format("Joint %s assigned %d \n")%vjoints[ic]->name%ic));
-                    boost::shared_ptr<Joint> pjoint = vjoints[ic];
+                    urdf::JointSharedPtr pjoint = vjoints[ic];
                     pjoint->child_link_name = pchildlink->name;
 
 #define PRINT_POSE(pname, apose) ROS_DEBUG(pname" pos: %f %f %f, rot: %f %f %f %f", \
@@ -1154,8 +1162,8 @@ protected:
 
         plink->visual->geometry = _CreateGeometry(plink->name, listGeomProperties);
         // visual_groups deprecated
-        //boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > viss;
-        //viss.reset(new std::vector<boost::shared_ptr<Visual > >);
+        //boost::shared_ptr<std::vector<urdf::VisualSharedPtr > > viss;
+        //viss.reset(new std::vector<urdf::VisualSharedPtr >);
         //viss->push_back(plink->visual);
         //plink->visual_groups.insert(std::make_pair("default", viss));
 
@@ -1170,15 +1178,15 @@ protected:
         }
 
         // collision_groups deprecated
-        //boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > cols;
-        //cols.reset(new std::vector<boost::shared_ptr<Collision > >);
+        //boost::shared_ptr<std::vector<urdf::CollisionSharedPtr > > cols;
+        //cols.reset(new std::vector<urdf::CollisionSharedPtr >);
         //cols->push_back(plink->collision);
         //plink->collision_groups.insert(std::make_pair("default", cols));
 
         return plink;
     }
 
-    boost::shared_ptr<Geometry> _CreateGeometry(const std::string& name, const std::list<GEOMPROPERTIES>& listGeomProperties)
+    urdf::GeometrySharedPtr _CreateGeometry(const std::string& name, const std::list<GEOMPROPERTIES>& listGeomProperties)
     {
         std::vector<std::vector<Vector3> > vertices;
         std::vector<std::vector<int> > indices;
@@ -1219,12 +1227,12 @@ protected:
         }
 
         if (vert_counter == 0) {
-          boost::shared_ptr<Mesh> ret;
+          urdf::MeshSharedPtr ret;
           ret.reset();
           return ret;
         }
 
-        boost::shared_ptr<Mesh> geometry(new Mesh());
+        urdf::MeshSharedPtr geometry(new Mesh());
         geometry->type = Geometry::MESH;
         geometry->scale.x = 1;
         geometry->scale.y = 1;
@@ -2020,7 +2028,7 @@ protected:
                 //std::string aname = pextra->getAttribute("name");
                 domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
                 if( !!tec ) {
-                    boost::shared_ptr<Joint> pjoint;
+                    urdf::JointSharedPtr pjoint;
                     daeElementRef domactuator;
                     {
                         daeElementRef bact = tec->getChild("bind_actuator");
@@ -2413,7 +2421,7 @@ protected:
         return name.substr(pos+1)==type;
     }
 
-    boost::shared_ptr<Joint> _getJointFromRef(xsToken targetref, daeElementRef peltref) {
+        urdf::JointSharedPtr _getJointFromRef(xsToken targetref, daeElementRef peltref) {
         daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt;
         domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
 
@@ -2426,10 +2434,10 @@ protected:
 
         if (!pdomjoint || pdomjoint->typeID() != domJoint::ID() || !pdomjoint->getName()) {
             ROS_WARN_STREAM(str(boost::format("could not find collada joint %s!\n")%targetref));
-            return boost::shared_ptr<Joint>();
+            return urdf::JointSharedPtr();
         }
 
-        boost::shared_ptr<Joint> pjoint;
+        urdf::JointSharedPtr pjoint;
         std::string name(pdomjoint->getName());
         if (_model->joints_.find(name) == _model->joints_.end()) {
             pjoint.reset();
@@ -2797,7 +2805,7 @@ protected:
     int _nGlobalSensorId, _nGlobalManipulatorId;
     std::string _filename;
     std::string _resourcedir;
-    boost::shared_ptr<ModelInterface> _model;
+    urdf::ModelInterfaceSharedPtr _model;
     Pose _RootOrigin;
     Pose _VisualRootOrigin;
 };
@@ -2805,9 +2813,9 @@ protected:
 
 
 
-boost::shared_ptr<ModelInterface> parseCollada(const std::string &xml_str)
+urdf::ModelInterfaceSharedPtr parseCollada(const std::string &xml_str)
 {
-    boost::shared_ptr<ModelInterface> model(new ModelInterface);
+    urdf::ModelInterfaceSharedPtr model(new ModelInterface);
 
     ColladaModelReader reader(model);
     if (!reader.InitFromData(xml_str))
diff --git a/collada_parser/src/collada_parser_plugin.cpp b/collada_parser/src/collada_parser_plugin.cpp
index c4d98b0..d8f5369 100644
--- a/collada_parser/src/collada_parser_plugin.cpp
+++ b/collada_parser/src/collada_parser_plugin.cpp
@@ -38,7 +38,7 @@
 #include "collada_parser/collada_parser.h"
 #include <class_loader/class_loader.h>
 
-boost::shared_ptr<urdf::ModelInterface> urdf::ColladaURDFParser::parse(const std::string &xml_string)
+urdf::ModelInterfaceSharedPtr urdf::ColladaURDFParser::parse(const std::string &xml_string)
 {
   return urdf::parseCollada(xml_string);
 }
diff --git a/collada_urdf/src/collada_to_urdf.cpp b/collada_urdf/src/collada_to_urdf.cpp
index 3a4d6e6..14e9741 100644
--- a/collada_urdf/src/collada_to_urdf.cpp
+++ b/collada_urdf/src/collada_to_urdf.cpp
@@ -188,7 +188,7 @@ void assimp_calc_bbox(string fname, float &minx, float &miny, float &minz,
   }
 }
 
-void addChildLinkNamesXML(boost::shared_ptr<const Link> link, ofstream& os)
+void addChildLinkNamesXML(urdf::LinkConstSharedPtr link, ofstream& os)
 {
   os << "  <link name=\"" << link->name << "\">" << endl;
   if ( !!link->visual ) {
@@ -405,14 +405,14 @@ void addChildLinkNamesXML(boost::shared_ptr<const Link> link, ofstream& os)
   }
 #endif
 
-  for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
+  for (std::vector<urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
     addChildLinkNamesXML(*child, os);
 }
 
-void addChildJointNamesXML(boost::shared_ptr<const Link> link, ofstream& os)
+void addChildJointNamesXML(urdf::LinkConstSharedPtr link, ofstream& os)
 {
   double r, p, y;
-  for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
+  for (std::vector<urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
     (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y);
     std::string jtype;
     if ( (*child)->parent_joint->type == urdf::Joint::UNKNOWN ) {
@@ -443,7 +443,7 @@ void addChildJointNamesXML(boost::shared_ptr<const Link> link, ofstream& os)
     os << "    <axis   xyz=\"" <<  (*child)->parent_joint->axis.x << " ";
     os << (*child)->parent_joint->axis.y << " " << (*child)->parent_joint->axis.z << "\"/>" << endl;
     {
-      boost::shared_ptr<urdf::Joint> jt((*child)->parent_joint);
+      urdf::JointSharedPtr jt((*child)->parent_joint);
 
       if ( !!jt->limits ) {
         os << "    <limit ";
@@ -501,7 +501,7 @@ void addChildJointNamesXML(boost::shared_ptr<const Link> link, ofstream& os)
   }
 }
 
-void printTreeXML(boost::shared_ptr<const Link> link, string name, string file)
+void printTreeXML(urdf::LinkConstSharedPtr link, string name, string file)
 {
   std::ofstream os;
   os.open(file.c_str());
@@ -667,7 +667,7 @@ int main(int argc, char** argv)
   }
   xml_file.close();
 
-  boost::shared_ptr<ModelInterface> robot;
+  urdf::ModelInterfaceSharedPtr robot;
   if( xml_string.find("<COLLADA") != std::string::npos )
   {
     ROS_DEBUG("Parsing robot collada xml string");
diff --git a/collada_urdf/src/collada_urdf.cpp b/collada_urdf/src/collada_urdf.cpp
index e6bfebe..fc1a720 100644
--- a/collada_urdf/src/collada_urdf.cpp
+++ b/collada_urdf/src/collada_urdf.cpp
@@ -544,7 +544,7 @@ private:
         domInstance_with_extraRef piscene;
     };
 
-    typedef std::map< boost::shared_ptr<const urdf::Link>, urdf::Pose > MAPLINKPOSES;
+    typedef std::map< urdf::LinkConstSharedPtr, urdf::Pose > MAPLINKPOSES;
     struct LINKOUTPUT
     {
         list<pair<int,string> > listusedlinks;
@@ -568,7 +568,7 @@ private:
             axis_output() : iaxis(0) {
             }
             string sid, nodesid;
-            boost::shared_ptr<const urdf::Joint> pjoint;
+            urdf::JointConstSharedPtr pjoint;
             int iaxis;
             string jointnodesid;
         };
@@ -794,7 +794,7 @@ protected:
 
         for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
             string axis_infosid = _ComputeId(str(boost::format("axis_info_inst%d")%idof));
-            boost::shared_ptr<const urdf::Joint> pjoint = _ikmout->kmout->vaxissids.at(idof).pjoint;
+            urdf::JointConstSharedPtr pjoint = _ikmout->kmout->vaxissids.at(idof).pjoint;
             BOOST_ASSERT(_mapjointindices[pjoint] == (int)idof);
             //int iaxis = _ikmout->kmout->vaxissids.at(idof).iaxis;
 
@@ -972,7 +972,7 @@ protected:
         kmout->vlinksids.resize(_robot.links_.size());
 
         FOREACHC(itjoint, _robot.joints_) {
-            boost::shared_ptr<urdf::Joint> pjoint = itjoint->second;
+            urdf::JointSharedPtr pjoint = itjoint->second;
             int index = _mapjointindices[itjoint->second];
             domJointRef pdomjoint = daeSafeCast<domJoint>(ktec->add(COLLADA_ELEMENT_JOINT));
             string jointid = _ComputeId(pjoint->name); //str(boost::format("joint%d")%index);
@@ -1045,7 +1045,7 @@ protected:
         // create the formulas for all mimic joints
         FOREACHC(itjoint, _robot.joints_) {
             string jointsid = _ComputeId(itjoint->second->name);
-            boost::shared_ptr<urdf::Joint> pjoint = itjoint->second;
+            urdf::JointSharedPtr pjoint = itjoint->second;
             if( !pjoint->mimic ) {
                 continue;
             }
@@ -1131,7 +1131,7 @@ protected:
     /// \param pkinparent Kinbody parent
     /// \param pnodeparent Node parent
     /// \param strModelUri
-    virtual LINKOUTPUT _WriteLink(boost::shared_ptr<const urdf::Link> plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri)
+    virtual LINKOUTPUT _WriteLink(urdf::LinkConstSharedPtr plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri)
     {
         LINKOUTPUT out;
         int linkindex = _maplinkindices[plink];
@@ -1147,8 +1147,8 @@ protected:
         pnode->setSid(nodesid.c_str());
         pnode->setName(plink->name.c_str());
 
-        boost::shared_ptr<urdf::Geometry> geometry;
-        boost::shared_ptr<urdf::Material> material;
+        urdf::GeometrySharedPtr geometry;
+        urdf::MaterialSharedPtr material;
         urdf::Pose geometry_origin;
         if( !!plink->visual ) {
             geometry = plink->visual->geometry;
@@ -1167,7 +1167,7 @@ protected:
             if ( !!plink->visual ) {
               if (plink->visual_array.size() > 1) {
 		int igeom = 0;
-		for (std::vector<boost::shared_ptr<urdf::Visual > >::const_iterator it = plink->visual_array.begin();
+		for (std::vector<urdf::VisualSharedPtr >::const_iterator it = plink->visual_array.begin();
 		     it != plink->visual_array.end(); it++) {
 		  // geom
 		  string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom));
@@ -1214,7 +1214,7 @@ protected:
 
         // process all children
         FOREACHC(itjoint, plink->child_joints) {
-            boost::shared_ptr<urdf::Joint> pjoint = *itjoint;
+            urdf::JointSharedPtr pjoint = *itjoint;
             int index = _mapjointindices[pjoint];
 
             // <attachment_full joint="k1/joint0">
@@ -1275,7 +1275,7 @@ protected:
         return out;
     }
 
-    domGeometryRef _WriteGeometry(boost::shared_ptr<urdf::Geometry> geometry, const std::string& geometry_id, urdf::Pose *org_trans = NULL)
+    domGeometryRef _WriteGeometry(urdf::GeometrySharedPtr geometry, const std::string& geometry_id, urdf::Pose *org_trans = NULL)
     {
         domGeometryRef cgeometry = daeSafeCast<domGeometry>(_geometriesLib->add(COLLADA_ELEMENT_GEOMETRY));
         cgeometry->setId(geometry_id.c_str());
@@ -1314,7 +1314,7 @@ protected:
         return cgeometry;
     }
 
-    void _WriteMaterial(const string& geometry_id, boost::shared_ptr<urdf::Material> material)
+    void _WriteMaterial(const string& geometry_id, urdf::MaterialSharedPtr material)
     {
         string effid = geometry_id+string("_eff");
         string matid = geometry_id+string("_mat");
@@ -1392,7 +1392,7 @@ protected:
             rigid_body->setSid(rigidsid.c_str());
             rigid_body->setName(itlink->second->name.c_str());
             domRigid_body::domTechnique_commonRef ptec = daeSafeCast<domRigid_body::domTechnique_common>(rigid_body->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
-            boost::shared_ptr<urdf::Inertial> inertial = itlink->second->inertial;
+            urdf::InertialSharedPtr inertial = itlink->second->inertial;
             if( !!inertial ) {
                 daeSafeCast<domRigid_body::domTechnique_common::domDynamic>(ptec->add(COLLADA_ELEMENT_DYNAMIC))->setValue(xsBoolean(true)); //!!inertial));
                 domTargetable_floatRef mass = daeSafeCast<domTargetable_float>(ptec->add(COLLADA_ELEMENT_MASS));
@@ -1922,9 +1922,9 @@ private:
 
     boost::shared_ptr<instance_kinematics_model_output> _ikmout;
     boost::shared_ptr<instance_articulated_system_output> _iasout;
-    std::map< boost::shared_ptr<const urdf::Joint>, int > _mapjointindices;
-    std::map< boost::shared_ptr<const urdf::Link>, int > _maplinkindices;
-    std::map< boost::shared_ptr<const urdf::Material>, int > _mapmaterialindices;
+    std::map< urdf::JointConstSharedPtr, int > _mapjointindices;
+    std::map< urdf::LinkConstSharedPtr, int > _maplinkindices;
+    std::map< urdf::MaterialConstSharedPtr, int > _mapmaterialindices;
     Assimp::Importer _importer;
 };
 
diff --git a/kdl_parser/src/kdl_parser.cpp b/kdl_parser/src/kdl_parser.cpp
index 9777320..37ed58c 100644
--- a/kdl_parser/src/kdl_parser.cpp
+++ b/kdl_parser/src/kdl_parser.cpp
@@ -36,6 +36,7 @@
 
 #include "kdl_parser/kdl_parser.hpp"
 #include <urdf/model.h>
+#include <urdf/urdfdom_compatibility.h>
 #include <kdl/frames_io.hpp>
 #include <ros/console.h>
 
@@ -64,7 +65,7 @@ Frame toKdl(urdf::Pose p)
 }
 
 // construct joint
-Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
+Joint toKdl(urdf::JointSharedPtr jnt)
 {
   Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
 
@@ -93,7 +94,7 @@ Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
 }
 
 // construct inertia
-RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
+RigidBodyInertia toKdl(urdf::InertialSharedPtr i)
 {
   Frame origin = toKdl(i->origin);
   
@@ -124,9 +125,9 @@ RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
 
 
 // recursive function to walk through tree
-bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
+bool addChildrenToTree(urdf::LinkConstSharedPtr root, Tree& tree)
 {
-  std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links;
+  std::vector<urdf::LinkSharedPtr > children = root->child_links;
   ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
 
   // constructs the optional inertia
diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt
index e65181f..e6fc0dc 100644
--- a/urdf/CMakeLists.txt
+++ b/urdf/CMakeLists.txt
@@ -10,13 +10,13 @@ find_package(catkin REQUIRED COMPONENTS
 
 find_package(TinyXML REQUIRED)
 
-if("${urdfdom_headers_VERSION}" VERSION_LESS "1.0")
-  # for Wily: maintain compatibility between urdfdom 0.3 and 0.4 (definining SharedPtr types)
-  set(URDFDOM_DECLARE_TYPES 1)
-else()
-  # declare ModelInterface's SharedPtr
-  set(URDFDOM_DECLARE_TYPES 0)
+# Find version components
+if(NOT urdfdom_headers_VERSION)
+set(urdfdom_headers_VERSION "0.0.0")
 endif()
+string(REGEX REPLACE "^([0-9]+).*" "\\1" URDFDOM_HEADERS_MAJOR_VERSION "${urdfdom_headers_VERSION}")
+string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" URDFDOM_HEADERS_MINOR_VERSION "${urdfdom_headers_VERSION}")
+string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" URDFDOM_HEADERS_REVISION_VERSION "${urdfdom_headers_VERSION}")
 set(generated_compat_header "${CATKIN_DEVEL_PREFIX}/include/${PROJECT_NAME}/urdfdom_compatibility.h")
 include_directories("${CATKIN_DEVEL_PREFIX}/include")
 configure_file(urdfdom_compatibility.h.in "${generated_compat_header}" @ONLY)
diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp
index a721056..69f96b8 100644
--- a/urdf/src/model.cpp
+++ b/urdf/src/model.cpp
@@ -136,7 +136,7 @@ bool Model::initXml(TiXmlElement *robot_xml)
 
 bool Model::initString(const std::string& xml_string)
 {
-  boost::shared_ptr<ModelInterface> model;
+  urdf::ModelInterfaceSharedPtr model;
 
   // necessary for COLLADA compatibility
   if( IsColladaData(xml_string) ) {
diff --git a/urdf/test/test_robot_model_parser.cpp b/urdf/test/test_robot_model_parser.cpp
index f6286a5..81f90af 100644
--- a/urdf/test/test_robot_model_parser.cpp
+++ b/urdf/test/test_robot_model_parser.cpp
@@ -54,7 +54,7 @@ public:
   bool checkModel(urdf::Model & robot)
   {
     // get root link
-    boost::shared_ptr<const Link> root_link = robot.getRoot();
+    urdf::LinkConstSharedPtr root_link = robot.getRoot();
     if (!root_link)
     {
       ROS_ERROR("no root link %s", robot.getName().c_str());
@@ -79,12 +79,12 @@ protected:
   {
   }
 
-  bool traverse_tree(boost::shared_ptr<const Link> link,int level = 0)
+  bool traverse_tree(urdf::LinkConstSharedPtr link,int level = 0)
   {
     ROS_INFO("Traversing tree at level %d, link size %lu", level, link->child_links.size());
     level+=2;
     bool retval = true;
-    for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
+    for (std::vector<urdf::LinkSharedPtr>::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
     {
       ++num_links;
       if (*child && (*child)->parent_joint)
@@ -142,7 +142,7 @@ TEST_F(TestParser, test)
   ASSERT_TRUE(robot.initFile(folder + file));
 
   EXPECT_EQ(robot.getName(), robot_name);
-  boost::shared_ptr<const urdf::Link> root = robot.getRoot();
+  urdf::LinkConstSharedPtr root = robot.getRoot();
   ASSERT_TRUE(static_cast<bool>(root));
   EXPECT_EQ(root->name, root_name);
 
diff --git a/urdf/urdfdom_compatibility.h.in b/urdf/urdfdom_compatibility.h.in
index eddbc38..b96e186 100644
--- a/urdf/urdfdom_compatibility.h.in
+++ b/urdf/urdfdom_compatibility.h.in
@@ -37,7 +37,12 @@
 #ifndef URDF_URDFDOM_COMPATIBILITY_
 #define URDF_URDFDOM_COMPATIBILITY_
 
-#if @URDFDOM_DECLARE_TYPES@ // This is needed for urdfdom <= 0.4
+#define URDFDOM_HEADERS_MAJOR_VERSION @URDFDOM_HEADERS_MAJOR_VERSION@
+#define URDFDOM_HEADERS_MINOR_VERSION @URDFDOM_HEADERS_MINOR_VERSION@
+#define URDFDOM_HEADERS_REVISION_VERSION @URDFDOM_HEADERS_REVISION_VERSION@
+
+// for Wily: maintain compatibility between urdfdom 0.3 and 0.4 (definining SharedPtr types)
+#if URDFDOM_HEADERS_MAJOR_VERSION == 0 && URDFDOM_HEADERS_MINOR_VERSION <= 4
 
 #include <boost/shared_ptr.hpp>
 #include <boost/weak_ptr.hpp>
@@ -75,9 +80,14 @@ URDF_TYPEDEF_CLASS_POINTER(ModelInterface);
 
 #else // urdfdom <= 0.4
 
+#include <urdf_model/types.h>
+#include <urdf_world/types.h>
+
+namespace urdf {
 typedef std::shared_ptr<ModelInterface> ModelInterfaceSharedPtr;
 typedef std::shared_ptr<const ModelInterface> ModelInterfaceConstSharedPtr;
 typedef std::weak_ptr<ModelInterface> ModelInterfaceWeakPtr;
+}
 
 #endif // urdfdom > 0.4
 
diff --git a/urdf_parser_plugin/include/urdf_parser_plugin/parser.h b/urdf_parser_plugin/include/urdf_parser_plugin/parser.h
index 7032b13..92de411 100644
--- a/urdf_parser_plugin/include/urdf_parser_plugin/parser.h
+++ b/urdf_parser_plugin/include/urdf_parser_plugin/parser.h
@@ -37,7 +37,7 @@
 #ifndef URDF_PARSER_PLUGIN_H
 #define URDF_PARSER_PLUGIN_H
 
-#include <urdf_model/model.h>
+#include <urdf/urdfdom_compatibility.h>
 
 namespace urdf
 {
@@ -54,7 +54,7 @@ public:
   }
 
   /// \brief Load Model from string
-  virtual boost::shared_ptr<ModelInterface> parse(const std::string &xml_string) = 0;
+  virtual urdf::ModelInterfaceSharedPtr parse(const std::string &xml_string) = 0;
 };
   
 }