diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index 635a44294..f32a21b18 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -215,6 +215,10 @@ class SDFORMAT_VISIBLE ParserConfig /// and merge the child link into the parent. public: bool URDFPreserveFixedJoint() const; + /// \brief Set the storeResolvedURIs flag value. + /// \sa SetStoreResolvedURIs + public: void SetStoreResovledURIs(bool _resolveURI); + /// \brief Set the storeResolvedURIs flag value. /// \param[in] _resolveURI True to make the parser attempt to resolve any /// URIs found and store them. False to preserve original URIs @@ -224,7 +228,7 @@ class SDFORMAT_VISIBLE ParserConfig /// If the FindFileCallback provides a non-empty string, the URI will be /// stored in the DOM object, and the original (unresolved) URI will be /// stored in the underlying Element. - public: void SetStoreResovledURIs(bool _resolveURI); + public: void SetStoreResolvedURIs(bool _resolveURI); /// \brief Get the storeResolvedURIs flag value. /// \return True if the parser will attempt to resolve any URIs found and diff --git a/src/Heightmap.cc b/src/Heightmap.cc index 5c7b1a8b6..18433c11e 100644 --- a/src/Heightmap.cc +++ b/src/Heightmap.cc @@ -15,6 +15,7 @@ * */ +#include #include #include "Utils.hh" @@ -133,9 +134,15 @@ Errors HeightmapTexture::Load(ElementPtr _sdf, const ParserConfig &_config) if (_sdf->HasElement("diffuse")) { + std::unordered_set paths; + if (!this->dataPtr->sdf->FilePath().empty()) + { + paths.insert(std::filesystem::path( + this->dataPtr->sdf->FilePath()).parent_path().string()); + } this->dataPtr->diffuse = resolveURI( _sdf->Get(errors, "diffuse", this->dataPtr->diffuse).first, - _config, errors); + _config, errors, paths); } else { @@ -145,9 +152,15 @@ Errors HeightmapTexture::Load(ElementPtr _sdf, const ParserConfig &_config) if (_sdf->HasElement("normal")) { + std::unordered_set paths; + if (!this->dataPtr->sdf->FilePath().empty()) + { + paths.insert(std::filesystem::path( + this->dataPtr->sdf->FilePath()).parent_path().string()); + } this->dataPtr->normal = resolveURI( _sdf->Get(errors, "normal", this->dataPtr->normal).first, - _config, errors); + _config, errors, paths); } else { @@ -326,9 +339,15 @@ Errors Heightmap::Load(ElementPtr _sdf, const ParserConfig &_config) if (_sdf->HasElement("uri")) { + std::unordered_set paths; + if (!this->dataPtr->filePath.empty()) + { + paths.insert(std::filesystem::path( + this->dataPtr->filePath).parent_path().string()); + } this->dataPtr->uri = resolveURI( _sdf->Get(errors, "uri", "").first, - _config, errors); + _config, errors, paths); } else { diff --git a/src/Material.cc b/src/Material.cc index 0a0b77c42..6a0456686 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -14,8 +14,10 @@ * limitations under the License. * */ -#include + +#include #include +#include #include #include @@ -122,7 +124,14 @@ Errors Material::Load(sdf::ElementPtr _sdf, const sdf::ParserConfig &_config) " element is empty."}); } - this->dataPtr->scriptUri = resolveURI(uriPair.first, _config, errors); + std::unordered_set paths; + if (!this->dataPtr->filePath.empty()) + { + paths.insert(std::filesystem::path( + this->dataPtr->filePath).parent_path().string()); + } + this->dataPtr->scriptUri = resolveURI(uriPair.first, _config, errors, + paths); std::pair namePair = elem->Get(errors, "name", ""); diff --git a/src/Mesh.cc b/src/Mesh.cc index fa180b2bb..9dfddcf2b 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -14,7 +14,7 @@ * limitations under the License. * */ - +#include #include #include @@ -89,9 +89,15 @@ Errors Mesh::Load(ElementPtr _sdf, const ParserConfig &_config) if (_sdf->HasElement("uri")) { + std::unordered_set paths; + if (!this->dataPtr->filePath.empty()) + { + paths.insert(std::filesystem::path( + this->dataPtr->filePath).parent_path().string()); + } this->dataPtr->uri = resolveURI( _sdf->Get(errors, "uri", "").first, - _config, errors); + _config, errors, paths); } else { diff --git a/src/ParserConfig.cc b/src/ParserConfig.cc index 32211f812..d3c60e7af 100644 --- a/src/ParserConfig.cc +++ b/src/ParserConfig.cc @@ -220,6 +220,12 @@ bool ParserConfig::URDFPreserveFixedJoint() const ///////////////////////////////////////////////// void ParserConfig::SetStoreResovledURIs(bool _resolveURI) +{ + this->SetStoreResolvedURIs(_resolveURI); +} + +///////////////////////////////////////////////// +void ParserConfig::SetStoreResolvedURIs(bool _resolveURI) { this->dataPtr->storeResolvedURIs = _resolveURI; } @@ -229,4 +235,3 @@ bool ParserConfig::StoreResolvedURIs() const { return this->dataPtr->storeResolvedURIs; } - diff --git a/src/Sky.cc b/src/Sky.cc index 672748ff8..ba46f209b 100644 --- a/src/Sky.cc +++ b/src/Sky.cc @@ -14,6 +14,8 @@ * limitations under the License. * */ +#include + #include "sdf/parser.hh" #include "sdf/Sky.hh" #include "Utils.hh" @@ -201,9 +203,15 @@ Errors Sky::Load(ElementPtr _sdf, const ParserConfig &_config) if (_sdf->HasElement("cubemap_uri")) { + std::unordered_set paths; + if (!_sdf->FilePath().empty()) + { + paths.insert(std::filesystem::path( + _sdf->FilePath()).parent_path().string()); + } this->dataPtr->cubemapUri = resolveURI( _sdf->Get(errors, "cubemap_uri", "").first, - _config, errors); + _config, errors, paths); } if ( _sdf->HasElement("clouds")) diff --git a/src/Utils.cc b/src/Utils.cc index 31a192b75..c6a89e867 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -14,10 +14,12 @@ * limitations under the License. * */ +#include #include #include #include #include "sdf/Assert.hh" +#include "sdf/Filesystem.hh" #include "sdf/SDFImpl.hh" #include "Utils.hh" @@ -382,11 +384,28 @@ void copyChildren(ElementPtr _sdf, tinyxml2::XMLElement *_xml, ///////////////////////////////////////////////// std::string resolveURI(const std::string &_inputURI, - const sdf::ParserConfig &_config, sdf::Errors &_errors) + const sdf::ParserConfig &_config, sdf::Errors &_errors, + const std::unordered_set &_searchPaths) { std::string resolvedURI = _inputURI; if (_config.StoreResolvedURIs()) { + std::string sep("://"); + if (!_searchPaths.empty() && + _inputURI.find(sep) == std::string::npos && + !std::filesystem::path(_inputURI).is_absolute()) + { + for (const auto &sp : _searchPaths) + { + // find file by searching local path but do not use callbacks + std::string fullPath = sdf::filesystem::append(sp, _inputURI); + resolvedURI = sdf::findFile(fullPath, true, false); + if (!resolvedURI.empty()) + return resolvedURI; + } + } + + // find file by searching local path and use registered callbacks resolvedURI = sdf::findFile(_inputURI, true, true, _config); if (resolvedURI.empty()) { diff --git a/src/Utils.hh b/src/Utils.hh index 18898eadd..7a253aa34 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -23,6 +23,7 @@ #include #include #include +#include #include "sdf/Error.hh" #include "sdf/Element.hh" #include "sdf/InterfaceElements.hh" @@ -262,10 +263,14 @@ namespace sdf /// \param[in] _inputURI URI from parsed SDF file to resolve /// \param[in] _config Parser configuration to use to resolve /// \param[in, out] _errors Error vector to append to if resolution fails + /// \param[in] _searchPaths Optional additional search paths (directory) + /// when resolving URI /// \return Resolved URI or Original URI, depending on parser configuration std::string resolveURI(const std::string &_inputURI, const sdf::ParserConfig &_config, - sdf::Errors &_errors); + sdf::Errors &_errors, + const std::unordered_set + &_searchPaths = {}); } } #endif diff --git a/test/integration/model/double_pendulum.sdf b/test/integration/model/double_pendulum.sdf index 0d6890ceb..c8de2e0ff 100644 --- a/test/integration/model/double_pendulum.sdf +++ b/test/integration/model/double_pendulum.sdf @@ -16,7 +16,7 @@ @@ -28,7 +28,7 @@ @@ -96,7 +96,7 @@ @@ -146,7 +146,7 @@ diff --git a/test/integration/resolve_uris.cc b/test/integration/resolve_uris.cc index e7d2a67d2..f7273e5e5 100644 --- a/test/integration/resolve_uris.cc +++ b/test/integration/resolve_uris.cc @@ -15,6 +15,7 @@ * */ +#include #include #include @@ -59,7 +60,7 @@ TEST(ResolveURIs, StoreResolvedDisabled) const std::string sdfDir = sdf::testing::TestFile("sdf"); sdf::ParserConfig config; - config.SetStoreResovledURIs(false); + config.SetStoreResolvedURIs(false); EXPECT_FALSE(config.StoreResolvedURIs()); size_t callbackCount = 0; @@ -175,7 +176,7 @@ TEST(ResolveURIs, StoreResolvedEnabled) const std::string sdfDir = sdf::testing::TestFile("sdf"); sdf::ParserConfig config; - config.SetStoreResovledURIs(true); + config.SetStoreResolvedURIs(true); EXPECT_TRUE(config.StoreResolvedURIs()); size_t callbackCount = 0; @@ -301,7 +302,7 @@ TEST(ResolveURIs, BadCallback) const std::string sdfDir = sdf::testing::TestFile("sdf"); sdf::ParserConfig config; - config.SetStoreResovledURIs(true); + config.SetStoreResolvedURIs(true); EXPECT_TRUE(config.StoreResolvedURIs()); size_t callbackCount = 0; @@ -418,3 +419,151 @@ TEST(ResolveURIs, BadCallback) EXPECT_EQ(kDiffuseUri, diffuseElemUri); EXPECT_EQ(kNormalUri, normalElemUri); } + +///////////////////////////////////////////////// +TEST(ResolveURIs, ResolvedRelativeURIs) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "resolve_relative_uris.sdf"); + const std::string sdfDir = sdf::testing::TestFile("sdf"); + + sdf::ParserConfig config; + config.SetStoreResolvedURIs(true); + EXPECT_TRUE(config.StoreResolvedURIs()); + + size_t callbackCount = 0; + config.SetFindCallback( + [=, &callbackCount](const std::string &) + { + callbackCount++; + return ""; + }); + + sdf::Root root; + auto errors = root.Load(testFile, config); + EXPECT_TRUE(errors.empty()); + // no user callbacks should be invoked + EXPECT_EQ(0, callbackCount); + + auto world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + // Skybox Cubemap Uri + auto sky = world->Scene()->Sky(); + auto cubemapUri = sky->CubemapUri(); + + // Visual mesh and material script uri + auto model = world->ModelByName("model"); + ASSERT_NE(nullptr, model); + auto link = model->LinkByName("link"); + ASSERT_NE(nullptr, link); + auto visual = link->VisualByName("visual"); + ASSERT_NE(nullptr, visual); + auto meshVisualUri = visual->Geom()->MeshShape()->Uri(); + auto scriptUri = visual->Material()->ScriptUri(); + + // Collision mesh uri + auto meshCol = link->CollisionByName("collision"); + ASSERT_NE(nullptr, meshCol); + auto meshColUri = meshCol->Geom()->MeshShape()->Uri(); + + EXPECT_NE(std::string::npos, cubemapUri.find("dummy_cubemap.dds")); + EXPECT_NE(std::string::npos, scriptUri.find("dummy_material.material")); + EXPECT_NE(std::string::npos, meshVisualUri.find("dummy_mesh.dae")); + EXPECT_NE(std::string::npos, meshColUri.find("dummy_mesh.dae")); + EXPECT_EQ(meshVisualUri, meshColUri); + + EXPECT_TRUE(std::filesystem::path(cubemapUri).is_absolute()); + EXPECT_TRUE(std::filesystem::path(scriptUri).is_absolute()); + EXPECT_TRUE(std::filesystem::path(meshVisualUri).is_absolute()); + EXPECT_TRUE(std::filesystem::path(meshColUri).is_absolute()); + + EXPECT_TRUE(sdf::filesystem::exists(cubemapUri)); + EXPECT_TRUE(sdf::filesystem::exists(scriptUri)); + EXPECT_TRUE(sdf::filesystem::exists(meshVisualUri)); + EXPECT_TRUE(sdf::filesystem::exists(meshColUri)); +} + +///////////////////////////////////////////////// +TEST(ResolveURIs, ResolvedRelativeURIsSdfString) +{ + const std::string testSdfString = R"( + + + + + + + media/dummy_mesh.dae + + + + + false + + + media/dummy_mesh.dae + + + + + + + + + )"; + + sdf::ParserConfig config; + config.SetStoreResolvedURIs(true); + EXPECT_TRUE(config.StoreResolvedURIs()); + + const std::string kTestMediaPath = sdf::testing::TestFile("sdf", "media"); + size_t callbackCount = 0; + config.SetFindCallback( + [=, &callbackCount](const std::string &_file) + { + std::string path = _file; + std::string media = "media"; + path.replace(path.find(media), media.length(), kTestMediaPath); + callbackCount++; + return path; + }); + + sdf::Root root; + auto errors = root.LoadSdfString(testSdfString, config); + EXPECT_TRUE(errors.empty()); + // callbacks should be invoked for all relative uris + EXPECT_EQ(3, callbackCount); + + auto model = root.Model(); + ASSERT_NE(nullptr, model); + + // Visual mesh and material script uri + auto link = model->LinkByName("link"); + ASSERT_NE(nullptr, link); + auto visual = link->VisualByName("visual"); + ASSERT_NE(nullptr, visual); + auto meshVisualUri = visual->Geom()->MeshShape()->Uri(); + auto scriptUri = visual->Material()->ScriptUri(); + + // Collision mesh uri + auto meshCol = link->CollisionByName("collision"); + ASSERT_NE(nullptr, meshCol); + auto meshColUri = meshCol->Geom()->MeshShape()->Uri(); + + EXPECT_NE(std::string::npos, scriptUri.find("dummy_material.material")); + EXPECT_NE(std::string::npos, meshVisualUri.find("dummy_mesh.dae")); + EXPECT_NE(std::string::npos, meshColUri.find("dummy_mesh.dae")); + EXPECT_EQ(meshVisualUri, meshColUri); + + EXPECT_TRUE(std::filesystem::path(scriptUri).is_absolute()); + EXPECT_TRUE(std::filesystem::path(meshVisualUri).is_absolute()); + EXPECT_TRUE(std::filesystem::path(meshColUri).is_absolute()); + + EXPECT_TRUE(sdf::filesystem::exists(scriptUri)); + EXPECT_TRUE(sdf::filesystem::exists(meshVisualUri)); + EXPECT_TRUE(sdf::filesystem::exists(meshColUri)); +} diff --git a/test/sdf/media/dummy_cubemap.dds b/test/sdf/media/dummy_cubemap.dds new file mode 100644 index 000000000..e69de29bb diff --git a/test/sdf/media/dummy_material.material b/test/sdf/media/dummy_material.material new file mode 100644 index 000000000..e69de29bb diff --git a/test/sdf/media/dummy_mesh.dae b/test/sdf/media/dummy_mesh.dae new file mode 100644 index 000000000..e69de29bb diff --git a/test/sdf/resolve_relative_uris.sdf b/test/sdf/resolve_relative_uris.sdf new file mode 100644 index 000000000..642db70d5 --- /dev/null +++ b/test/sdf/resolve_relative_uris.sdf @@ -0,0 +1,54 @@ + + + + + + 0.3 0.4 0.5 + 0.6 0.7 0.8 + true + true + true + + + 4 + 21 + + 1.2 + 1.5 + 0.2 + 0.9 + 0.1 0.2 0.3 + + media/dummy_cubemap.dds + + + + + true + + + + + media/dummy_mesh.dae + + + + + false + + + media/dummy_mesh.dae + + + + + + + + + + +