diff --git a/src/SMURFParser.cpp b/src/SMURFParser.cpp index 1cbe804..c96fcec 100644 --- a/src/SMURFParser.cpp +++ b/src/SMURFParser.cpp @@ -30,7 +30,45 @@ namespace smurf_parser { - boost::shared_ptr parseFile(configmaps::ConfigMap* map, + + /**Replaces relative paths inside the urdf model with absolut paths. */ + void relativeToAbsolut(urdf::ModelInterfaceSharedPtr 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::iterator it; + for(it = model->links_.begin(); it != model->links_.end(); ++it) + { + std::vector& visuals = it->second->visual_array; + std::vector::iterator visIt; + for(visIt = visuals.begin(); visIt != visuals.end(); ++visIt) + { + urdf::GeometrySharedPtr geometry = (*visIt)->geometry; + if(geometry->type == urdf::Geometry::MESH) + { + urdf::MeshSharedPtr mesh = urdf::dynamic_pointer_cast(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(); + } + } + } + } + } + + + urdf::ModelInterfaceSharedPtr parseFile(configmaps::ConfigMap* map, std::string path, std::string smurffilename, bool expandURIs) { path+="/"; // secure that path and file are combined correctly @@ -54,11 +92,16 @@ namespace smurf_parser { // parse URDF model and return fprintf(stderr, " ...loading urdf data from %s.\n", urdfpath.c_str()); - boost::shared_ptr model = urdf::parseURDFFile(urdfpath); + urdf::ModelInterfaceSharedPtr model = urdf::parseURDFFile(urdfpath); + if (!model) { - return boost::shared_ptr(); + return urdf::ModelInterfaceSharedPtr(); } + + //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; } - } 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 -#include +#include #include namespace smurf_parser { - boost::shared_ptr parseFile(configmaps::ConfigMap* map, + urdf::ModelInterfaceSharedPtr parseFile(configmaps::ConfigMap* map, std::string path, std::string smurffilename, bool expandURIs); } // end of namespace smurf_parser