Break out server constructor (#2638)

* Copy the code to the new function - won't compile * Fix the build * Replace static default world with sdf default world * Added string and utility headers * test moving header file Signed-off-by: Nate Koenig <[email protected]> * Move header file back Signed-off-by: Nate Koenig <[email protected]> * Move meshinertiacalculator header Signed-off-by: Nate Koenig <[email protected]> * Testing remove of literals Signed-off-by: Nate Koenig <[email protected]> * Undo tests, and just move the MeshInertiaCalculator.hh to top of file Signed-off-by: Nate Koenig <[email protected]> --------- Signed-off-by: Nate Koenig <[email protected]>
gazebosim · Oct 14, 2024 · d94c11e · d94c11e
1 parent 63b210a
commit d94c11e
Show file tree

Hide file tree

Showing 3 changed files with 129 additions and 120 deletions.
diff --git a/src/Server.cc b/src/Server.cc
@@ -33,31 +33,12 @@
 #include "gz/sim/Server.hh"
 #include "gz/sim/Util.hh"
 
-#include "MeshInertiaCalculator.hh"
 #include "ServerPrivate.hh"
 #include "SimulationRunner.hh"
 
 using namespace gz;
 using namespace sim;
 
-/// \brief This struct provides access to the default world.
-struct DefaultWorld
-{
-  /// \brief Get the default world as a string.
-  /// Plugins will be loaded from the server.config file.
-  /// \return An SDF string that contains the default world.
-  public: static std::string &World()
-  {
-    static std::string world = std::string("<?xml version='1.0'?>"
-      "<sdf version='1.6'>"
-        "<world name='default'>") +
-        "</world>"
-      "</sdf>";
-
-    return world;
-  }
-};
-
 /////////////////////////////////////////////////
 Server::Server(const ServerConfig &_config)
   : dataPtr(new ServerPrivate)
@@ -96,107 +77,8 @@ Server::Server(const ServerConfig &_config)
 
   addResourcePaths();
 
-  sdf::Errors errors;
-
-  switch (_config.Source())
-  {
-    // Load a world if specified. Check SDF string first, then SDF file
-    case ServerConfig::SourceType::kSdfRoot:
-    {
-      this->dataPtr->sdfRoot = _config.SdfRoot()->Clone();
-      gzmsg << "Loading SDF world from SDF DOM.\n";
-      break;
-    }
-
-    case ServerConfig::SourceType::kSdfString:
-    {
-      std::string msg = "Loading SDF string. ";
-      if (_config.SdfFile().empty())
-      {
-        msg += "File path not available.\n";
-      }
-      else
-      {
-        msg += "File path [" + _config.SdfFile() + "].\n";
-      }
-      gzmsg <<  msg;
-      sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig();
-      sdfParserConfig.SetStoreResolvedURIs(true);
-      sdfParserConfig.SetCalculateInertialConfiguration(
-        sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD);
-      errors = this->dataPtr->sdfRoot.LoadSdfString(
-        _config.SdfString(), sdfParserConfig);
-      this->dataPtr->sdfRoot.ResolveAutoInertials(errors, sdfParserConfig);
-      break;
-    }
-
-    case ServerConfig::SourceType::kSdfFile:
-    {
-      std::string filePath = resolveSdfWorldFile(_config.SdfFile(),
-          _config.ResourceCache());
-
-      if (filePath.empty())
-      {
-        gzerr << "Failed to find world [" << _config.SdfFile() << "]"
-               << std::endl;
-        return;
-      }
-
-      gzmsg << "Loading SDF world file[" << filePath << "].\n";
-
-      sdf::Root sdfRoot;
-      sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig();
-      sdfParserConfig.SetStoreResolvedURIs(true);
-      sdfParserConfig.SetCalculateInertialConfiguration(
-        sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD);
-
-      MeshInertiaCalculator meshInertiaCalculator;
-      sdfParserConfig.RegisterCustomInertiaCalc(meshInertiaCalculator);
-      // \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 = sdfRoot.Load(filePath, sdfParserConfig);
-      if (errors.empty() || _config.BehaviorOnSdfErrors() !=
-          ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY)
-      {
-        if (sdfRoot.Model() == nullptr) {
-          this->dataPtr->sdfRoot = std::move(sdfRoot);
-        }
-        else
-        {
-          // If the specified file only contains a model, load the default
-          // world and add the model to it.
-          errors = this->dataPtr->sdfRoot.LoadSdfString(
-            DefaultWorld::World(), sdfParserConfig);
-          sdf::World *world = this->dataPtr->sdfRoot.WorldByIndex(0);
-          if (world == nullptr) {
-            return;
-          }
-          world->AddModel(*sdfRoot.Model());
-          if (errors.empty() || _config.BehaviorOnSdfErrors() !=
-              ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY)
-          {
-            errors = this->dataPtr->sdfRoot.UpdateGraphs();
-          }
-        }
-      }
-
-      this->dataPtr->sdfRoot.ResolveAutoInertials(errors, sdfParserConfig);
-      break;
-    }
-
-    case ServerConfig::SourceType::kNone:
-    default:
-    {
-      gzmsg << "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;
-    }
-  }
+  // Loads the SDF root object based on values in a ServerConfig object.
+  sdf::Errors errors = this->dataPtr->LoadSdfRootHelper(_config);
 
   if (!errors.empty())
   {

diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc
@@ -14,10 +14,14 @@
  * limitations under the License.
  *
 */
+#include "MeshInertiaCalculator.hh"
 #include "ServerPrivate.hh"
 
 #include <tinyxml2.h>
 
+#include <string>
+#include <utility>
+
 #include <gz/msgs/boolean.pb.h>
 #include <gz/msgs/server_control.pb.h>
 #include <gz/msgs/stringmsg.pb.h>
@@ -574,3 +578,120 @@ std::string ServerPrivate::FetchResourceUri(const common::URI &_uri)
 {
   return this->FetchResource(_uri.Str());
 }
+
+//////////////////////////////////////////////////
+sdf::Errors ServerPrivate::LoadSdfRootHelper(const ServerConfig &_config)
+{
+  sdf::Errors errors;
+
+  switch (_config.Source())
+  {
+    // Load a world if specified. Check SDF string first, then SDF file
+    case ServerConfig::SourceType::kSdfRoot:
+    {
+      this->sdfRoot = _config.SdfRoot()->Clone();
+      gzmsg << "Loading SDF world from SDF DOM.\n";
+      break;
+    }
+
+    case ServerConfig::SourceType::kSdfString:
+    {
+      std::string msg = "Loading SDF string. ";
+      if (_config.SdfFile().empty())
+      {
+        msg += "File path not available.\n";
+      }
+      else
+      {
+        msg += "File path [" + _config.SdfFile() + "].\n";
+      }
+      gzmsg << msg;
+      sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig();
+      sdfParserConfig.SetStoreResolvedURIs(true);
+      sdfParserConfig.SetCalculateInertialConfiguration(
+        sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD);
+      errors = this->sdfRoot.LoadSdfString(
+        _config.SdfString(), sdfParserConfig);
+      this->sdfRoot.ResolveAutoInertials(errors, sdfParserConfig);
+      break;
+    }
+
+    case ServerConfig::SourceType::kSdfFile:
+    {
+      std::string filePath = resolveSdfWorldFile(_config.SdfFile(),
+          _config.ResourceCache());
+
+      if (filePath.empty())
+      {
+        std::string errStr =  "Failed to find world ["
+          + _config.SdfFile() + "]";
+        gzerr << errStr << std::endl;
+        errors.push_back({sdf::ErrorCode::FILE_READ, errStr});
+        return errors;
+      }
+
+      gzmsg << "Loading SDF world file[" << filePath << "].\n";
+
+      sdf::Root sdfRootLocal;
+      sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig();
+      sdfParserConfig.SetStoreResolvedURIs(true);
+      sdfParserConfig.SetCalculateInertialConfiguration(
+        sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD);
+
+      MeshInertiaCalculator meshInertiaCalculator;
+      sdfParserConfig.RegisterCustomInertiaCalc(meshInertiaCalculator);
+      // \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 = sdfRootLocal.Load(filePath, sdfParserConfig);
+      if (errors.empty() || _config.BehaviorOnSdfErrors() !=
+          ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY)
+      {
+        if (sdfRootLocal.Model() == nullptr) {
+          this->sdfRoot = std::move(sdfRootLocal);
+        }
+        else
+        {
+          sdf::World defaultWorld;
+          defaultWorld.SetName("default");
+
+          // If the specified file only contains a model, load the default
+          // world and add the model to it.
+          errors = this->sdfRoot.AddWorld(defaultWorld);
+          sdf::World *world = this->sdfRoot.WorldByIndex(0);
+          if (world == nullptr) {
+            errors.push_back({sdf::ErrorCode::FATAL_ERROR,
+              "sdf::World pointer is null"});
+            return errors;
+          }
+          world->AddModel(*sdfRootLocal.Model());
+          if (errors.empty() || _config.BehaviorOnSdfErrors() !=
+              ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY)
+          {
+            errors = this->sdfRoot.UpdateGraphs();
+          }
+        }
+      }
+
+      this->sdfRoot.ResolveAutoInertials(errors, sdfParserConfig);
+      break;
+    }
+
+    case ServerConfig::SourceType::kNone:
+    default:
+    {
+      gzmsg << "Loading default world.\n";
+
+      sdf::World defaultWorld;
+      defaultWorld.SetName("default");
+
+      // Load an empty world.
+      errors = this->sdfRoot.AddWorld(defaultWorld);
+      break;
+    }
+  }
+
+  return errors;
+}
diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh
@@ -99,6 +99,12 @@ namespace gz
       /// \return Path to the downloaded resource, empty on error.
       public: std::string FetchResourceUri(const common::URI &_uri);
 
+      /// \brief Helper function that loads an SDF root object based on
+      /// values in a ServerConfig object.
+      /// \param[in] _config Server config to read from.
+      /// \return Set of SDF errors.
+      public: sdf::Errors LoadSdfRootHelper(const ServerConfig &_config);
+
       /// \brief Signal handler callback
       /// \param[in] _sig The signal number
       private: void OnSignal(int _sig);