From 39e8e822ae6c5d48eca05da785f006e6b6e8d0fd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Arne=20B=C3=B6ckmann?= <arne.boeckmann@dfki.de>
Date: Thu, 3 Dec 2015 15:47:41 +0100
Subject: [PATCH 1/2] Replace relative mesh paths with absolut paths while
 parsing

---
 src/SMURFParser.cpp | 45 ++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 44 insertions(+), 1 deletion(-)

diff --git a/src/SMURFParser.cpp b/src/SMURFParser.cpp
index 1cbe804..a464395 100644
--- a/src/SMURFParser.cpp
+++ b/src/SMURFParser.cpp
@@ -30,6 +30,44 @@
 
 namespace smurf_parser {
 
+  
+  /**Replaces relative paths inside the urdf model with absolut paths. */
+  void relativeToAbsolut(boost::shared_ptr<urdf::ModelInterface> model,
+                         const std::string& urdfPathStr)
+  {
+    assert(urdfPathStr.size() > 0);
+    assert(model.get() != NULL);
+    
+    //strip filename from urdf path (if there is one)
+    boost::filesystem::path urdfPath(urdfPathStr);
+    urdfPath.remove_filename(); 
+     
+    std::map<std::string, boost::shared_ptr<urdf::Link> >::iterator it;
+    for(it = model->links_.begin(); it != model->links_.end(); ++it)
+    {
+      std::vector<boost::shared_ptr<urdf::Visual> >& visuals = it->second->visual_array;
+      std::vector<boost::shared_ptr<urdf::Visual> >::iterator visIt;
+      for(visIt = visuals.begin(); visIt != visuals.end(); ++visIt)
+      {
+        boost::shared_ptr<urdf::Geometry> geometry = (*visIt)->geometry;
+        if(geometry->type == urdf::Geometry::MESH)
+        {
+          boost::shared_ptr<urdf::Mesh> mesh = boost::dynamic_pointer_cast<urdf::Mesh>(geometry);
+          assert(mesh != NULL); //otherwise implementation error in parser
+          boost::filesystem::path path(mesh->filename);
+          if(!path.is_absolute())
+          {
+            boost::filesystem::path p(urdfPath);
+            p /= path; //append path to urdfPath
+            p = boost::filesystem::canonical(p); //remove symlinks and ".." 
+            mesh->filename = p.generic_string();
+          }
+        }
+      }
+    }
+  }
+  
+  
   boost::shared_ptr<urdf::ModelInterface> parseFile(configmaps::ConfigMap* map,
           std::string path, std::string smurffilename, bool expandURIs) {
 
@@ -55,10 +93,15 @@ namespace smurf_parser {
     // parse URDF model and return
     fprintf(stderr, "  ...loading urdf data from %s.\n", urdfpath.c_str());
     boost::shared_ptr<urdf::ModelInterface> model = urdf::parseURDFFile(urdfpath);
+    
     if (!model) {
       return boost::shared_ptr<urdf::ModelInterface>();
     }
+    
+    //there might be relative paths inside the model. Since we do not return the urdf path 
+    //there is no easy way for the caller to know to which dir the paths are relativ. 
+    //Therefore they are converted to absolut paths.
+    relativeToAbsolut(model, urdfpath);
     return model;
   }
-
 }

From 56b3b9ca55d6eccf16ffa6c8cbd9497adcb1afa7 Mon Sep 17 00:00:00 2001
From: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
Date: Tue, 2 Aug 2016 16:41:15 +0200
Subject: [PATCH 2/2] compatibility with non-boost shared_pointers

---
 src/SMURFParser.cpp | 18 +++++++++---------
 src/SMURFParser.h   |  4 ++--
 2 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/src/SMURFParser.cpp b/src/SMURFParser.cpp
index a464395..c96fcec 100644
--- a/src/SMURFParser.cpp
+++ b/src/SMURFParser.cpp
@@ -32,7 +32,7 @@ namespace smurf_parser {
 
   
   /**Replaces relative paths inside the urdf model with absolut paths. */
-  void relativeToAbsolut(boost::shared_ptr<urdf::ModelInterface> model,
+  void relativeToAbsolut(urdf::ModelInterfaceSharedPtr model,
                          const std::string& urdfPathStr)
   {
     assert(urdfPathStr.size() > 0);
@@ -42,17 +42,17 @@ namespace smurf_parser {
     boost::filesystem::path urdfPath(urdfPathStr);
     urdfPath.remove_filename(); 
      
-    std::map<std::string, boost::shared_ptr<urdf::Link> >::iterator it;
+    std::map<std::string, urdf::LinkSharedPtr >::iterator it;
     for(it = model->links_.begin(); it != model->links_.end(); ++it)
     {
-      std::vector<boost::shared_ptr<urdf::Visual> >& visuals = it->second->visual_array;
-      std::vector<boost::shared_ptr<urdf::Visual> >::iterator visIt;
+      std::vector<urdf::VisualSharedPtr>& visuals = it->second->visual_array;
+      std::vector<urdf::VisualSharedPtr>::iterator visIt;
       for(visIt = visuals.begin(); visIt != visuals.end(); ++visIt)
       {
-        boost::shared_ptr<urdf::Geometry> geometry = (*visIt)->geometry;
+        urdf::GeometrySharedPtr geometry = (*visIt)->geometry;
         if(geometry->type == urdf::Geometry::MESH)
         {
-          boost::shared_ptr<urdf::Mesh> mesh = boost::dynamic_pointer_cast<urdf::Mesh>(geometry);
+          urdf::MeshSharedPtr mesh = urdf::dynamic_pointer_cast<urdf::Mesh>(geometry);
           assert(mesh != NULL); //otherwise implementation error in parser
           boost::filesystem::path path(mesh->filename);
           if(!path.is_absolute())
@@ -68,7 +68,7 @@ namespace smurf_parser {
   }
   
   
-  boost::shared_ptr<urdf::ModelInterface> parseFile(configmaps::ConfigMap* map,
+  urdf::ModelInterfaceSharedPtr parseFile(configmaps::ConfigMap* map,
           std::string path, std::string smurffilename, bool expandURIs) {
 
     path+="/"; // secure that path and file are combined correctly
@@ -92,10 +92,10 @@ namespace smurf_parser {
 
     // parse URDF model and return
     fprintf(stderr, "  ...loading urdf data from %s.\n", urdfpath.c_str());
-    boost::shared_ptr<urdf::ModelInterface> model = urdf::parseURDFFile(urdfpath);
+    urdf::ModelInterfaceSharedPtr model = urdf::parseURDFFile(urdfpath);
     
     if (!model) {
-      return boost::shared_ptr<urdf::ModelInterface>();
+      return urdf::ModelInterfaceSharedPtr();
     }
     
     //there might be relative paths inside the model. Since we do not return the urdf path 
diff --git a/src/SMURFParser.h b/src/SMURFParser.h
index 8c8f056..97e1815 100644
--- a/src/SMURFParser.h
+++ b/src/SMURFParser.h
@@ -30,12 +30,12 @@
 #endif
 
 #include <boost/function.hpp>
-#include <urdf_model/model.h>
+#include <urdf_world/types.h>
 #include <configmaps/ConfigData.h>
 
 namespace smurf_parser {
 
-    boost::shared_ptr<urdf::ModelInterface> parseFile(configmaps::ConfigMap* map,
+    urdf::ModelInterfaceSharedPtr parseFile(configmaps::ConfigMap* map,
       std::string path, std::string smurffilename, bool expandURIs);
 
 } // end of namespace smurf_parser