ServerConfig accepts an sdf::Root DOM object (#1333)

Signed-off-by: Nate Koenig <nate@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>
Co-authored-by: Michael Carroll <michael@openrobotics.org>
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

Author: 4 people

include/ignition/gazebo/ServerConfig.hh

@@ -24,6 +24,7 @@
 #include <string>
 #include <vector>
 #include <sdf/Element.hh>
+#include <sdf/Root.hh>
 #include <ignition/gazebo/config.hh>
 #include <ignition/gazebo/Export.hh>
 
@@ -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<sdf::Root> &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<std::chrono::system_clock> &
               Timestamp() const;
 
+      /// \brief Get the type of source
+      /// \return The source type.
+      public: SourceType Source() const;
+
       /// \brief Private data pointer
       private: std::unique_ptr<ServerConfigPrivate> dataPtr;
     };

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

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())

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<sdf::Root> 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<std::string> &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<sdf::Root> &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)
 {

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());
+}

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))
 {