From c48536b3d882f9d1bb52e871780e8810b2b1b226 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 2 Oct 2024 05:34:48 -0700 Subject: [PATCH 1/9] Copy the code to the new function - won't compile --- src/Server.cc | 102 +--------------------------------------- src/ServerPrivate.cc | 108 +++++++++++++++++++++++++++++++++++++++++++ src/ServerPrivate.hh | 6 +++ 3 files changed, 115 insertions(+), 101 deletions(-) diff --git a/src/Server.cc b/src/Server.cc index 86b8b7d1cc..8bb49b2515 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -96,107 +96,7 @@ 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; - } - } + sdf::Errors errors = this->dataPtr->LoadSdfRootHelper(_config); if (!errors.empty()) { diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 1d23f81586..7847d6a86c 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -574,3 +574,111 @@ 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()) + { + gzerr << "Failed to find world [" << _config.SdfFile() << "]" + << std::endl; + return errors; + } + + 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->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->sdfRoot.LoadSdfString( + DefaultWorld::World(), sdfParserConfig); + sdf::World *world = this->sdfRoot.WorldByIndex(0); + if (world == nullptr) { + return errors; + } + world->AddModel(*sdfRoot.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"; + // Load an empty world. + /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root. + errors = this->sdfRoot.LoadSdfString(DefaultWorld::World()); + 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); From 3c7e333a0b6064608fe187f60ee4f549a8d7cb84 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 2 Oct 2024 05:48:52 -0700 Subject: [PATCH 2/9] Fix the build --- src/Server.cc | 20 +------------------- src/ServerPrivate.cc | 37 ++++++++++++++++++++++++++++++------- 2 files changed, 31 insertions(+), 26 deletions(-) diff --git a/src/Server.cc b/src/Server.cc index 8bb49b2515..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,6 +77,7 @@ Server::Server(const ServerConfig &_config) addResourcePaths(); + // 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 7847d6a86c..45ec01bda0 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -32,6 +32,7 @@ #include #include "gz/sim/Util.hh" +#include "MeshInertiaCalculator.hh" #include "SimulationRunner.hh" using namespace gz; @@ -40,6 +41,24 @@ using namespace sim; const char ServerPrivate::kClassicMaterialScriptUri[] = "file://media/materials/scripts/gazebo.material"; +/// \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; + } +}; + /// \brief This struct provides access to the record plugin SDF string struct LoggingPlugin { @@ -619,14 +638,16 @@ sdf::Errors ServerPrivate::LoadSdfRootHelper(const ServerConfig &_config) if (filePath.empty()) { - gzerr << "Failed to find world [" << _config.SdfFile() << "]" - << std::endl; + 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 sdfRoot; + sdf::Root sdfRootLocal; sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig(); sdfParserConfig.SetStoreResolvedURIs(true); sdfParserConfig.SetCalculateInertialConfiguration( @@ -639,12 +660,12 @@ sdf::Errors ServerPrivate::LoadSdfRootHelper(const ServerConfig &_config) // 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); + errors = sdfRootLocal.Load(filePath, sdfParserConfig); if (errors.empty() || _config.BehaviorOnSdfErrors() != ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY) { - if (sdfRoot.Model() == nullptr) { - this->sdfRoot = std::move(sdfRoot); + if (sdfRootLocal.Model() == nullptr) { + this->sdfRoot = std::move(sdfRootLocal); } else { @@ -654,9 +675,11 @@ sdf::Errors ServerPrivate::LoadSdfRootHelper(const ServerConfig &_config) DefaultWorld::World(), sdfParserConfig); 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(*sdfRoot.Model()); + world->AddModel(*sdfRootLocal.Model()); if (errors.empty() || _config.BehaviorOnSdfErrors() != ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY) { From 21ccdf0a2713bd9b995d961e2d2c9a6a9dbb0d9e Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 2 Oct 2024 06:05:20 -0700 Subject: [PATCH 3/9] Replace static default world with sdf default world --- src/ServerPrivate.cc | 31 +++++++++---------------------- 1 file changed, 9 insertions(+), 22 deletions(-) diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 45ec01bda0..531e2e92a7 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -41,24 +41,6 @@ using namespace sim; const char ServerPrivate::kClassicMaterialScriptUri[] = "file://media/materials/scripts/gazebo.material"; -/// \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; - } -}; - /// \brief This struct provides access to the record plugin SDF string struct LoggingPlugin { @@ -669,10 +651,12 @@ sdf::Errors ServerPrivate::LoadSdfRootHelper(const ServerConfig &_config) } 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.LoadSdfString( - DefaultWorld::World(), sdfParserConfig); + errors = this->sdfRoot.AddWorld(defaultWorld); sdf::World *world = this->sdfRoot.WorldByIndex(0); if (world == nullptr) { errors.push_back({sdf::ErrorCode::FATAL_ERROR, @@ -696,9 +680,12 @@ sdf::Errors ServerPrivate::LoadSdfRootHelper(const ServerConfig &_config) default: { gzmsg << "Loading default world.\n"; + + sdf::World defaultWorld; + defaultWorld.SetName("default"); + // Load an empty world. - /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root. - errors = this->sdfRoot.LoadSdfString(DefaultWorld::World()); + errors = this->sdfRoot.AddWorld(defaultWorld); break; } } From 219a79c0ae50a64fa5e3ae94f1f6ac48817b20f4 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 2 Oct 2024 08:26:58 -0700 Subject: [PATCH 4/9] Added string and utility headers --- src/ServerPrivate.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 531e2e92a7..06a15b0794 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -18,6 +18,9 @@ #include +#include +#include + #include #include #include From dc8ee70149ec84feb1abfeb5fea469895aacd4b5 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 4 Oct 2024 06:07:25 -0700 Subject: [PATCH 5/9] test moving header file Signed-off-by: Nate Koenig --- src/ServerPrivate.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 06a15b0794..f93d4bbab8 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include "MeshInertiaCalculator.hh" #include "ServerPrivate.hh" #include @@ -35,7 +36,6 @@ #include #include "gz/sim/Util.hh" -#include "MeshInertiaCalculator.hh" #include "SimulationRunner.hh" using namespace gz; From c32ebee756e86b1cf45ddd646f50486903e9b1fb Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 4 Oct 2024 09:46:59 -0700 Subject: [PATCH 6/9] Move header file back Signed-off-by: Nate Koenig --- src/ServerPrivate.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index f93d4bbab8..06a15b0794 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -14,7 +14,6 @@ * limitations under the License. * */ -#include "MeshInertiaCalculator.hh" #include "ServerPrivate.hh" #include @@ -36,6 +35,7 @@ #include #include "gz/sim/Util.hh" +#include "MeshInertiaCalculator.hh" #include "SimulationRunner.hh" using namespace gz; From e9c547b4a3cb040ead247daffed91134b3876132 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 8 Oct 2024 05:17:45 -0700 Subject: [PATCH 7/9] Move meshinertiacalculator header Signed-off-by: Nate Koenig --- src/ServerPrivate.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 06a15b0794..f16db558b0 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -15,6 +15,7 @@ * */ #include "ServerPrivate.hh" +#include "MeshInertiaCalculator.hh" #include @@ -35,7 +36,6 @@ #include #include "gz/sim/Util.hh" -#include "MeshInertiaCalculator.hh" #include "SimulationRunner.hh" using namespace gz; From 3fc3a68ba85e0270d005cb4888908f965a52bfd4 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 8 Oct 2024 06:18:17 -0700 Subject: [PATCH 8/9] Testing remove of literals Signed-off-by: Nate Koenig --- src/ServerPrivate.hh | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index 4b81144feb..e29f3e9703 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -48,8 +48,6 @@ #include "gz/sim/ServerConfig.hh" #include "gz/sim/SystemLoader.hh" -using namespace std::chrono_literals; - namespace gz { namespace sim From d2f11dc71e168dbf0e77748dbd2a1720ba0efa90 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 8 Oct 2024 08:06:29 -0700 Subject: [PATCH 9/9] Undo tests, and just move the MeshInertiaCalculator.hh to top of file Signed-off-by: Nate Koenig --- src/ServerPrivate.cc | 2 +- src/ServerPrivate.hh | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index f16db558b0..f93d4bbab8 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -14,8 +14,8 @@ * limitations under the License. * */ -#include "ServerPrivate.hh" #include "MeshInertiaCalculator.hh" +#include "ServerPrivate.hh" #include diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index e29f3e9703..4b81144feb 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -48,6 +48,8 @@ #include "gz/sim/ServerConfig.hh" #include "gz/sim/SystemLoader.hh" +using namespace std::chrono_literals; + namespace gz { namespace sim