diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index a975cc7bc4..7311d092e8 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -42,6 +43,23 @@ namespace ignition /// configuration. class IGNITION_GAZEBO_VISIBLE ServerConfig { + /// \brief Type of SDF source. + public: enum class SourceType + { + // No source specified. + kNone, + + // The source is an SDF Root object. + kSdfRoot, + + // The source is an SDF file. + kSdfFile, + + // The source is an SDF string. + kSdfString, + }; + + class PluginInfoPrivate; /// \brief Information about a plugin that should be loaded by the /// server. @@ -175,6 +193,17 @@ namespace ignition /// \return The full contents of the SDF string, or empty string. public: std::string SdfString() const; + /// \brief Set the SDF Root DOM object. The sdf::Root object will take + /// precendence over ServerConfig::SdfString() and + /// ServerConfig::SdfFile(). + /// \param[in] _root SDF Root object to use. + public: void SetSdfRoot(const sdf::Root &_root) const; + + /// \brief Get the SDF Root DOM object. + /// \return SDF Root object to use, or std::nullopt if the sdf::Root + /// has not been set via ServerConfig::SetSdfRoot(). + public: std::optional &SdfRoot() const; + /// \brief Set the update rate in Hertz. Value <=0 are ignored. /// \param[in] _hz The desired update rate of the server in Hertz. public: void SetUpdateRate(const double &_hz); @@ -383,6 +412,10 @@ namespace ignition public: const std::chrono::time_point & Timestamp() const; + /// \brief Get the type of source + /// \return The source type. + public: SourceType Source() const; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 67e405659f..18cc6798c9 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -210,6 +210,25 @@ namespace ignition const EntityComponentManager &_ecm, bool _excludeWorld = true); + /// \brief Convert an SDF world filename string, such as "shapes.sdf", to + /// full system file path. + /// The provided SDF filename may be a Fuel URI, relative path, name + /// of an installed Gazebo world filename, or an absolute path. + /// \param[in] _sdfFile An SDF world filename such as: + /// 1. "shapes.sdf" - This is referencing an installed world file. + /// 2. "../shapes.sdf" - This is referencing a relative world file. + /// 3. "/home/user/shapes.sdf" - This is reference an absolute world + /// file. + /// 4. "https://fuel.ignitionrobotics.org/1.0/openrobotics/worlds/shapes.sdf" + /// This is referencing a Fuel URI. This will download the world file. + /// \param[in] _fuelResourceCache Path to a Fuel resource cache, if + /// known. + /// \return Full path to the SDF world file. An empty string is returned + /// if the file could not be found. + std::string IGNITION_GAZEBO_VISIBLE resolveSdfWorldFile( + const std::string &_sdfFilename, + const std::string &_fuelResourceCache = ""); + /// \brief Helper function to "enable" a component (i.e. create it if it /// doesn't exist) or "disable" a component (i.e. remove it if it exists). /// \param[in] _ecm Mutable reference to the ECM diff --git a/src/Server.cc b/src/Server.cc index 9b2a2b8448..87381ee516 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -33,31 +33,6 @@ using namespace ignition; using namespace gazebo; -////////////////////////////////////////////////// -// Getting the first .sdf file in the path -std::string findFuelResourceSdf(const std::string &_path) -{ - if (!common::exists(_path)) - return ""; - - for (common::DirIter file(_path); file != common::DirIter(); ++file) - { - std::string current(*file); - if (!common::isFile(current)) - continue; - - auto fileName = common::basename(current); - auto fileExtensionIndex = fileName.rfind("."); - auto fileExtension = fileName.substr(fileExtensionIndex + 1); - - if (fileExtension == "sdf") - { - return current; - } - } - return ""; -} - /// \brief This struct provides access to the default world. struct DefaultWorld { @@ -98,83 +73,64 @@ Server::Server(const ServerConfig &_config) sdf::Errors errors; - // Load a world if specified. Check SDF string first, then SDF file - if (!_config.SdfString().empty()) + switch (_config.Source()) { - std::string msg = "Loading SDF string. "; - if (_config.SdfFile().empty()) + // Load a world if specified. Check SDF string first, then SDF file + case ServerConfig::SourceType::kSdfRoot: { - msg += "File path not available.\n"; + this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); + ignmsg << "Loading SDF world from SDF DOM.\n"; + break; } - else - { - msg += "File path [" + _config.SdfFile() + "].\n"; - } - ignmsg << msg; - errors = this->dataPtr->sdfRoot.LoadSdfString(_config.SdfString()); - } - else if (!_config.SdfFile().empty()) - { - std::string filePath; - // Check Fuel if it's a URL - auto sdfUri = common::URI(_config.SdfFile()); - if (sdfUri.Scheme() == "http" || sdfUri.Scheme() == "https") + case ServerConfig::SourceType::kSdfString: { - std::string fuelCachePath; - if (this->dataPtr->fuelClient->CachedWorld(common::URI(_config.SdfFile()), - fuelCachePath)) + std::string msg = "Loading SDF string. "; + if (_config.SdfFile().empty()) { - filePath = findFuelResourceSdf(fuelCachePath); - } - else if (auto result = this->dataPtr->fuelClient->DownloadWorld( - common::URI(_config.SdfFile()), fuelCachePath)) - { - filePath = findFuelResourceSdf(fuelCachePath); + msg += "File path not available.\n"; } else { - ignwarn << "Fuel couldn't download URL [" << _config.SdfFile() - << "], error: [" << result.ReadableResult() << "]" - << std::endl; + msg += "File path [" + _config.SdfFile() + "].\n"; } + ignmsg << msg; + errors = this->dataPtr->sdfRoot.LoadSdfString(_config.SdfString()); + break; } - if (filePath.empty()) + case ServerConfig::SourceType::kSdfFile: { - common::SystemPaths systemPaths; + std::string filePath = resolveSdfWorldFile(_config.SdfFile(), + _config.ResourceCache()); - // Worlds from environment variable - systemPaths.SetFilePathEnv(kResourcePathEnv); + if (filePath.empty()) + { + ignerr << "Failed to find world [" << _config.SdfFile() << "]" + << std::endl; + return; + } - // Worlds installed with ign-gazebo - systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); + ignmsg << "Loading SDF world file[" << filePath << "].\n"; - filePath = systemPaths.FindFile(_config.SdfFile()); + // \todo(nkoenig) Async resource download. + // This call can block for a long period of time while + // resources are downloaded. Blocking here causes the GUI to block with + // a black screen (search for "Async resource download" in + // 'src/gui_main.cc'. + errors = this->dataPtr->sdfRoot.Load(filePath); + break; } - if (filePath.empty()) + case ServerConfig::SourceType::kNone: + default: { - ignerr << "Failed to find world [" << _config.SdfFile() << "]" - << std::endl; - return; + ignmsg << "Loading default world.\n"; + // Load an empty world. + /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root. + errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World()); + break; } - - ignmsg << "Loading SDF world file[" << filePath << "].\n"; - - // \todo(nkoenig) Async resource download. - // This call can block for a long period of time while - // resources are downloaded. Blocking here causes the GUI to block with - // a black screen (search for "Async resource download" in - // 'src/gui_main.cc'. - errors = this->dataPtr->sdfRoot.Load(filePath); - } - else - { - ignmsg << "Loading default world.\n"; - // Load an empty world. - /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root. - errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World()); } if (!errors.empty()) diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index fb0d9fdace..1024eb5d11 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -301,6 +301,12 @@ class ignition::gazebo::ServerConfigPrivate /// \brief is the headless mode active. public: bool isHeadlessRendering{false}; + + /// \brief Optional SDF root object. + public: std::optional sdfRoot; + + /// \brief Type of source used. + public: ServerConfig::SourceType source{ServerConfig::SourceType::kNone}; }; ////////////////////////////////////////////////// @@ -321,8 +327,10 @@ ServerConfig::~ServerConfig() = default; ////////////////////////////////////////////////// bool ServerConfig::SetSdfFile(const std::string &_file) { + this->dataPtr->source = ServerConfig::SourceType::kSdfFile; this->dataPtr->sdfFile = _file; this->dataPtr->sdfString = ""; + this->dataPtr->sdfRoot = std::nullopt; return true; } @@ -335,8 +343,10 @@ std::string ServerConfig::SdfFile() const ////////////////////////////////////////////////// bool ServerConfig::SetSdfString(const std::string &_sdfString) { + this->dataPtr->source = ServerConfig::SourceType::kSdfString; this->dataPtr->sdfFile = ""; this->dataPtr->sdfString = _sdfString; + this->dataPtr->sdfRoot = std::nullopt; return true; } @@ -697,6 +707,35 @@ const std::vector &ServerConfig::LogRecordTopics() const return this->dataPtr->logRecordTopics; } +///////////////////////////////////////////////// +void ServerConfig::SetSdfRoot(const sdf::Root &_root) const +{ + this->dataPtr->source = ServerConfig::SourceType::kSdfRoot; + this->dataPtr->sdfRoot.emplace(); + + for (uint64_t i = 0; i < _root.WorldCount(); ++i) + { + const sdf::World *world = _root.WorldByIndex(i); + if (world) + this->dataPtr->sdfRoot->AddWorld(*world); + } + + this->dataPtr->sdfFile = ""; + this->dataPtr->sdfString = ""; +} + +///////////////////////////////////////////////// +std::optional &ServerConfig::SdfRoot() const +{ + return this->dataPtr->sdfRoot; +} + +///////////////////////////////////////////////// +ServerConfig::SourceType ServerConfig::Source() const +{ + return this->dataPtr->source; +} + ///////////////////////////////////////////////// void copyElement(sdf::ElementPtr _sdf, const tinyxml2::XMLElement *_xml) { diff --git a/src/ServerConfig_TEST.cc b/src/ServerConfig_TEST.cc index 733a55ef37..bc93120ef2 100644 --- a/src/ServerConfig_TEST.cc +++ b/src/ServerConfig_TEST.cc @@ -228,3 +228,31 @@ TEST(ServerConfig, GenerateRecordPlugin) EXPECT_EQ(plugin.Name(), "ignition::gazebo::systems::LogRecord"); } +////////////////////////////////////////////////// +TEST(ServerConfig, SdfRoot) +{ + ServerConfig config; + EXPECT_FALSE(config.SdfRoot()); + EXPECT_TRUE(config.SdfFile().empty()); + EXPECT_TRUE(config.SdfString().empty()); + EXPECT_EQ(ServerConfig::SourceType::kNone, config.Source()); + + config.SetSdfString("string"); + EXPECT_FALSE(config.SdfRoot()); + EXPECT_TRUE(config.SdfFile().empty()); + EXPECT_FALSE(config.SdfString().empty()); + EXPECT_EQ(ServerConfig::SourceType::kSdfString, config.Source()); + + config.SetSdfFile("file"); + EXPECT_FALSE(config.SdfRoot()); + EXPECT_FALSE(config.SdfFile().empty()); + EXPECT_TRUE(config.SdfString().empty()); + EXPECT_EQ(ServerConfig::SourceType::kSdfFile, config.Source()); + + sdf::Root root; + config.SetSdfRoot(root); + EXPECT_TRUE(config.SdfRoot()); + EXPECT_TRUE(config.SdfFile().empty()); + EXPECT_TRUE(config.SdfString().empty()); + EXPECT_EQ(ServerConfig::SourceType::kSdfRoot, config.Source()); +} diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index 879806b2e3..a2c6d1803d 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -295,6 +295,49 @@ TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(SdfServerConfig)) EXPECT_FALSE(server.HasEntity("bad", 1)); } +///////////////////////////////////////////////// +TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(SdfRootServerConfig)) +{ + ignition::gazebo::ServerConfig serverConfig; + + serverConfig.SetSdfString(TestWorldSansPhysics::World()); + EXPECT_TRUE(serverConfig.SdfFile().empty()); + EXPECT_FALSE(serverConfig.SdfString().empty()); + + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "air_pressure.sdf")); + EXPECT_FALSE(serverConfig.SdfFile().empty()); + EXPECT_TRUE(serverConfig.SdfString().empty()); + + sdf::Root root; + root.Load(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "shapes.sdf")); + + // Setting the SDF Root should override the string and file. + serverConfig.SetSdfRoot(root); + + EXPECT_TRUE(serverConfig.SdfRoot()); + EXPECT_TRUE(serverConfig.SdfFile().empty()); + EXPECT_TRUE(serverConfig.SdfString().empty()); + + gazebo::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + EXPECT_TRUE(*server.Paused()); + EXPECT_EQ(0u, *server.IterationCount()); + EXPECT_EQ(24u, *server.EntityCount()); + EXPECT_EQ(3u, *server.SystemCount()); + + EXPECT_TRUE(server.HasEntity("box")); + EXPECT_FALSE(server.HasEntity("box", 1)); + EXPECT_TRUE(server.HasEntity("sphere")); + EXPECT_TRUE(server.HasEntity("cylinder")); + EXPECT_TRUE(server.HasEntity("capsule")); + EXPECT_TRUE(server.HasEntity("ellipsoid")); + EXPECT_FALSE(server.HasEntity("bad", 0)); + EXPECT_FALSE(server.HasEntity("bad", 1)); +} + ///////////////////////////////////////////////// TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(ServerConfigLogRecord)) { diff --git a/src/Util.cc b/src/Util.cc index e46e1ffb5a..e24718bfac 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -36,6 +36,9 @@ #include #include +#include +#include + #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/Joint.hh" @@ -579,7 +582,80 @@ std::optional sphericalCoordinates(Entity _entity, // Return degrees return math::Vector3d(IGN_RTOD(rad.X()), IGN_RTOD(rad.Y()), rad.Z()); } + +////////////////////////////////////////////////// +// Getting the first .sdf file in the path +std::string findFuelResourceSdf(const std::string &_path) +{ + if (!common::exists(_path)) + return ""; + + for (common::DirIter file(_path); file != common::DirIter(); ++file) + { + std::string current(*file); + if (!common::isFile(current)) + continue; + + auto fileName = common::basename(current); + auto fileExtensionIndex = fileName.rfind("."); + auto fileExtension = fileName.substr(fileExtensionIndex + 1); + + if (fileExtension == "sdf") + { + return current; + } + } + return ""; +} + +////////////////////////////////////////////////// +std::string resolveSdfWorldFile(const std::string &_sdfFile, + const std::string &_fuelResourceCache) +{ + std::string filePath; + + // Check Fuel if it's a URL + auto sdfUri = common::URI(_sdfFile); + if (sdfUri.Scheme() == "http" || sdfUri.Scheme() == "https") + { + fuel_tools::ClientConfig config; + if (!_fuelResourceCache.empty()) + config.SetCacheLocation(_fuelResourceCache); + fuel_tools::FuelClient fuelClient(config); + + std::string fuelCachePath; + if (fuelClient.CachedWorld(common::URI(_sdfFile), fuelCachePath)) + { + filePath = findFuelResourceSdf(fuelCachePath); + } + else if (auto result = fuelClient.DownloadWorld( + common::URI(_sdfFile), fuelCachePath)) + { + filePath = findFuelResourceSdf(fuelCachePath); + } + else + { + ignwarn << "Fuel couldn't download URL [" << _sdfFile + << "], error: [" << result.ReadableResult() << "]" + << std::endl; + } + } + + if (filePath.empty()) + { + common::SystemPaths systemPaths; + + // Worlds from environment variable + systemPaths.SetFilePathEnv(kResourcePathEnv); + + // Worlds installed with ign-gazebo + systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); + + filePath = systemPaths.FindFile(_sdfFile); + } + + return filePath; +} } } } - diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index ac83bf2643..3f2b4443a9 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -21,6 +21,8 @@ #include #include +#include + #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/Joint.hh" @@ -716,3 +718,42 @@ TEST_F(UtilTest, EnableComponent) EXPECT_FALSE(enableComponent(ecm, entity1, false)); EXPECT_EQ(nullptr, ecm.Component(entity1)); } + +///////////////////////////////////////////////// +TEST_F(UtilTest, ResolveSdfWorldFile) +{ + // Test resolving a Fuel URI + fuel_tools::ClientConfig config; + + // URI to a Fuel world. + std::string fuelUri = + "https://fuel.ignitionrobotics.org/1.0/openrobotics/worlds/test world"; + + // The expect path for the local Fuel world. + std::string expectedPath = common::joinPaths( + config.CacheLocation(), "fuel.ignitionrobotics.org", + "openrobotics", "worlds", "test world"); + + // Get the Fuel world. + std::string resolvedPath = resolveSdfWorldFile(fuelUri, + config.CacheLocation()); + + // The Fuel model has not been downloaded, so it should not exist. + EXPECT_FALSE(resolvedPath.empty()); + + // The expected path should be the first part of the resolved path. The + // resolved path will have extra world version information at the end. + EXPECT_EQ(0u, resolvedPath.find(expectedPath)); + + // Now try to resolve the downloaded world file using an aboslute path + EXPECT_EQ(resolvedPath, resolveSdfWorldFile(resolvedPath)); + + // The "shapes.sdf" world file should resolve. + EXPECT_FALSE(resolveSdfWorldFile("shapes.sdf").empty()); + + // A bad absolute path should return an empty string + EXPECT_TRUE(resolveSdfWorldFile("/invalid/does_not_exist.sdf").empty()); + + // A bad relative path should return an empty string + EXPECT_TRUE(resolveSdfWorldFile("../invalid/does_not_exist.sdf").empty()); +}