From 0896ac1dcc2ae3f847f9d8f103f03a30a5f38ec1 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 10 Feb 2022 20:59:04 -0800 Subject: [PATCH 1/5] Added more tests Signed-off-by: Nate Koenig --- include/ignition/gazebo/ServerConfig.hh | 12 ++++ include/ignition/gazebo/Util.hh | 18 ++++++ src/Server.cc | 71 +++-------------------- src/ServerConfig.cc | 27 +++++++++ src/ServerConfig_TEST.cc | 24 ++++++++ src/Server_TEST.cc | 43 ++++++++++++++ src/Util.cc | 77 +++++++++++++++++++++++++ src/Util_TEST.cc | 42 ++++++++++++++ 8 files changed, 251 insertions(+), 63 deletions(-) diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index a975cc7bc4..663459c6b6 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -175,6 +176,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: const 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); diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 67e405659f..9dcd4b273a 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -210,6 +210,24 @@ 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..751faa2021 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 { @@ -99,7 +74,12 @@ Server::Server(const ServerConfig &_config) sdf::Errors errors; // Load a world if specified. Check SDF string first, then SDF file - if (!_config.SdfString().empty()) + if (_config.SdfRoot()) + { + this->dataPtr->sdfRoot.Clone(*_config.SdfRoot()); + ignmsg << "Loading SDF world from SDF DOM.\n"; + } + else if (!_config.SdfString().empty()) { std::string msg = "Loading SDF string. "; if (_config.SdfFile().empty()) @@ -115,43 +95,8 @@ Server::Server(const ServerConfig &_config) } 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") - { - std::string fuelCachePath; - if (this->dataPtr->fuelClient->CachedWorld(common::URI(_config.SdfFile()), - fuelCachePath)) - { - filePath = findFuelResourceSdf(fuelCachePath); - } - else if (auto result = this->dataPtr->fuelClient->DownloadWorld( - common::URI(_config.SdfFile()), fuelCachePath)) - { - filePath = findFuelResourceSdf(fuelCachePath); - } - else - { - ignwarn << "Fuel couldn't download URL [" << _config.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(_config.SdfFile()); - } + std::string filePath = resolveSdfWorldFile(_config.SdfFile(), + _config.ResourceCache()); if (filePath.empty()) { diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index fb0d9fdace..99eb6550aa 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -301,6 +301,9 @@ class ignition::gazebo::ServerConfigPrivate /// \brief is the headless mode active. public: bool isHeadlessRendering{false}; + + /// \brief Optional SDF root object. + public: std::optional sdfRoot; }; ////////////////////////////////////////////////// @@ -323,6 +326,7 @@ bool ServerConfig::SetSdfFile(const std::string &_file) { this->dataPtr->sdfFile = _file; this->dataPtr->sdfString = ""; + this->dataPtr->sdfRoot = std::nullopt; return true; } @@ -337,6 +341,7 @@ bool ServerConfig::SetSdfString(const std::string &_sdfString) { this->dataPtr->sdfFile = ""; this->dataPtr->sdfString = _sdfString; + this->dataPtr->sdfRoot = std::nullopt; return true; } @@ -697,6 +702,28 @@ const std::vector &ServerConfig::LogRecordTopics() const return this->dataPtr->logRecordTopics; } +///////////////////////////////////////////////// +void ServerConfig::SetSdfRoot(const sdf::Root &_root) const +{ + 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 = ""; +} + +///////////////////////////////////////////////// +const std::optional &ServerConfig::SdfRoot() const +{ + return this->dataPtr->sdfRoot; +} + ///////////////////////////////////////////////// void copyElement(sdf::ElementPtr _sdf, const tinyxml2::XMLElement *_xml) { diff --git a/src/ServerConfig_TEST.cc b/src/ServerConfig_TEST.cc index 733a55ef37..8486b2d70d 100644 --- a/src/ServerConfig_TEST.cc +++ b/src/ServerConfig_TEST.cc @@ -228,3 +228,27 @@ 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()); + + config.SetSdfString("string"); + EXPECT_FALSE(config.SdfRoot()); + EXPECT_TRUE(config.SdfFile().empty()); + EXPECT_FALSE(config.SdfString().empty()); + + config.SetSdfFile("file"); + EXPECT_FALSE(config.SdfRoot()); + EXPECT_FALSE(config.SdfFile().empty()); + EXPECT_TRUE(config.SdfString().empty()); + + sdf::Root root; + config.SetSdfRoot(root); + EXPECT_TRUE(config.SdfRoot()); + EXPECT_TRUE(config.SdfFile().empty()); + EXPECT_TRUE(config.SdfString().empty()); +} diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index 2c11a7697e..2c5ea8027f 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -292,6 +292,49 @@ TEST_P(ServerFixture, SdfServerConfig) EXPECT_FALSE(server.HasEntity("bad", 1)); } +///////////////////////////////////////////////// +TEST_P(ServerFixture, 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, ServerConfigLogRecord) { diff --git a/src/Util.cc b/src/Util.cc index ec6efd57dc..39a3f16303 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,6 +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 IGNITION_GAZEBO_VISIBLE 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..388661193e 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" @@ -37,6 +39,7 @@ #include "ignition/gazebo/Util.hh" #include "helpers/EnvTestFixture.hh" +#include "helpers/UniqueTestDirectoryEnv.hh" using namespace ignition; using namespace gazebo; @@ -716,3 +719,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()); +} From 2770692dca44be7b8b3ba5ae3fd005c542840283 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Feb 2022 08:42:36 -0800 Subject: [PATCH 2/5] Change clone Signed-off-by: Nate Koenig --- src/Server.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Server.cc b/src/Server.cc index 751faa2021..fbb65fb2f5 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -76,7 +76,7 @@ Server::Server(const ServerConfig &_config) // Load a world if specified. Check SDF string first, then SDF file if (_config.SdfRoot()) { - this->dataPtr->sdfRoot.Clone(*_config.SdfRoot()); + this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); ignmsg << "Loading SDF world from SDF DOM.\n"; } else if (!_config.SdfString().empty()) From f32555b2884167d7da28613229f447dbfaace483 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Feb 2022 13:35:24 -0800 Subject: [PATCH 3/5] Address review comments Signed-off-by: Nate Koenig --- include/ignition/gazebo/ServerConfig.hh | 22 ++++++- include/ignition/gazebo/Util.hh | 3 +- src/Server.cc | 87 ++++++++++++++----------- src/ServerConfig.cc | 14 +++- src/Util_TEST.cc | 1 - 5 files changed, 85 insertions(+), 42 deletions(-) diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index 663459c6b6..bdc1ce277f 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -37,6 +37,22 @@ namespace ignition // Forward declarations. class ServerConfigPrivate; + /// \brief Type of SDF source. + enum class SourceType + { + // The source is an SDF file. + kSdfFile, + + // No source specified. + kNone, + + // The source is an SDF Root object. + kSdfRoot, + + // The source is an SDF string. + kSdfString, + }; + /// \class ServerConfig ServerConfig.hh ignition/gazebo/ServerConfig.hh /// \brief Configuration parameters for a Server. An instance of this /// object can be used to construct a Server with a particular @@ -185,7 +201,7 @@ namespace ignition /// \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: const std::optional &SdfRoot() const; + 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. @@ -395,6 +411,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 9dcd4b273a..18cc6798c9 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -219,7 +219,8 @@ namespace ignition /// 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. + /// 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 diff --git a/src/Server.cc b/src/Server.cc index fbb65fb2f5..eee8862f41 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -73,53 +73,64 @@ Server::Server(const ServerConfig &_config) sdf::Errors errors; - // Load a world if specified. Check SDF string first, then SDF file - if (_config.SdfRoot()) + switch (_config.Source()) { - this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); - ignmsg << "Loading SDF world from SDF DOM.\n"; - } - else if (!_config.SdfString().empty()) - { - std::string msg = "Loading SDF string. "; - if (_config.SdfFile().empty()) + // Load a world if specified. Check SDF string first, then SDF file + case SourceType::kSdfRoot: { - msg += "File path not available.\n"; + this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); + ignmsg << "Loading SDF world from SDF DOM.\n"; + break; } - else + + case SourceType::kSdfString: { - msg += "File path [" + _config.SdfFile() + "].\n"; + std::string msg = "Loading SDF string. "; + if (_config.SdfFile().empty()) + { + msg += "File path not available.\n"; + } + else + { + msg += "File path [" + _config.SdfFile() + "].\n"; + } + ignmsg << msg; + errors = this->dataPtr->sdfRoot.LoadSdfString(_config.SdfString()); + break; } - ignmsg << msg; - errors = this->dataPtr->sdfRoot.LoadSdfString(_config.SdfString()); - } - else if (!_config.SdfFile().empty()) - { - std::string filePath = resolveSdfWorldFile(_config.SdfFile(), - _config.ResourceCache()); - if (filePath.empty()) + case SourceType::kSdfFile: { - ignerr << "Failed to find world [" << _config.SdfFile() << "]" - << std::endl; - return; + std::string filePath = resolveSdfWorldFile(_config.SdfFile(), + _config.ResourceCache()); + + if (filePath.empty()) + { + ignerr << "Failed to find world [" << _config.SdfFile() << "]" + << std::endl; + return; + } + + 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); + 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()); + case SourceType::kNone: + default: + { + 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; + } } if (!errors.empty()) diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 99eb6550aa..7101d211ba 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -304,6 +304,9 @@ class ignition::gazebo::ServerConfigPrivate /// \brief Optional SDF root object. public: std::optional sdfRoot; + + /// \brief Type of source used. + public: SourceType source{SourceType::kNone}; }; ////////////////////////////////////////////////// @@ -324,6 +327,7 @@ ServerConfig::~ServerConfig() = default; ////////////////////////////////////////////////// bool ServerConfig::SetSdfFile(const std::string &_file) { + this->dataPtr->source = SourceType::kSdfFile; this->dataPtr->sdfFile = _file; this->dataPtr->sdfString = ""; this->dataPtr->sdfRoot = std::nullopt; @@ -339,6 +343,7 @@ std::string ServerConfig::SdfFile() const ////////////////////////////////////////////////// bool ServerConfig::SetSdfString(const std::string &_sdfString) { + this->dataPtr->source = SourceType::kSdfString; this->dataPtr->sdfFile = ""; this->dataPtr->sdfString = _sdfString; this->dataPtr->sdfRoot = std::nullopt; @@ -705,6 +710,7 @@ const std::vector &ServerConfig::LogRecordTopics() const ///////////////////////////////////////////////// void ServerConfig::SetSdfRoot(const sdf::Root &_root) const { + this->dataPtr->source = SourceType::kSdfRoot; this->dataPtr->sdfRoot.emplace(); for (uint64_t i = 0; i < _root.WorldCount(); ++i) @@ -719,11 +725,17 @@ void ServerConfig::SetSdfRoot(const sdf::Root &_root) const } ///////////////////////////////////////////////// -const std::optional &ServerConfig::SdfRoot() const +std::optional &ServerConfig::SdfRoot() const { return this->dataPtr->sdfRoot; } +///////////////////////////////////////////////// +SourceType ServerConfig::Source() const +{ + return this->dataPtr->source; +} + ///////////////////////////////////////////////// void copyElement(sdf::ElementPtr _sdf, const tinyxml2::XMLElement *_xml) { diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 388661193e..3f2b4443a9 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -39,7 +39,6 @@ #include "ignition/gazebo/Util.hh" #include "helpers/EnvTestFixture.hh" -#include "helpers/UniqueTestDirectoryEnv.hh" using namespace ignition; using namespace gazebo; From 7d33cf0aafacefa3a4bdaa1522ce02b7d7b5263b Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Feb 2022 14:17:40 -0800 Subject: [PATCH 4/5] Fix linter, and added tests Signed-off-by: Nate Koenig --- include/ignition/gazebo/ServerConfig.hh | 33 +++++++++++++------------ src/Server.cc | 8 +++--- src/ServerConfig.cc | 10 ++++---- src/ServerConfig_TEST.cc | 4 +++ src/Util.cc | 5 ++-- 5 files changed, 32 insertions(+), 28 deletions(-) diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index bdc1ce277f..7311d092e8 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -37,28 +37,29 @@ namespace ignition // Forward declarations. class ServerConfigPrivate; - /// \brief Type of SDF source. - enum class SourceType - { - // The source is an SDF file. - kSdfFile, - - // No source specified. - kNone, - - // The source is an SDF Root object. - kSdfRoot, - - // The source is an SDF string. - kSdfString, - }; - /// \class ServerConfig ServerConfig.hh ignition/gazebo/ServerConfig.hh /// \brief Configuration parameters for a Server. An instance of this /// object can be used to construct a Server with a particular /// 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. diff --git a/src/Server.cc b/src/Server.cc index eee8862f41..87381ee516 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -76,14 +76,14 @@ Server::Server(const ServerConfig &_config) switch (_config.Source()) { // Load a world if specified. Check SDF string first, then SDF file - case SourceType::kSdfRoot: + case ServerConfig::SourceType::kSdfRoot: { this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); ignmsg << "Loading SDF world from SDF DOM.\n"; break; } - case SourceType::kSdfString: + case ServerConfig::SourceType::kSdfString: { std::string msg = "Loading SDF string. "; if (_config.SdfFile().empty()) @@ -99,7 +99,7 @@ Server::Server(const ServerConfig &_config) break; } - case SourceType::kSdfFile: + case ServerConfig::SourceType::kSdfFile: { std::string filePath = resolveSdfWorldFile(_config.SdfFile(), _config.ResourceCache()); @@ -122,7 +122,7 @@ Server::Server(const ServerConfig &_config) break; } - case SourceType::kNone: + case ServerConfig::SourceType::kNone: default: { ignmsg << "Loading default world.\n"; diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 7101d211ba..1024eb5d11 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -306,7 +306,7 @@ class ignition::gazebo::ServerConfigPrivate public: std::optional sdfRoot; /// \brief Type of source used. - public: SourceType source{SourceType::kNone}; + public: ServerConfig::SourceType source{ServerConfig::SourceType::kNone}; }; ////////////////////////////////////////////////// @@ -327,7 +327,7 @@ ServerConfig::~ServerConfig() = default; ////////////////////////////////////////////////// bool ServerConfig::SetSdfFile(const std::string &_file) { - this->dataPtr->source = SourceType::kSdfFile; + this->dataPtr->source = ServerConfig::SourceType::kSdfFile; this->dataPtr->sdfFile = _file; this->dataPtr->sdfString = ""; this->dataPtr->sdfRoot = std::nullopt; @@ -343,7 +343,7 @@ std::string ServerConfig::SdfFile() const ////////////////////////////////////////////////// bool ServerConfig::SetSdfString(const std::string &_sdfString) { - this->dataPtr->source = SourceType::kSdfString; + this->dataPtr->source = ServerConfig::SourceType::kSdfString; this->dataPtr->sdfFile = ""; this->dataPtr->sdfString = _sdfString; this->dataPtr->sdfRoot = std::nullopt; @@ -710,7 +710,7 @@ const std::vector &ServerConfig::LogRecordTopics() const ///////////////////////////////////////////////// void ServerConfig::SetSdfRoot(const sdf::Root &_root) const { - this->dataPtr->source = SourceType::kSdfRoot; + this->dataPtr->source = ServerConfig::SourceType::kSdfRoot; this->dataPtr->sdfRoot.emplace(); for (uint64_t i = 0; i < _root.WorldCount(); ++i) @@ -731,7 +731,7 @@ std::optional &ServerConfig::SdfRoot() const } ///////////////////////////////////////////////// -SourceType ServerConfig::Source() const +ServerConfig::SourceType ServerConfig::Source() const { return this->dataPtr->source; } diff --git a/src/ServerConfig_TEST.cc b/src/ServerConfig_TEST.cc index 8486b2d70d..bc93120ef2 100644 --- a/src/ServerConfig_TEST.cc +++ b/src/ServerConfig_TEST.cc @@ -235,20 +235,24 @@ TEST(ServerConfig, SdfRoot) 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/Util.cc b/src/Util.cc index 39a3f16303..09babb612c 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -609,8 +609,8 @@ std::string findFuelResourceSdf(const std::string &_path) } ////////////////////////////////////////////////// -std::string IGNITION_GAZEBO_VISIBLE resolveSdfWorldFile( - const std::string &_sdfFile, const std::string &_fuelResourceCache) +std::string resolveSdfWorldFile(const std::string &_sdfFile, + const std::string &_fuelResourceCache) { std::string filePath; @@ -659,4 +659,3 @@ std::string IGNITION_GAZEBO_VISIBLE resolveSdfWorldFile( } } } - From d0695a417483c3ddb5c31c311206f39bf54c39a8 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Mar 2022 05:10:58 -0700 Subject: [PATCH 5/5] use sdf 12.4 Signed-off-by: Nate Koenig --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index df48d9915a..9a1d2999cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,7 +59,7 @@ set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # as protobuf could be find transitively by any dependency set(protobuf_MODULE_COMPATIBLE TRUE) -ign_find_package(sdformat12 REQUIRED VERSION 12.3) +ign_find_package(sdformat12 REQUIRED VERSION 12.4) set(SDF_VER ${sdformat12_VERSION_MAJOR}) #--------------------------------------