Port Server changes from gz-sim8 to gz-sim9 (#2638 and #2754)

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

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>
@@ -596,3 +600,116 @@ std::string ServerPrivate::FetchResourceUri(const common::URI &_uri)
 {
   return this->FetchResource(_uri.Str());
 }
+
+//////////////////////////////////////////////////
+sdf::Errors ServerPrivate::LoadSdfRootHelper(const ServerConfig &_config)
+{
+  sdf::Errors errors;
+
+  sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig();
+  sdfParserConfig.SetStoreResolvedURIs(true);
+  sdfParserConfig.SetCalculateInertialConfiguration(
+    sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD);
+  MeshInertiaCalculator meshInertiaCalculator;
+  sdfParserConfig.RegisterCustomInertiaCalc(meshInertiaCalculator);
+
+  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;
+      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;
+      // \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;
+}

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

test/integration/mesh_inertia_calculation.cc

@@ -51,20 +51,39 @@ class MeshInertiaCalculationTest : public InternalFixture<::testing::Test>
 {
 };
 
-TEST(MeshInertiaCalculationTest, CylinderColladaMeshInertiaCalculation)
+/// \brief Load an SDF world and run mesh inertia tests. Two tests are run
+/// one after another: 1) the server is launched with path to SDF file, and
+/// 2) ther server is launched from an sdf string.
+/// \param[in] _path Path to SDF
+/// \param[in] _testFunc Test function that checks mesh inertia values
+void loadSdfAndTest(const std::string &_path,
+    std::function<void(const gz::sim::ServerConfig &)> _testFunc)
 {
-  size_t kIter = 100u;
-
-  // Start server and run.
+  // Test mesh inertial calculator with sdf loaded from file
   gz::sim::ServerConfig serverConfig;
-  serverConfig.SetSdfFile(common::joinPaths(
-      PROJECT_SOURCE_PATH, "test", "worlds", "mesh_inertia_calculation.sdf"));
-
+  serverConfig.SetSdfFile(_path);
   common::setenv(
       "GZ_SIM_RESOURCE_PATH",
       common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models"));
+  _testFunc(serverConfig);
 
-  gz::sim::Server server(serverConfig);
+
+  // Test mesh inertial calculator with sdf loaded from string
+  std::ifstream sdfFile(_path);
+  std::stringstream buffer;
+  buffer << sdfFile.rdbuf();
+  gz::sim::ServerConfig serverConfigSdfStr;
+  serverConfigSdfStr.SetSdfString(buffer.str());
+  _testFunc(serverConfigSdfStr);
+}
+
+void cylinderColladaMeshInertiaCalculation(
+    const gz::sim::ServerConfig &_serverConfig)
+{
+  size_t kIter = 100u;
+
+  // Start server and run.
+  gz::sim::Server server(_serverConfig);
 
   // Create a system just to get the ECM
   EntityComponentManager *ecm;
@@ -127,21 +146,20 @@ TEST(MeshInertiaCalculationTest, CylinderColladaMeshInertiaCalculation)
   EXPECT_EQ(link.WorldInertialPose(*ecm).value(), gz::math::Pose3d::Zero);
 }
 
-TEST(MeshInertiaCalculationTest,
-  CylinderColladaMeshWithNonCenterOriginInertiaCalculation)
+TEST(MeshInertiaCalculationTest, CylinderColladaMeshInertiaCalculation)
+{
+  std::string sdfFilePath = common::joinPaths(
+      PROJECT_SOURCE_PATH, "test", "worlds", "mesh_inertia_calculation.sdf");
+  loadSdfAndTest(sdfFilePath, cylinderColladaMeshInertiaCalculation);
+}
+
+void cylinderColladaMeshWithNonCenterOriginInertiaCalculation(
+    const gz::sim::ServerConfig &_serverConfig)
 {
   size_t kIter = 100u;
 
   // Start server and run.
-  gz::sim::ServerConfig serverConfig;
-  serverConfig.SetSdfFile(common::joinPaths(
-      PROJECT_SOURCE_PATH, "test", "worlds", "mesh_inertia_calculation.sdf"));
-
-  common::setenv(
-      "GZ_SIM_RESOURCE_PATH",
-      common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models"));
-
-  gz::sim::Server server(serverConfig);
+  gz::sim::Server server(_serverConfig);
 
   // Create a system just to get the ECM
   EntityComponentManager *ecm;
@@ -207,6 +225,16 @@ TEST(MeshInertiaCalculationTest,
   // the center of mass (inertial pose) will be 1m above the ground
   EXPECT_EQ(link.WorldInertialPose(*ecm).value(),
     gz::math::Pose3d(0, 0, 1, 0, 0, 0));
+
+}
+
+TEST(MeshInertiaCalculationTest,
+  CylinderColladaMeshWithNonCenterOriginInertiaCalculation)
+{
+  std::string sdfFilePath = common::joinPaths(
+      PROJECT_SOURCE_PATH, "test", "worlds", "mesh_inertia_calculation.sdf");
+
+  loadSdfAndTest(sdfFilePath, cylinderColladaMeshInertiaCalculation);
 }
 
 TEST(MeshInertiaCalculationTest, CylinderColladaOptimizedMeshInertiaCalculation)