From d94c11e911d90e92633b87ee17eb35d3786e81fc Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 14 Oct 2024 14:24:04 -0700 Subject: [PATCH] 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 * Move header file back Signed-off-by: Nate Koenig * Move meshinertiacalculator header Signed-off-by: Nate Koenig * Testing remove of literals Signed-off-by: Nate Koenig * Undo tests, and just move the MeshInertiaCalculator.hh to top of file Signed-off-by: Nate Koenig --------- Signed-off-by: Nate Koenig --- src/Server.cc | 122 +------------------------------------------ src/ServerPrivate.cc | 121 ++++++++++++++++++++++++++++++++++++++++++ src/ServerPrivate.hh | 6 +++ 3 files changed, 129 insertions(+), 120 deletions(-) diff --git a/src/Server.cc b/src/Server.cc index 86b8b7d1cc..f2ce3cdc59 100644 --- 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("" - "" - "") + - "" - ""; - - 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 index 1d23f81586..f93d4bbab8 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -14,10 +14,14 @@ * limitations under the License. * */ +#include "MeshInertiaCalculator.hh" #include "ServerPrivate.hh" #include +#include +#include + #include #include #include @@ -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 index cbb298db3f..4b81144feb 100644 --- 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);