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