From 0896ac1dcc2ae3f847f9d8f103f03a30a5f38ec1 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 10 Feb 2022 20:59:04 -0800 Subject: [PATCH 01/14] Added more tests Signed-off-by: Nate Koenig --- include/ignition/gazebo/ServerConfig.hh | 12 ++++ include/ignition/gazebo/Util.hh | 18 ++++++ src/Server.cc | 71 +++-------------------- src/ServerConfig.cc | 27 +++++++++ src/ServerConfig_TEST.cc | 24 ++++++++ src/Server_TEST.cc | 43 ++++++++++++++ src/Util.cc | 77 +++++++++++++++++++++++++ src/Util_TEST.cc | 42 ++++++++++++++ 8 files changed, 251 insertions(+), 63 deletions(-) diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index a975cc7bc4..663459c6b6 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -175,6 +176,17 @@ namespace ignition /// \return The full contents of the SDF string, or empty string. public: std::string SdfString() const; + /// \brief Set the SDF Root DOM object. The sdf::Root object will take + /// precendence over ServerConfig::SdfString() and + /// ServerConfig::SdfFile(). + /// \param[in] _root SDF Root object to use. + public: void SetSdfRoot(const sdf::Root &_root) const; + + /// \brief Get the SDF Root DOM object. + /// \return SDF Root object to use, or std::nullopt if the sdf::Root + /// has not been set via ServerConfig::SetSdfRoot(). + public: const std::optional &SdfRoot() const; + /// \brief Set the update rate in Hertz. Value <=0 are ignored. /// \param[in] _hz The desired update rate of the server in Hertz. public: void SetUpdateRate(const double &_hz); diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 67e405659f..9dcd4b273a 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -210,6 +210,24 @@ namespace ignition const EntityComponentManager &_ecm, bool _excludeWorld = true); + /// \brief Convert an SDF world filename string, such as "shapes.sdf", to + /// full system file path. + /// The provided SDF filename may be a Fuel URI, relative path, name + /// of an installed Gazebo world filename, or an absolute path. + /// \param[in] _sdfFile An SDF world filename such as: + /// 1. "shapes.sdf" - This is referencing an installed world file. + /// 2. "../shapes.sdf" - This is referencing a relative world file. + /// 3. "/home/user/shapes.sdf" - This is reference an absolute world + /// file. + /// 4. "https://fuel.ignitionrobotics.org/1.0/openrobotics/worlds/shapes.sdf" - This is referencing a Fuel URI. This will download the world file. + /// \param[in] _fuelResourceCache Path to a Fuel resource cache, if + /// known. + /// \return Full path to the SDF world file. An empty string is returned + /// if the file could not be found. + std::string IGNITION_GAZEBO_VISIBLE resolveSdfWorldFile( + const std::string &_sdfFilename, + const std::string &_fuelResourceCache = ""); + /// \brief Helper function to "enable" a component (i.e. create it if it /// doesn't exist) or "disable" a component (i.e. remove it if it exists). /// \param[in] _ecm Mutable reference to the ECM diff --git a/src/Server.cc b/src/Server.cc index 9b2a2b8448..751faa2021 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -33,31 +33,6 @@ using namespace ignition; using namespace gazebo; -////////////////////////////////////////////////// -// Getting the first .sdf file in the path -std::string findFuelResourceSdf(const std::string &_path) -{ - if (!common::exists(_path)) - return ""; - - for (common::DirIter file(_path); file != common::DirIter(); ++file) - { - std::string current(*file); - if (!common::isFile(current)) - continue; - - auto fileName = common::basename(current); - auto fileExtensionIndex = fileName.rfind("."); - auto fileExtension = fileName.substr(fileExtensionIndex + 1); - - if (fileExtension == "sdf") - { - return current; - } - } - return ""; -} - /// \brief This struct provides access to the default world. struct DefaultWorld { @@ -99,7 +74,12 @@ Server::Server(const ServerConfig &_config) sdf::Errors errors; // Load a world if specified. Check SDF string first, then SDF file - if (!_config.SdfString().empty()) + if (_config.SdfRoot()) + { + this->dataPtr->sdfRoot.Clone(*_config.SdfRoot()); + ignmsg << "Loading SDF world from SDF DOM.\n"; + } + else if (!_config.SdfString().empty()) { std::string msg = "Loading SDF string. "; if (_config.SdfFile().empty()) @@ -115,43 +95,8 @@ Server::Server(const ServerConfig &_config) } else if (!_config.SdfFile().empty()) { - std::string filePath; - - // Check Fuel if it's a URL - auto sdfUri = common::URI(_config.SdfFile()); - if (sdfUri.Scheme() == "http" || sdfUri.Scheme() == "https") - { - std::string fuelCachePath; - if (this->dataPtr->fuelClient->CachedWorld(common::URI(_config.SdfFile()), - fuelCachePath)) - { - filePath = findFuelResourceSdf(fuelCachePath); - } - else if (auto result = this->dataPtr->fuelClient->DownloadWorld( - common::URI(_config.SdfFile()), fuelCachePath)) - { - filePath = findFuelResourceSdf(fuelCachePath); - } - else - { - ignwarn << "Fuel couldn't download URL [" << _config.SdfFile() - << "], error: [" << result.ReadableResult() << "]" - << std::endl; - } - } - - if (filePath.empty()) - { - common::SystemPaths systemPaths; - - // Worlds from environment variable - systemPaths.SetFilePathEnv(kResourcePathEnv); - - // Worlds installed with ign-gazebo - systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); - - filePath = systemPaths.FindFile(_config.SdfFile()); - } + std::string filePath = resolveSdfWorldFile(_config.SdfFile(), + _config.ResourceCache()); if (filePath.empty()) { diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index fb0d9fdace..99eb6550aa 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -301,6 +301,9 @@ class ignition::gazebo::ServerConfigPrivate /// \brief is the headless mode active. public: bool isHeadlessRendering{false}; + + /// \brief Optional SDF root object. + public: std::optional sdfRoot; }; ////////////////////////////////////////////////// @@ -323,6 +326,7 @@ bool ServerConfig::SetSdfFile(const std::string &_file) { this->dataPtr->sdfFile = _file; this->dataPtr->sdfString = ""; + this->dataPtr->sdfRoot = std::nullopt; return true; } @@ -337,6 +341,7 @@ bool ServerConfig::SetSdfString(const std::string &_sdfString) { this->dataPtr->sdfFile = ""; this->dataPtr->sdfString = _sdfString; + this->dataPtr->sdfRoot = std::nullopt; return true; } @@ -697,6 +702,28 @@ const std::vector &ServerConfig::LogRecordTopics() const return this->dataPtr->logRecordTopics; } +///////////////////////////////////////////////// +void ServerConfig::SetSdfRoot(const sdf::Root &_root) const +{ + this->dataPtr->sdfRoot.emplace(); + + for (uint64_t i = 0; i < _root.WorldCount(); ++i) + { + const sdf::World *world = _root.WorldByIndex(i); + if (world) + this->dataPtr->sdfRoot->AddWorld(*world); + } + + this->dataPtr->sdfFile = ""; + this->dataPtr->sdfString = ""; +} + +///////////////////////////////////////////////// +const std::optional &ServerConfig::SdfRoot() const +{ + return this->dataPtr->sdfRoot; +} + ///////////////////////////////////////////////// void copyElement(sdf::ElementPtr _sdf, const tinyxml2::XMLElement *_xml) { diff --git a/src/ServerConfig_TEST.cc b/src/ServerConfig_TEST.cc index 733a55ef37..8486b2d70d 100644 --- a/src/ServerConfig_TEST.cc +++ b/src/ServerConfig_TEST.cc @@ -228,3 +228,27 @@ TEST(ServerConfig, GenerateRecordPlugin) EXPECT_EQ(plugin.Name(), "ignition::gazebo::systems::LogRecord"); } +////////////////////////////////////////////////// +TEST(ServerConfig, SdfRoot) +{ + ServerConfig config; + EXPECT_FALSE(config.SdfRoot()); + EXPECT_TRUE(config.SdfFile().empty()); + EXPECT_TRUE(config.SdfString().empty()); + + config.SetSdfString("string"); + EXPECT_FALSE(config.SdfRoot()); + EXPECT_TRUE(config.SdfFile().empty()); + EXPECT_FALSE(config.SdfString().empty()); + + config.SetSdfFile("file"); + EXPECT_FALSE(config.SdfRoot()); + EXPECT_FALSE(config.SdfFile().empty()); + EXPECT_TRUE(config.SdfString().empty()); + + sdf::Root root; + config.SetSdfRoot(root); + EXPECT_TRUE(config.SdfRoot()); + EXPECT_TRUE(config.SdfFile().empty()); + EXPECT_TRUE(config.SdfString().empty()); +} diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index 2c11a7697e..2c5ea8027f 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -292,6 +292,49 @@ TEST_P(ServerFixture, SdfServerConfig) EXPECT_FALSE(server.HasEntity("bad", 1)); } +///////////////////////////////////////////////// +TEST_P(ServerFixture, SdfRootServerConfig) +{ + ignition::gazebo::ServerConfig serverConfig; + + serverConfig.SetSdfString(TestWorldSansPhysics::World()); + EXPECT_TRUE(serverConfig.SdfFile().empty()); + EXPECT_FALSE(serverConfig.SdfString().empty()); + + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "air_pressure.sdf")); + EXPECT_FALSE(serverConfig.SdfFile().empty()); + EXPECT_TRUE(serverConfig.SdfString().empty()); + + sdf::Root root; + root.Load(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "shapes.sdf")); + + // Setting the SDF Root should override the string and file. + serverConfig.SetSdfRoot(root); + + EXPECT_TRUE(serverConfig.SdfRoot()); + EXPECT_TRUE(serverConfig.SdfFile().empty()); + EXPECT_TRUE(serverConfig.SdfString().empty()); + + gazebo::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + EXPECT_TRUE(*server.Paused()); + EXPECT_EQ(0u, *server.IterationCount()); + EXPECT_EQ(24u, *server.EntityCount()); + EXPECT_EQ(3u, *server.SystemCount()); + + EXPECT_TRUE(server.HasEntity("box")); + EXPECT_FALSE(server.HasEntity("box", 1)); + EXPECT_TRUE(server.HasEntity("sphere")); + EXPECT_TRUE(server.HasEntity("cylinder")); + EXPECT_TRUE(server.HasEntity("capsule")); + EXPECT_TRUE(server.HasEntity("ellipsoid")); + EXPECT_FALSE(server.HasEntity("bad", 0)); + EXPECT_FALSE(server.HasEntity("bad", 1)); +} + ///////////////////////////////////////////////// TEST_P(ServerFixture, ServerConfigLogRecord) { diff --git a/src/Util.cc b/src/Util.cc index ec6efd57dc..39a3f16303 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -36,6 +36,9 @@ #include #include +#include +#include + #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/Joint.hh" @@ -579,6 +582,80 @@ std::optional sphericalCoordinates(Entity _entity, // Return degrees return math::Vector3d(IGN_RTOD(rad.X()), IGN_RTOD(rad.Y()), rad.Z()); } + +////////////////////////////////////////////////// +// Getting the first .sdf file in the path +std::string findFuelResourceSdf(const std::string &_path) +{ + if (!common::exists(_path)) + return ""; + + for (common::DirIter file(_path); file != common::DirIter(); ++file) + { + std::string current(*file); + if (!common::isFile(current)) + continue; + + auto fileName = common::basename(current); + auto fileExtensionIndex = fileName.rfind("."); + auto fileExtension = fileName.substr(fileExtensionIndex + 1); + + if (fileExtension == "sdf") + { + return current; + } + } + return ""; +} + +////////////////////////////////////////////////// +std::string IGNITION_GAZEBO_VISIBLE resolveSdfWorldFile( + const std::string &_sdfFile, const std::string &_fuelResourceCache) +{ + std::string filePath; + + // Check Fuel if it's a URL + auto sdfUri = common::URI(_sdfFile); + if (sdfUri.Scheme() == "http" || sdfUri.Scheme() == "https") + { + fuel_tools::ClientConfig config; + if (!_fuelResourceCache.empty()) + config.SetCacheLocation(_fuelResourceCache); + fuel_tools::FuelClient fuelClient(config); + + std::string fuelCachePath; + if (fuelClient.CachedWorld(common::URI(_sdfFile), fuelCachePath)) + { + filePath = findFuelResourceSdf(fuelCachePath); + } + else if (auto result = fuelClient.DownloadWorld( + common::URI(_sdfFile), fuelCachePath)) + { + filePath = findFuelResourceSdf(fuelCachePath); + } + else + { + ignwarn << "Fuel couldn't download URL [" << _sdfFile + << "], error: [" << result.ReadableResult() << "]" + << std::endl; + } + } + + if (filePath.empty()) + { + common::SystemPaths systemPaths; + + // Worlds from environment variable + systemPaths.SetFilePathEnv(kResourcePathEnv); + + // Worlds installed with ign-gazebo + systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); + + filePath = systemPaths.FindFile(_sdfFile); + } + + return filePath; +} } } } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index ac83bf2643..388661193e 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -21,6 +21,8 @@ #include #include +#include + #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/Joint.hh" @@ -37,6 +39,7 @@ #include "ignition/gazebo/Util.hh" #include "helpers/EnvTestFixture.hh" +#include "helpers/UniqueTestDirectoryEnv.hh" using namespace ignition; using namespace gazebo; @@ -716,3 +719,42 @@ TEST_F(UtilTest, EnableComponent) EXPECT_FALSE(enableComponent(ecm, entity1, false)); EXPECT_EQ(nullptr, ecm.Component(entity1)); } + +///////////////////////////////////////////////// +TEST_F(UtilTest, ResolveSdfWorldFile) +{ + // Test resolving a Fuel URI + fuel_tools::ClientConfig config; + + // URI to a Fuel world. + std::string fuelUri = + "https://fuel.ignitionrobotics.org/1.0/openrobotics/worlds/test world"; + + // The expect path for the local Fuel world. + std::string expectedPath = common::joinPaths( + config.CacheLocation(), "fuel.ignitionrobotics.org", + "openrobotics", "worlds", "test world"); + + // Get the Fuel world. + std::string resolvedPath = resolveSdfWorldFile(fuelUri, + config.CacheLocation()); + + // The Fuel model has not been downloaded, so it should not exist. + EXPECT_FALSE(resolvedPath.empty()); + + // The expected path should be the first part of the resolved path. The + // resolved path will have extra world version information at the end. + EXPECT_EQ(0u, resolvedPath.find(expectedPath)); + + // Now try to resolve the downloaded world file using an aboslute path + EXPECT_EQ(resolvedPath, resolveSdfWorldFile(resolvedPath)); + + // The "shapes.sdf" world file should resolve. + EXPECT_FALSE(resolveSdfWorldFile("shapes.sdf").empty()); + + // A bad absolute path should return an empty string + EXPECT_TRUE(resolveSdfWorldFile("/invalid/does_not_exist.sdf").empty()); + + // A bad relative path should return an empty string + EXPECT_TRUE(resolveSdfWorldFile("../invalid/does_not_exist.sdf").empty()); +} From 2770692dca44be7b8b3ba5ae3fd005c542840283 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Feb 2022 08:42:36 -0800 Subject: [PATCH 02/14] Change clone Signed-off-by: Nate Koenig --- src/Server.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Server.cc b/src/Server.cc index 751faa2021..fbb65fb2f5 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -76,7 +76,7 @@ Server::Server(const ServerConfig &_config) // Load a world if specified. Check SDF string first, then SDF file if (_config.SdfRoot()) { - this->dataPtr->sdfRoot.Clone(*_config.SdfRoot()); + this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); ignmsg << "Loading SDF world from SDF DOM.\n"; } else if (!_config.SdfString().empty()) From 4a9822b5b75075d68f637b4186802ddb5264340a Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Feb 2022 12:47:10 -0800 Subject: [PATCH 03/14] Use sdf::Plugin instead of sdf::ElementPtr Signed-off-by: Nate Koenig --- include/ignition/gazebo/Events.hh | 8 ++ include/ignition/gazebo/ServerConfig.hh | 32 +++++++ include/ignition/gazebo/SystemLoader.hh | 9 ++ include/ignition/gazebo/components/Visual.hh | 9 ++ src/LevelManager.cc | 4 +- src/SdfEntityCreator.cc | 54 ++++++------ src/Server.cc | 2 +- src/ServerConfig.cc | 75 ++++++++++++---- src/ServerPrivate.cc | 90 +++++++------------- src/Server_TEST.cc | 1 + src/SimulationRunner.cc | 48 ++++++++++- src/SimulationRunner.hh | 19 ++++- src/SystemLoader.cc | 87 +++++++++++++++++++ 13 files changed, 332 insertions(+), 106 deletions(-) diff --git a/include/ignition/gazebo/Events.hh b/include/ignition/gazebo/Events.hh index 672c264c03..7f3762f6e4 100644 --- a/include/ignition/gazebo/Events.hh +++ b/include/ignition/gazebo/Events.hh @@ -18,6 +18,7 @@ #define IGNITION_GAZEBO_EVENTS_HH_ #include +#include #include @@ -56,8 +57,15 @@ namespace ignition /// \brief Event used to load plugins for an entity into simulation. /// Pass in the entity which will own the plugins, and an SDF element for /// the entity, which may contain multiple `` tags. + /// \todo(nkoenig) Deprecate this in Garden. using LoadPlugins = common::EventT; + + /// \brief Event used to load plugins for an entity into simulation. + /// Pass in the entity which will own the plugins, and an SDF element for + /// the entity, which may contain multiple `` tags. + using LoadSdfPlugins = common::EventT; } } // namespace events } // namespace gazebo diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index 663459c6b6..06f4b7b7d8 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -69,12 +70,25 @@ namespace ignition /// \param[in] _name Name of the interface within the plugin library /// to load. /// \param[in] _sdf Plugin XML elements associated with this plugin. + /// \todo(nkoenig) Deprecate this in Garden. public: PluginInfo(const std::string &_entityName, const std::string &_entityType, const std::string &_filename, const std::string &_name, const sdf::ElementPtr &_sdf); + /// \brief Constructor with plugin information specified. + /// \param[in] _entityName Name of the entity which should receive + /// this plugin. The name is used in conjuction with _entityType to + /// uniquely identify an entity. + /// \param[in] _entityType Entity type which should receive this + /// plugin. The type is used in conjuction with _entityName to + /// uniquely identify an entity. + /// \param[in] _plugin SDF Plugin library information. + public: PluginInfo(const std::string &_entityName, + const std::string &_entityType, + const sdf::Plugin &_plugin); + /// \brief Copy constructor. /// \param[in] _info Plugin to copy. public: PluginInfo(const PluginInfo &_info); @@ -110,32 +124,50 @@ namespace ignition /// \brief Get the plugin library filename. /// \return Plugin library filename. + /// \todo(nkoenig) Deprecate this function in Garden. public: const std::string &Filename() const; /// \brief Set the type of the entity which should receive this /// plugin. The type is used in conjuction with EntityName to /// uniquely identify an entity. /// \param[in] _filename Entity type string. + /// \todo(nkoenig) Deprecate this function in Garden. public: void SetFilename(const std::string &_filename); /// \brief Name of the interface within the plugin library /// to load. /// \return Interface name. + /// \todo(nkoenig) Deprecate this function in Garden. public: const std::string &Name() const; /// \brief Set the name of the interface within the plugin library /// to load. /// \param[in] _name Interface name. + /// \todo(nkoenig) Deprecate this function in Garden. public: void SetName(const std::string &_name); /// \brief Plugin XML elements associated with this plugin. /// \return SDF pointer. + /// \todo(nkoenig) Deprecate this function in Garden. public: const sdf::ElementPtr &Sdf() const; /// \brief Set the plugin XML elements associated with this plugin. /// \param[in] _sdf SDF pointer, it will be cloned. + /// \todo(nkoenig) Deprecate this function in Garden. public: void SetSdf(const sdf::ElementPtr &_sdf); + /// \brief Get the SDF plugin information. + /// \return The SDF Plugin object. + public: const sdf::Plugin &Plugin() const; + + /// \brief Get a mutable version of the SDF plugin information. + /// \return The SDF Plugin object. + public: sdf::Plugin &Plugin(); + + /// \brief Set the SDF plugin information. + /// \param[in] _plugin The SDF Plugin object to use. + public: void SetPlugin(const sdf::Plugin &_plugin) const; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/include/ignition/gazebo/SystemLoader.hh b/include/ignition/gazebo/SystemLoader.hh index 7b8537b061..719fd825ef 100644 --- a/include/ignition/gazebo/SystemLoader.hh +++ b/include/ignition/gazebo/SystemLoader.hh @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -53,6 +54,7 @@ namespace ignition /// \brief Load and instantiate system plugin from an SDF element. /// \param[in] _sdf SDF Element describing plugin instance to be loaded. /// \returns Shared pointer to system instance or nullptr. + /// \todo(nkoenig) Deprecate this function in Garden public: std::optional LoadPlugin( const sdf::ElementPtr &_sdf); @@ -61,11 +63,18 @@ namespace ignition /// \param[in] _name Class name to be instantiated. /// \param[in] _sdf SDF Element describing plugin instance to be loaded. /// \returns Shared pointer to system instance or nullptr. + /// \todo(nkoenig) Deprecate this function in Garden public: std::optional LoadPlugin( const std::string &_filename, const std::string &_name, const sdf::ElementPtr &_sdf); + /// \brief Load and instantiate system plugin from name/filename. + /// \param[in] _plugin SDF Plugin to be loaded. + /// \returns Shared pointer to system instance or nullptr. + public: std::optional LoadPlugin( + const sdf::Plugin &_plugin); + /// \brief Makes a printable string with info about systems /// \returns A pretty string public: std::string PrettyStr() const; diff --git a/include/ignition/gazebo/components/Visual.hh b/include/ignition/gazebo/components/Visual.hh index de94818059..dc88a79d2b 100644 --- a/include/ignition/gazebo/components/Visual.hh +++ b/include/ignition/gazebo/components/Visual.hh @@ -81,11 +81,20 @@ namespace components IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Visual", Visual) /// \brief A component that contains a visual plugin's SDF element. + /// \todo(nkoenig) Deprecate this in Garden using VisualPlugin = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.VisualPlugin", VisualPlugin) + + /// \brief A component that contains a visual plugin's SDF objects. + using VisualPlugins = Component>; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.VisualPlugins", + VisualPlugins) + } } } diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 81fa242484..16a14baf6f 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -229,8 +229,8 @@ void LevelManager::ReadLevelPerformerInfo() this->ConfigureDefaultLevel(); // Load world plugins. - this->runner->EventMgr().Emit(this->worldEntity, - this->runner->sdfWorld->Element()); + this->runner->EventMgr().Emit(this->worldEntity, + this->runner->sdfWorld->Plugins()); // Store the world's SDF DOM to be used when saving the world to file this->runner->entityCompMgr.CreateComponent( diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index d54e1f377f..cf1ea82830 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -89,15 +89,15 @@ class ignition::gazebo::SdfEntityCreatorPrivate /// \brief Keep track of new sensors being added, so we load their plugins /// only after we have their scoped name. - public: std::map newSensors; + public: std::map newSensors; /// \brief Keep track of new models being added, so we load their plugins /// only after we have their scoped name. - public: std::map newModels; + public: std::map newModels; /// \brief Keep track of new visuals being added, so we load their plugins /// only after we have their scoped name. - public: std::map newVisuals; + public: std::map newVisuals; }; using namespace ignition; @@ -321,8 +321,8 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) this->dataPtr->ecm->CreateComponent(worldEntity, components::MagneticField(_world->MagneticField())); - this->dataPtr->eventManager->Emit(worldEntity, - _world->Element()); + this->dataPtr->eventManager->Emit(worldEntity, + _world->Plugins()); // Store the world's SDF DOM to be used when saving the world to file this->dataPtr->ecm->CreateComponent( @@ -339,23 +339,23 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model) auto ent = this->CreateEntities(_model, false); // Load all model plugins afterwards, so we get scoped name for nested models. - for (const auto &[entity, element] : this->dataPtr->newModels) + for (const auto &[entity, plugins] : this->dataPtr->newModels) { - this->dataPtr->eventManager->Emit(entity, element); + this->dataPtr->eventManager->Emit(entity, plugins); } this->dataPtr->newModels.clear(); // Load sensor plugins after model, so we get scoped name. - for (const auto &[entity, element] : this->dataPtr->newSensors) + for (const auto &[entity, plugins] : this->dataPtr->newSensors) { - this->dataPtr->eventManager->Emit(entity, element); + this->dataPtr->eventManager->Emit(entity, plugins); } this->dataPtr->newSensors.clear(); // Load visual plugins after model, so we get scoped name. - for (const auto &[entity, element] : this->dataPtr->newVisuals) + for (const auto &[entity, plugins] : this->dataPtr->newVisuals) { - this->dataPtr->eventManager->Emit(entity, element); + this->dataPtr->eventManager->Emit(entity, plugins); } this->dataPtr->newVisuals.clear(); @@ -382,8 +382,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model, modelEntity, components::WindMode(_model->EnableWind())); this->dataPtr->ecm->CreateComponent( modelEntity, components::SelfCollide(_model->SelfCollide())); - this->dataPtr->ecm->CreateComponent( - modelEntity, components::SourceFilePath(_model->Element()->FilePath())); + if (_model->Element()) + { + this->dataPtr->ecm->CreateComponent( + modelEntity, components::SourceFilePath(_model->Element()->FilePath())); + } // NOTE: Pose components of links, visuals, and collisions are expressed in // the parent frame until we get frames working. @@ -450,7 +453,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model, << canonicalLinkPair.second << "\n"; } } - else if(!isStatic) + else if (!isStatic) { ignerr << "Could not resolve the canonical link for " << _model->Name() << "\n"; @@ -462,7 +465,8 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model, // Keep track of models so we can load their plugins after loading the entire // model and having its full scoped name. - this->dataPtr->newModels[modelEntity] = _model->Element(); + if (!_model->Plugins().empty()) + this->dataPtr->newModels[modelEntity] = _model->Plugins(); return modelEntity; } @@ -483,8 +487,8 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Actor *_actor) components::Name(_actor->Name())); // Actor plugins - this->dataPtr->eventManager->Emit(actorEntity, - _actor->Element()); + this->dataPtr->eventManager->Emit(actorEntity, + _actor->Plugins()); return actorEntity; } @@ -750,19 +754,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) } // store the plugin in a component - if (_visual->Element()) + if (!_visual->Plugins().empty()) { - sdf::ElementPtr pluginElem = _visual->Element()->FindElement("plugin"); - if (pluginElem) - { - this->dataPtr->ecm->CreateComponent(visualEntity, - components::VisualPlugin(pluginElem)); - } + this->dataPtr->ecm->CreateComponent(visualEntity, + components::VisualPlugins(_visual->Plugins())); } // Keep track of visuals so we can load their plugins after loading the // entire model and having its full scoped name. - this->dataPtr->newVisuals[visualEntity] = _visual->Element(); + if (!_visual->Plugins().empty()) + this->dataPtr->newVisuals[visualEntity] = _visual->Plugins(); return visualEntity; } @@ -959,7 +960,8 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) // Keep track of sensors so we can load their plugins after loading the entire // model and having its full scoped name. - this->dataPtr->newSensors[sensorEntity] = _sensor->Element(); + if (!_sensor->Plugins().empty()) + this->dataPtr->newSensors[sensorEntity] = _sensor->Plugins(); return sensorEntity; } diff --git a/src/Server.cc b/src/Server.cc index 751faa2021..fbb65fb2f5 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -76,7 +76,7 @@ Server::Server(const ServerConfig &_config) // Load a world if specified. Check SDF string first, then SDF file if (_config.SdfRoot()) { - this->dataPtr->sdfRoot.Clone(*_config.SdfRoot()); + this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); ignmsg << "Loading SDF world from SDF DOM.\n"; } else if (!_config.SdfString().empty()) diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 99eb6550aa..3b6eae6f8b 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -43,10 +43,10 @@ class ignition::gazebo::ServerConfig::PluginInfoPrivate : entityName(_info->entityName), entityType(_info->entityType), filename(_info->filename), - name(_info->name) + name(_info->name), + plugin(_info->plugin) { - if (_info->sdf) - this->sdf = _info->sdf->Clone(); + this->sdf = plugin.Element(); } /// \brief Constructor based on values. @@ -56,23 +56,20 @@ class ignition::gazebo::ServerConfig::PluginInfoPrivate /// \param[in] _entityType Entity type which should receive this /// plugin. The type is used in conjuction with _entityName to /// uniquely identify an entity. - /// \param[in] _filename Plugin library filename. - /// \param[in] _name Name of the interface within the plugin library - /// to load. - /// \param[in] _sdf Plugin XML elements associated with this plugin. + /// \param[in] _plugin SDF Plugin associated with this plugin. // cppcheck-suppress passedByValue public: PluginInfoPrivate(std::string _entityName, // cppcheck-suppress passedByValue std::string _entityType, // cppcheck-suppress passedByValue - std::string _filename, - // cppcheck-suppress passedByValue - std::string _name) + sdf::Plugin _plugin) : entityName(std::move(_entityName)), entityType(std::move(_entityType)), - filename(std::move(_filename)), - name(std::move(_name)) + plugin(std::move(_plugin)) { + this->filename = this->plugin.Filename(); + this->name = this->plugin.Name(); + this->sdf = this->plugin.Element(); } /// \brief The name of the entity. @@ -89,6 +86,9 @@ class ignition::gazebo::ServerConfig::PluginInfoPrivate /// \brief XML elements associated with this plugin public: sdf::ElementPtr sdf = nullptr; + + /// \brief SDF plugin information. + public: sdf::Plugin plugin; }; ////////////////////////////////////////////////// @@ -106,11 +106,28 @@ ServerConfig::PluginInfo::PluginInfo(const std::string &_entityName, const std::string &_filename, const std::string &_name, const sdf::ElementPtr &_sdf) - : dataPtr(new ServerConfig::PluginInfoPrivate(_entityName, _entityType, - _filename, _name)) + : dataPtr(new ServerConfig::PluginInfoPrivate()) { if (_sdf) + { this->dataPtr->sdf = _sdf->Clone(); + this->dataPtr->plugin.Load(this->dataPtr->sdf); + } + this->dataPtr->plugin.SetName(_name); + this->dataPtr->plugin.SetFilename(_filename); + this->dataPtr->filename = _filename; + this->dataPtr->name = _name; + this->dataPtr->entityName = _entityName; + this->dataPtr->entityType = _entityType; +} + +////////////////////////////////////////////////// +ServerConfig::PluginInfo::PluginInfo(const std::string &_entityName, + const std::string &_entityType, + const sdf::Plugin &_plugin) + : dataPtr(new ServerConfig::PluginInfoPrivate(_entityName, _entityType, + _plugin)) +{ } ////////////////////////////////////////////////// @@ -161,6 +178,7 @@ const std::string &ServerConfig::PluginInfo::Filename() const ////////////////////////////////////////////////// void ServerConfig::PluginInfo::SetFilename(const std::string &_filename) { + this->dataPtr->plugin.SetFilename(_filename); this->dataPtr->filename = _filename; } @@ -173,6 +191,7 @@ const std::string &ServerConfig::PluginInfo::Name() const ////////////////////////////////////////////////// void ServerConfig::PluginInfo::SetName(const std::string &_name) { + this->dataPtr->plugin.SetName(_name); this->dataPtr->name = _name; } @@ -186,11 +205,34 @@ const sdf::ElementPtr &ServerConfig::PluginInfo::Sdf() const void ServerConfig::PluginInfo::SetSdf(const sdf::ElementPtr &_sdf) { if (_sdf) + { this->dataPtr->sdf = _sdf->Clone(); + this->dataPtr->plugin.Load(_sdf); + } else this->dataPtr->sdf = nullptr; } +////////////////////////////////////////////////// +const sdf::Plugin &ServerConfig::PluginInfo::Plugin() const +{ + return this->dataPtr->plugin; +} + +////////////////////////////////////////////////// +sdf::Plugin &ServerConfig::PluginInfo::Plugin() +{ + return this->dataPtr->plugin; +} + +////////////////////////////////////////////////// +void ServerConfig::PluginInfo::SetPlugin(const sdf::Plugin &_plugin) const +{ + this->dataPtr->plugin = _plugin; + this->dataPtr->filename = _plugin.Filename(); + this->dataPtr->name = _plugin.Name(); +} + /// \brief Private data for ServerConfig. class ignition::gazebo::ServerConfigPrivate { @@ -823,9 +865,11 @@ parsePluginsFromDoc(const tinyxml2::XMLDocument &_doc) // Create an SDF element of the plugin sdf::ElementPtr sdf(new sdf::Element); copyElement(sdf, elem); + sdf::Plugin plugin; + plugin.Load(sdf); // Add the plugin to the server config - ret.push_back({entityName, entityType, file, name, sdf}); + ret.push_back({entityName, entityType, plugin}); } return ret; } @@ -848,7 +892,6 @@ ignition::gazebo::parsePluginsFromString(const std::string &_str) return parsePluginsFromDoc(doc); } - ///////////////////////////////////////////////// std::list ignition::gazebo::loadPluginInfo(bool _isPlayback) diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 7bd36065ce..83504ddbb5 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -184,78 +184,54 @@ bool ServerPrivate::Run(const uint64_t _iterations, return result; } -////////////////////////////////////////////////// -sdf::ElementPtr GetRecordPluginElem(sdf::Root &_sdfRoot) -{ - sdf::ElementPtr rootElem = _sdfRoot.Element(); - - if (rootElem->HasElement("world")) - { - sdf::ElementPtr worldElem = rootElem->GetElement("world"); - - if (worldElem->HasElement("plugin")) - { - sdf::ElementPtr pluginElem = worldElem->GetElement("plugin"); - - while (pluginElem != nullptr) - { - sdf::ParamPtr pluginName = pluginElem->GetAttribute("name"); - sdf::ParamPtr pluginFileName = pluginElem->GetAttribute("filename"); - - if (pluginName != nullptr && pluginFileName != nullptr) - { - // Found a logging plugin - if (pluginFileName->GetAsString().find( - LoggingPlugin::LoggingPluginSuffix()) != std::string::npos) - { - if (pluginName->GetAsString() == LoggingPlugin::RecordPluginName()) - { - return pluginElem; - } - } - } - - pluginElem = pluginElem->GetNextElement(); - } - } - } - return nullptr; -} - ////////////////////////////////////////////////// void ServerPrivate::AddRecordPlugin(const ServerConfig &_config) { - auto recordPluginElem = GetRecordPluginElem(this->sdfRoot); - bool sdfUseLogRecord = (recordPluginElem != nullptr); - bool hasRecordResources {false}; bool hasRecordTopics {false}; bool sdfRecordResources; std::vector sdfRecordTopics; - if (sdfUseLogRecord) + for (uint64_t worldIndex = 0; worldIndex < this->sdfRoot.WorldCount(); + ++worldIndex) { - std::tie(sdfRecordResources, hasRecordResources) = - recordPluginElem->Get("record_resources", false); + sdf::World *world = this->sdfRoot.WorldByIndex(worldIndex); + sdf::Plugins &plugins = world->Plugins(); - hasRecordTopics = recordPluginElem->HasElement("record_topic"); - if (hasRecordTopics) + for (sdf::Plugins::iterator iter = plugins.begin(); + iter != plugins.end(); ++iter) { - sdf::ElementPtr recordTopicElem = - recordPluginElem->GetElement("record_topic"); - while (recordTopicElem) + std::string fname = iter->Filename(); + std::string name = iter->Name(); + if (fname.find( + LoggingPlugin::LoggingPluginSuffix()) != std::string::npos && + name == LoggingPlugin::RecordPluginName()) { - auto topic = recordTopicElem->Get(); - sdfRecordTopics.push_back(topic); - } + sdf::ElementPtr recordPluginElem = iter->ToElement(); - recordTopicElem = recordTopicElem->GetNextElement(); - } + std::tie(sdfRecordResources, hasRecordResources) = + recordPluginElem->Get("record_resources", false); - // Remove from SDF - recordPluginElem->RemoveFromParent(); - recordPluginElem->Reset(); + hasRecordTopics = recordPluginElem->HasElement("record_topic"); + if (hasRecordTopics) + { + sdf::ElementPtr recordTopicElem = + recordPluginElem->GetElement("record_topic"); + while (recordTopicElem) + { + auto topic = recordTopicElem->Get(); + sdfRecordTopics.push_back(topic); + } + + recordTopicElem = recordTopicElem->GetNextElement(); + } + + // Remove the plugin, which will be added back in by ServerConfig. + plugins.erase(iter); + break; + } + } } // Set the config based on what is in the SDF: diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index a2c6d1803d..479448f277 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -423,6 +423,7 @@ TEST_P(ServerFixture, SdfStringServerConfig) serverConfig.SetSdfString(TestWorldSansPhysics::World()); EXPECT_TRUE(serverConfig.SdfFile().empty()); EXPECT_FALSE(serverConfig.SdfString().empty()); + EXPECT_FALSE(serverConfig.SdfRoot()); gazebo::Server server(serverConfig); EXPECT_FALSE(server.Running()); diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 9264a41713..e9824d4863 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -121,10 +121,14 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, this->stopConn = this->eventMgr.Connect( std::bind(&SimulationRunner::OnStop, this)); - this->loadPluginsConn = this->eventMgr.Connect( + this->loadPluginsConn = this->eventMgr.Connect( std::bind(&SimulationRunner::LoadPlugins, this, std::placeholders::_1, std::placeholders::_2)); + this->loadPtrPluginsConn = this->eventMgr.Connect( + std::bind(&SimulationRunner::LoadPtrPlugins, this, std::placeholders::_1, + std::placeholders::_2)); + // Create the level manager this->levelMgr = std::make_unique(this, _config.UseLevels()); @@ -918,6 +922,25 @@ void SimulationRunner::LoadPlugin(const Entity _entity, } } +////////////////////////////////////////////////// +void SimulationRunner::LoadPlugin(const Entity _entity, + const sdf::Plugin &_plugin) +{ + std::optional system; + { + std::lock_guard lock(this->systemLoaderMutex); + system = this->systemLoader->LoadPlugin(_plugin); + } + + // System correctly loaded from library + if (system) + { + this->AddSystem(system.value(), _entity, _plugin.ToElement()); + igndbg << "Loaded system [" << _plugin.Name() + << "] for entity [" << _entity << "]" << std::endl; + } +} + ////////////////////////////////////////////////// void SimulationRunner::LoadServerPlugins( const std::list &_plugins) @@ -995,7 +1018,7 @@ void SimulationRunner::LoadServerPlugins( if (kNullEntity != entity) { - this->LoadPlugin(entity, plugin.Filename(), plugin.Name(), plugin.Sdf()); + this->LoadPlugin(entity, plugin.Plugin()); } } } @@ -1026,7 +1049,7 @@ void SimulationRunner::LoadLoggingPlugins(const ServerConfig &_config) } ////////////////////////////////////////////////// -void SimulationRunner::LoadPlugins(const Entity _entity, +void SimulationRunner::LoadPtrPlugins(const Entity _entity, const sdf::ElementPtr &_sdf) { sdf::ElementPtr pluginElem = _sdf->FindElement("plugin"); @@ -1047,6 +1070,25 @@ void SimulationRunner::LoadPlugins(const Entity _entity, } } +////////////////////////////////////////////////// +void SimulationRunner::LoadPlugins(const Entity _entity, + const sdf::Plugins &_plugins) +{ + for (const sdf::Plugin plugin : _plugins) + { + auto filename = plugin.Filename(); + auto name = plugin.Name(); + // No error message for the 'else' case of the following 'if' statement + // because SDF create a default element even if it's not + // specified. An error message would result in spamming + // the console. + if (filename != "__default__" && name != "__default__") + { + this->LoadPlugin(_entity, plugin); + } + } +} + ///////////////////////////////////////////////// bool SimulationRunner::Running() const { diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 5da194e209..8ff8750084 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -228,6 +228,7 @@ namespace ignition /// \param[in] _fname Filename of the plugin library /// \param[in] _name Name of the plugin /// \param[in] _sdf SDF element (content of plugin tag) + /// \todo(nkoenig) Remove this in Garden. public: void LoadPlugin(const Entity _entity, const std::string &_fname, const std::string &_name, @@ -236,9 +237,21 @@ namespace ignition /// \brief Load system plugins for a given entity. /// \param[in] _entity Entity /// \param[in] _sdf SDF element - public: void LoadPlugins(const Entity _entity, + /// \todo(nkoenig) Remove this in Garden. + public: void LoadPtrPlugins(const Entity _entity, const sdf::ElementPtr &_sdf); + /// \brief Load system plugin for a given entity. + /// \param[in] _entity Entity + /// \param[in] _plugin SDF Plugin to load + public: void LoadPlugin(const Entity _entity, const sdf::Plugin &_plugin); + + /// \brief Load system plugins for a given entity. + /// \param[in] _entity Entity + /// \param[in] _plugins SDF Plugins to load + public: void LoadPlugins(const Entity _entity, + const sdf::Plugins &_plugins); + /// \brief Load server plugins for a given entity. /// \param[in] _config Configuration to load plugins from. /// plugins based on the _config contents @@ -578,6 +591,10 @@ namespace ignition /// \brief Connection to the load plugins event. private: common::ConnectionPtr loadPluginsConn; + /// \brief Connection to the sdf element pointer load plugins event. + /// \todo(nkoenig) Remove this in Garden. + private: common::ConnectionPtr loadPtrPluginsConn; + /// \brief Pointer to the sdf::World object of this runner private: const sdf::World *sdfWorld; diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index 3a78dda509..8a151d92ca 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -109,6 +109,73 @@ class ignition::gazebo::SystemLoaderPrivate return true; } + ////////////////////////////////////////////////// + public: bool InstantiateSystemPlugin(const sdf::Plugin &_plugin, + ignition::plugin::PluginPtr &_ignPlugin) + { + ignition::common::SystemPaths systemPaths; + systemPaths.SetPluginPathEnv(pluginPathEnv); + + for (const auto &path : this->systemPluginPaths) + systemPaths.AddPluginPaths(path); + + std::string homePath; + ignition::common::env(IGN_HOMEDIR, homePath); + systemPaths.AddPluginPaths(homePath + "/.ignition/gazebo/plugins"); + systemPaths.AddPluginPaths(IGN_GAZEBO_PLUGIN_INSTALL_DIR); + + auto pathToLib = systemPaths.FindSharedLibrary(_plugin.Filename()); + if (pathToLib.empty()) + { + // We assume ignition::gazebo corresponds to the levels feature + if (_plugin.Name() != "ignition::gazebo") + { + ignerr << "Failed to load system plugin [" << _plugin.Filename() << + "] : couldn't find shared library." << std::endl; + } + return false; + } + + auto pluginNames = this->loader.LoadLib(pathToLib); + if (pluginNames.empty()) + { + ignerr << "Failed to load system plugin [" << _plugin.Filename() << + "] : couldn't load library on path [" << pathToLib << + "]." << std::endl; + return false; + } + + auto pluginName = *pluginNames.begin(); + if (pluginName.empty()) + { + ignerr << "Failed to load system plugin [" << _plugin.Filename() << + "] : couldn't load library on path [" << pathToLib << + "]." << std::endl; + return false; + } + + _ignPlugin = this->loader.Instantiate(_plugin.Name()); + if (!_ignPlugin) + { + ignerr << "Failed to load system plugin [" << _plugin.Name() << + "] : could not instantiate from library [" << _plugin.Filename() << + "] from path [" << pathToLib << "]." << std::endl; + return false; + } + + if (!_ignPlugin->HasInterface()) + { + ignerr << "Failed to load system plugin [" << _plugin.Name() << + "] : system not found in library [" << _plugin.Filename() << + "] from path [" << pathToLib << "]." << std::endl; + + return false; + } + + this->systemPluginsAdded.insert(_ignPlugin); + return true; + } + // Default plugin search path environment variable public: std::string pluginPathEnv{"IGN_GAZEBO_SYSTEM_PLUGIN_PATH"}; @@ -177,6 +244,26 @@ std::optional SystemLoader::LoadPlugin( return LoadPlugin(filename, pluginName, _sdf); } +////////////////////////////////////////////////// +std::optional SystemLoader::LoadPlugin( + const sdf::Plugin &_plugin) +{ + ignition::plugin::PluginPtr plugin; + + if (_plugin.Filename() == "" || _plugin.Name() == "") + { + ignerr << "Failed to instantiate system plugin: empty argument " + "[(filename): " << _plugin.Filename() << "] " << + "[(name): " << _plugin.Name() << "]." << std::endl; + return {}; + } + + auto ret = this->dataPtr->InstantiateSystemPlugin(_plugin, plugin); + if (ret && plugin) + return plugin; + + return {}; +} ////////////////////////////////////////////////// std::string SystemLoader::PrettyStr() const { From f32555b2884167d7da28613229f447dbfaace483 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Feb 2022 13:35:24 -0800 Subject: [PATCH 04/14] Address review comments Signed-off-by: Nate Koenig --- include/ignition/gazebo/ServerConfig.hh | 22 ++++++- include/ignition/gazebo/Util.hh | 3 +- src/Server.cc | 87 ++++++++++++++----------- src/ServerConfig.cc | 14 +++- src/Util_TEST.cc | 1 - 5 files changed, 85 insertions(+), 42 deletions(-) diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index 663459c6b6..bdc1ce277f 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -37,6 +37,22 @@ namespace ignition // Forward declarations. class ServerConfigPrivate; + /// \brief Type of SDF source. + enum class SourceType + { + // The source is an SDF file. + kSdfFile, + + // No source specified. + kNone, + + // The source is an SDF Root object. + kSdfRoot, + + // The source is an SDF string. + kSdfString, + }; + /// \class ServerConfig ServerConfig.hh ignition/gazebo/ServerConfig.hh /// \brief Configuration parameters for a Server. An instance of this /// object can be used to construct a Server with a particular @@ -185,7 +201,7 @@ namespace ignition /// \brief Get the SDF Root DOM object. /// \return SDF Root object to use, or std::nullopt if the sdf::Root /// has not been set via ServerConfig::SetSdfRoot(). - public: const std::optional &SdfRoot() const; + public: std::optional &SdfRoot() const; /// \brief Set the update rate in Hertz. Value <=0 are ignored. /// \param[in] _hz The desired update rate of the server in Hertz. @@ -395,6 +411,10 @@ namespace ignition public: const std::chrono::time_point & Timestamp() const; + /// \brief Get the type of source + /// \return The source type. + public: SourceType Source() const; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 9dcd4b273a..18cc6798c9 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -219,7 +219,8 @@ namespace ignition /// 2. "../shapes.sdf" - This is referencing a relative world file. /// 3. "/home/user/shapes.sdf" - This is reference an absolute world /// file. - /// 4. "https://fuel.ignitionrobotics.org/1.0/openrobotics/worlds/shapes.sdf" - This is referencing a Fuel URI. This will download the world file. + /// 4. "https://fuel.ignitionrobotics.org/1.0/openrobotics/worlds/shapes.sdf" + /// This is referencing a Fuel URI. This will download the world file. /// \param[in] _fuelResourceCache Path to a Fuel resource cache, if /// known. /// \return Full path to the SDF world file. An empty string is returned diff --git a/src/Server.cc b/src/Server.cc index fbb65fb2f5..eee8862f41 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -73,53 +73,64 @@ Server::Server(const ServerConfig &_config) sdf::Errors errors; - // Load a world if specified. Check SDF string first, then SDF file - if (_config.SdfRoot()) + switch (_config.Source()) { - this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); - ignmsg << "Loading SDF world from SDF DOM.\n"; - } - else if (!_config.SdfString().empty()) - { - std::string msg = "Loading SDF string. "; - if (_config.SdfFile().empty()) + // Load a world if specified. Check SDF string first, then SDF file + case SourceType::kSdfRoot: { - msg += "File path not available.\n"; + this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); + ignmsg << "Loading SDF world from SDF DOM.\n"; + break; } - else + + case SourceType::kSdfString: { - msg += "File path [" + _config.SdfFile() + "].\n"; + std::string msg = "Loading SDF string. "; + if (_config.SdfFile().empty()) + { + msg += "File path not available.\n"; + } + else + { + msg += "File path [" + _config.SdfFile() + "].\n"; + } + ignmsg << msg; + errors = this->dataPtr->sdfRoot.LoadSdfString(_config.SdfString()); + break; } - ignmsg << msg; - errors = this->dataPtr->sdfRoot.LoadSdfString(_config.SdfString()); - } - else if (!_config.SdfFile().empty()) - { - std::string filePath = resolveSdfWorldFile(_config.SdfFile(), - _config.ResourceCache()); - if (filePath.empty()) + case SourceType::kSdfFile: { - ignerr << "Failed to find world [" << _config.SdfFile() << "]" - << std::endl; - return; + std::string filePath = resolveSdfWorldFile(_config.SdfFile(), + _config.ResourceCache()); + + if (filePath.empty()) + { + ignerr << "Failed to find world [" << _config.SdfFile() << "]" + << std::endl; + return; + } + + ignmsg << "Loading SDF world file[" << filePath << "].\n"; + + // \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 = this->dataPtr->sdfRoot.Load(filePath); + break; } - ignmsg << "Loading SDF world file[" << filePath << "].\n"; - - // \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 = this->dataPtr->sdfRoot.Load(filePath); - } - else - { - ignmsg << "Loading default world.\n"; - // Load an empty world. - /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root. - errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World()); + case SourceType::kNone: + default: + { + ignmsg << "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; + } } if (!errors.empty()) diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 99eb6550aa..7101d211ba 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -304,6 +304,9 @@ class ignition::gazebo::ServerConfigPrivate /// \brief Optional SDF root object. public: std::optional sdfRoot; + + /// \brief Type of source used. + public: SourceType source{SourceType::kNone}; }; ////////////////////////////////////////////////// @@ -324,6 +327,7 @@ ServerConfig::~ServerConfig() = default; ////////////////////////////////////////////////// bool ServerConfig::SetSdfFile(const std::string &_file) { + this->dataPtr->source = SourceType::kSdfFile; this->dataPtr->sdfFile = _file; this->dataPtr->sdfString = ""; this->dataPtr->sdfRoot = std::nullopt; @@ -339,6 +343,7 @@ std::string ServerConfig::SdfFile() const ////////////////////////////////////////////////// bool ServerConfig::SetSdfString(const std::string &_sdfString) { + this->dataPtr->source = SourceType::kSdfString; this->dataPtr->sdfFile = ""; this->dataPtr->sdfString = _sdfString; this->dataPtr->sdfRoot = std::nullopt; @@ -705,6 +710,7 @@ const std::vector &ServerConfig::LogRecordTopics() const ///////////////////////////////////////////////// void ServerConfig::SetSdfRoot(const sdf::Root &_root) const { + this->dataPtr->source = SourceType::kSdfRoot; this->dataPtr->sdfRoot.emplace(); for (uint64_t i = 0; i < _root.WorldCount(); ++i) @@ -719,11 +725,17 @@ void ServerConfig::SetSdfRoot(const sdf::Root &_root) const } ///////////////////////////////////////////////// -const std::optional &ServerConfig::SdfRoot() const +std::optional &ServerConfig::SdfRoot() const { return this->dataPtr->sdfRoot; } +///////////////////////////////////////////////// +SourceType ServerConfig::Source() const +{ + return this->dataPtr->source; +} + ///////////////////////////////////////////////// void copyElement(sdf::ElementPtr _sdf, const tinyxml2::XMLElement *_xml) { diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 388661193e..3f2b4443a9 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -39,7 +39,6 @@ #include "ignition/gazebo/Util.hh" #include "helpers/EnvTestFixture.hh" -#include "helpers/UniqueTestDirectoryEnv.hh" using namespace ignition; using namespace gazebo; From 7d33cf0aafacefa3a4bdaa1522ce02b7d7b5263b Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Feb 2022 14:17:40 -0800 Subject: [PATCH 05/14] Fix linter, and added tests Signed-off-by: Nate Koenig --- include/ignition/gazebo/ServerConfig.hh | 33 +++++++++++++------------ src/Server.cc | 8 +++--- src/ServerConfig.cc | 10 ++++---- src/ServerConfig_TEST.cc | 4 +++ src/Util.cc | 5 ++-- 5 files changed, 32 insertions(+), 28 deletions(-) diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index bdc1ce277f..7311d092e8 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -37,28 +37,29 @@ namespace ignition // Forward declarations. class ServerConfigPrivate; - /// \brief Type of SDF source. - enum class SourceType - { - // The source is an SDF file. - kSdfFile, - - // No source specified. - kNone, - - // The source is an SDF Root object. - kSdfRoot, - - // The source is an SDF string. - kSdfString, - }; - /// \class ServerConfig ServerConfig.hh ignition/gazebo/ServerConfig.hh /// \brief Configuration parameters for a Server. An instance of this /// object can be used to construct a Server with a particular /// configuration. class IGNITION_GAZEBO_VISIBLE ServerConfig { + /// \brief Type of SDF source. + public: enum class SourceType + { + // No source specified. + kNone, + + // The source is an SDF Root object. + kSdfRoot, + + // The source is an SDF file. + kSdfFile, + + // The source is an SDF string. + kSdfString, + }; + + class PluginInfoPrivate; /// \brief Information about a plugin that should be loaded by the /// server. diff --git a/src/Server.cc b/src/Server.cc index eee8862f41..87381ee516 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -76,14 +76,14 @@ Server::Server(const ServerConfig &_config) switch (_config.Source()) { // Load a world if specified. Check SDF string first, then SDF file - case SourceType::kSdfRoot: + case ServerConfig::SourceType::kSdfRoot: { this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); ignmsg << "Loading SDF world from SDF DOM.\n"; break; } - case SourceType::kSdfString: + case ServerConfig::SourceType::kSdfString: { std::string msg = "Loading SDF string. "; if (_config.SdfFile().empty()) @@ -99,7 +99,7 @@ Server::Server(const ServerConfig &_config) break; } - case SourceType::kSdfFile: + case ServerConfig::SourceType::kSdfFile: { std::string filePath = resolveSdfWorldFile(_config.SdfFile(), _config.ResourceCache()); @@ -122,7 +122,7 @@ Server::Server(const ServerConfig &_config) break; } - case SourceType::kNone: + case ServerConfig::SourceType::kNone: default: { ignmsg << "Loading default world.\n"; diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 7101d211ba..1024eb5d11 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -306,7 +306,7 @@ class ignition::gazebo::ServerConfigPrivate public: std::optional sdfRoot; /// \brief Type of source used. - public: SourceType source{SourceType::kNone}; + public: ServerConfig::SourceType source{ServerConfig::SourceType::kNone}; }; ////////////////////////////////////////////////// @@ -327,7 +327,7 @@ ServerConfig::~ServerConfig() = default; ////////////////////////////////////////////////// bool ServerConfig::SetSdfFile(const std::string &_file) { - this->dataPtr->source = SourceType::kSdfFile; + this->dataPtr->source = ServerConfig::SourceType::kSdfFile; this->dataPtr->sdfFile = _file; this->dataPtr->sdfString = ""; this->dataPtr->sdfRoot = std::nullopt; @@ -343,7 +343,7 @@ std::string ServerConfig::SdfFile() const ////////////////////////////////////////////////// bool ServerConfig::SetSdfString(const std::string &_sdfString) { - this->dataPtr->source = SourceType::kSdfString; + this->dataPtr->source = ServerConfig::SourceType::kSdfString; this->dataPtr->sdfFile = ""; this->dataPtr->sdfString = _sdfString; this->dataPtr->sdfRoot = std::nullopt; @@ -710,7 +710,7 @@ const std::vector &ServerConfig::LogRecordTopics() const ///////////////////////////////////////////////// void ServerConfig::SetSdfRoot(const sdf::Root &_root) const { - this->dataPtr->source = SourceType::kSdfRoot; + this->dataPtr->source = ServerConfig::SourceType::kSdfRoot; this->dataPtr->sdfRoot.emplace(); for (uint64_t i = 0; i < _root.WorldCount(); ++i) @@ -731,7 +731,7 @@ std::optional &ServerConfig::SdfRoot() const } ///////////////////////////////////////////////// -SourceType ServerConfig::Source() const +ServerConfig::SourceType ServerConfig::Source() const { return this->dataPtr->source; } diff --git a/src/ServerConfig_TEST.cc b/src/ServerConfig_TEST.cc index 8486b2d70d..bc93120ef2 100644 --- a/src/ServerConfig_TEST.cc +++ b/src/ServerConfig_TEST.cc @@ -235,20 +235,24 @@ TEST(ServerConfig, SdfRoot) EXPECT_FALSE(config.SdfRoot()); EXPECT_TRUE(config.SdfFile().empty()); EXPECT_TRUE(config.SdfString().empty()); + EXPECT_EQ(ServerConfig::SourceType::kNone, config.Source()); config.SetSdfString("string"); EXPECT_FALSE(config.SdfRoot()); EXPECT_TRUE(config.SdfFile().empty()); EXPECT_FALSE(config.SdfString().empty()); + EXPECT_EQ(ServerConfig::SourceType::kSdfString, config.Source()); config.SetSdfFile("file"); EXPECT_FALSE(config.SdfRoot()); EXPECT_FALSE(config.SdfFile().empty()); EXPECT_TRUE(config.SdfString().empty()); + EXPECT_EQ(ServerConfig::SourceType::kSdfFile, config.Source()); sdf::Root root; config.SetSdfRoot(root); EXPECT_TRUE(config.SdfRoot()); EXPECT_TRUE(config.SdfFile().empty()); EXPECT_TRUE(config.SdfString().empty()); + EXPECT_EQ(ServerConfig::SourceType::kSdfRoot, config.Source()); } diff --git a/src/Util.cc b/src/Util.cc index 39a3f16303..09babb612c 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -609,8 +609,8 @@ std::string findFuelResourceSdf(const std::string &_path) } ////////////////////////////////////////////////// -std::string IGNITION_GAZEBO_VISIBLE resolveSdfWorldFile( - const std::string &_sdfFile, const std::string &_fuelResourceCache) +std::string resolveSdfWorldFile(const std::string &_sdfFile, + const std::string &_fuelResourceCache) { std::string filePath; @@ -659,4 +659,3 @@ std::string IGNITION_GAZEBO_VISIBLE resolveSdfWorldFile( } } } - From 711761d9429b7cc4ee79f5f57024d8aa609ca9a0 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 9 Mar 2022 08:30:52 -0800 Subject: [PATCH 06/14] Update branch Signed-off-by: Nate Koenig --- src/SimulationRunner.cc | 14 +------------- src/SystemManager.cc | 20 ++++++++++++++++++++ src/SystemManager.hh | 8 ++++++++ 3 files changed, 29 insertions(+), 13 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index b62b7afed4..fc461212cc 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -870,19 +870,7 @@ void SimulationRunner::LoadPlugin(const Entity _entity, void SimulationRunner::LoadPlugin(const Entity _entity, const sdf::Plugin &_plugin) { - std::optional system; - { - std::lock_guard lock(this->systemLoaderMutex); - system = this->systemLoader->LoadPlugin(_plugin); - } - - // System correctly loaded from library - if (system) - { - this->AddSystem(system.value(), _entity, _plugin.ToElement()); - igndbg << "Loaded system [" << _plugin.Name() - << "] for entity [" << _entity << "]" << std::endl; - } + this->systemMgr->LoadPlugin(_entity, _plugin); } ////////////////////////////////////////////////// diff --git a/src/SystemManager.cc b/src/SystemManager.cc index ba716f99a9..4374f9be51 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -30,6 +30,26 @@ SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader, { } +////////////////////////////////////////////////// +void SystemManager::LoadPlugin(const Entity _entity, + const sdf::Plugin &_plugin) +{ + std::optional system; + { + std::lock_guard lock(this->systemLoaderMutex); + system = this->systemLoader->LoadPlugin(_plugin); + } + + // System correctly loaded from library + if (system) + { + this->AddSystem(system.value(), _entity, _plugin.ToElement()); + igndbg << "Loaded system [" << _plugin.Name() + << "] for entity [" << _entity << "]" << std::endl; + } + +} + ////////////////////////////////////////////////// void SystemManager::LoadPlugin(const Entity _entity, const std::string &_fname, diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 9c38505922..7cabc15674 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -21,6 +21,8 @@ #include #include +#include + #include "ignition/gazebo/config.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/Export.hh" @@ -60,6 +62,12 @@ namespace ignition const std::string &_name, const sdf::ElementPtr &_sdf); + /// \brief Load system plugin for a given entity. + /// \param[in] _entity Entity + /// \param[in] _plugin Plugin to load + public: void LoadPlugin(const Entity _entity, + const sdf::Plugin &_plugin); + /// \brief Add a system to the manager /// \param[in] _system SystemPluginPtr to be added /// \param[in] _entity Entity that system is attached to. From d60d34a3675e9f08f0cdb415f30703b41b508902 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 9 Mar 2022 08:51:03 -0800 Subject: [PATCH 07/14] Tweaks Signed-off-by: Nate Koenig --- include/ignition/gazebo/ServerConfig.hh | 2 +- src/SimulationRunner.cc | 2 ++ src/SystemLoader.cc | 17 ++++++++++++++++- 3 files changed, 19 insertions(+), 2 deletions(-) diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index be11f7a7f3..b58a380fae 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -179,7 +179,7 @@ namespace ignition /// \brief Get a mutable version of the SDF plugin information. /// \return The SDF Plugin object. - public: sdf::Plugin &Plugin(); + public: sdf::Plugin &Plugin(); /// \brief Set the SDF plugin information. /// \param[in] _plugin The SDF Plugin object to use. diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index fc461212cc..895d1e782e 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -126,6 +126,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, std::bind(&SimulationRunner::LoadPlugins, this, std::placeholders::_1, std::placeholders::_2)); + /// \todo(nkoenig) Deprecate/remove this event in Garden this->loadPtrPluginsConn = this->eventMgr.Connect( std::bind(&SimulationRunner::LoadPtrPlugins, this, std::placeholders::_1, std::placeholders::_2)); @@ -981,6 +982,7 @@ void SimulationRunner::LoadLoggingPlugins(const ServerConfig &_config) } ////////////////////////////////////////////////// +/// \todo(nkoenig) Remove this function in Garden. void SimulationRunner::LoadPtrPlugins(const Entity _entity, const sdf::ElementPtr &_sdf) { diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index 8a151d92ca..40a35bf467 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -210,7 +210,20 @@ std::optional SystemLoader::LoadPlugin( const std::string &_name, const sdf::ElementPtr &_sdf) { - ignition::plugin::PluginPtr plugin; + if (_filename == "" || _name == "") + { + ignerr << "Failed to instantiate system plugin: empty argument " + "[(filename): " << _filename << "] " << + "[(name): " << _name << "]." << std::endl; + return {}; + } + + sdf::Plugin plugin; + plugin.Load(_sdf); + plugin.SetFilename(_filename); + plugin.SetName(_name); + return LoadPlugin(plugin); + /*ignition::plugin::PluginPtr plugin; if (_filename == "" || _name == "") { @@ -229,6 +242,7 @@ std::optional SystemLoader::LoadPlugin( } return {}; + */ } ////////////////////////////////////////////////// @@ -264,6 +278,7 @@ std::optional SystemLoader::LoadPlugin( return {}; } + ////////////////////////////////////////////////// std::string SystemLoader::PrettyStr() const { From ae00505d8f69c4110dbd74184110392b4d5020e0 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 14 Mar 2022 13:44:36 -0700 Subject: [PATCH 08/14] Updates to match garden forward port Signed-off-by: Nate Koenig --- include/ignition/gazebo/gui/GuiEvents.hh | 23 +++++++ src/ServerConfig.cc | 51 +++++--------- src/ServerConfig_TEST.cc | 34 +++++----- src/Server_TEST.cc | 66 +++++++++++++++++++ src/SimulationRunner.cc | 13 +--- src/SimulationRunner.hh | 11 ---- src/SimulationRunner_TEST.cc | 12 ++-- src/SystemLoader.cc | 26 +------- src/SystemLoader_TEST.cc | 8 ++- src/SystemManager.cc | 22 ------- src/SystemManager.hh | 10 --- src/gui/GuiEvents.cc | 30 +++++++++ src/gui/GuiRunner.cc | 38 +++++++---- .../plugins/scene_manager/GzSceneManager.cc | 33 +++++----- test/integration/battery_plugin.cc | 7 +- test/integration/follow_actor_system.cc | 7 +- test/integration/network_handshake.cc | 17 +++-- test/integration/sensors_system.cc | 7 +- test/integration/wind_effects.cc | 6 +- 19 files changed, 238 insertions(+), 183 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index afbb751879..dec2f049fc 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -29,6 +29,7 @@ #include #include #include +#include #include "ignition/gazebo/gui/Export.hh" #include "ignition/gazebo/Entity.hh" @@ -235,6 +236,28 @@ namespace events /// \brief Private data pointer IGN_UTILS_IMPL_PTR(dataPtr) }; + + /// \brief Event that notifies a visual plugin is to be loaded + class IGNITION_GAZEBO_GUI_VISIBLE VisualPluginSdf: public QEvent + { + /// \brief Constructor + /// \param[in] _entity Visual entity id + /// \param[in] _elem Visual plugin SDF element + public: explicit VisualPluginSdf(ignition::gazebo::Entity _entity, + const sdf::Plugin &_plugin); + + /// \brief Get the entity to load the visual plugin for + public: ignition::gazebo::Entity Entity() const; + + /// \brief Get the sdf plugin of the visual plugin + public: sdf::Plugin Plugin() const; + + static const QEvent::Type kType = QEvent::Type(QEvent::User + 8); + + /// \internal + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) + }; } // namespace events } } // namespace gui diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index e17e249601..47a6ffc9bf 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -612,45 +612,33 @@ void ServerConfig::AddPlugin(const ServerConfig::PluginInfo &_info) ServerConfig::PluginInfo ServerConfig::LogPlaybackPlugin() const { + sdf::Plugin plugin; auto entityName = "*"; auto entityType = "world"; - auto pluginName = "ignition::gazebo::systems::LogPlayback"; - auto pluginFilename = "ignition-gazebo-log-system"; - - sdf::ElementPtr playbackElem; - playbackElem = std::make_shared(); - playbackElem->SetName("plugin"); + plugin.SetName("ignition::gazebo::systems::LogPlayback"); + plugin.SetFilename("ignition-gazebo-log-system"); if (!this->LogPlaybackPath().empty()) { sdf::ElementPtr pathElem = std::make_shared(); pathElem->SetName("playback_path"); - playbackElem->AddElementDescription(pathElem); - pathElem = playbackElem->GetElement("playback_path"); pathElem->AddValue("string", "", false, ""); pathElem->Set(this->LogPlaybackPath()); + plugin.InsertContent(pathElem); } - return ServerConfig::PluginInfo(entityName, - entityType, - pluginFilename, - pluginName, - playbackElem); + return ServerConfig::PluginInfo(entityName, entityType, plugin); } ///////////////////////////////////////////////// ServerConfig::PluginInfo ServerConfig::LogRecordPlugin() const { + sdf::Plugin plugin; auto entityName = "*"; auto entityType = "world"; - auto pluginName = "ignition::gazebo::systems::LogRecord"; - auto pluginFilename = "ignition-gazebo-log-system"; - - sdf::ElementPtr recordElem; - - recordElem = std::make_shared(); - recordElem->SetName("plugin"); + plugin.SetName("ignition::gazebo::systems::LogRecord"); + plugin.SetFilename("ignition-gazebo-log-system"); igndbg << "Generating LogRecord SDF:" << std::endl; @@ -658,37 +646,33 @@ ServerConfig::LogRecordPlugin() const { sdf::ElementPtr pathElem = std::make_shared(); pathElem->SetName("record_path"); - recordElem->AddElementDescription(pathElem); - pathElem = recordElem->GetElement("record_path"); pathElem->AddValue("string", "", false, ""); pathElem->Set(this->LogRecordPath()); + plugin.InsertContent(pathElem); } // Set whether to record resources sdf::ElementPtr resourceElem = std::make_shared(); resourceElem->SetName("record_resources"); - recordElem->AddElementDescription(resourceElem); - resourceElem = recordElem->GetElement("record_resources"); resourceElem->AddValue("bool", "false", false, ""); resourceElem->Set(this->LogRecordResources() ? true : false); + plugin.InsertContent(resourceElem); if (!this->LogRecordCompressPath().empty()) { // Set whether to compress sdf::ElementPtr compressElem = std::make_shared(); compressElem->SetName("compress"); - recordElem->AddElementDescription(compressElem); - compressElem = recordElem->GetElement("compress"); compressElem->AddValue("bool", "false", false, ""); compressElem->Set(true); + plugin.InsertContent(compressElem); - // Set compress path + // Set compress path sdf::ElementPtr cPathElem = std::make_shared(); cPathElem->SetName("compress_path"); - recordElem->AddElementDescription(cPathElem); - cPathElem = recordElem->GetElement("compress_path"); cPathElem->AddValue("string", "", false, ""); cPathElem->Set(this->LogRecordCompressPath()); + plugin.InsertContent(cPathElem); } // If record topics specified, add in SDF @@ -696,19 +680,16 @@ ServerConfig::LogRecordPlugin() const { sdf::ElementPtr topicElem = std::make_shared(); topicElem->SetName("record_topic"); - recordElem->AddElementDescription(topicElem); - topicElem = recordElem->AddElement("record_topic"); topicElem->AddValue("string", "false", false, ""); topicElem->Set(topic); + plugin.InsertContent(topicElem); } - igndbg << recordElem->ToString("") << std::endl; + igndbg << plugin.ToElement()->ToString("") << std::endl; return ServerConfig::PluginInfo(entityName, entityType, - pluginFilename, - pluginName, - recordElem); + plugin); } ///////////////////////////////////////////////// diff --git a/src/ServerConfig_TEST.cc b/src/ServerConfig_TEST.cc index bc93120ef2..ed8427fce3 100644 --- a/src/ServerConfig_TEST.cc +++ b/src/ServerConfig_TEST.cc @@ -62,22 +62,22 @@ TEST(ParsePluginsFromString, Valid) EXPECT_EQ("default", plugin->EntityName()); EXPECT_EQ("world", plugin->EntityType()); - EXPECT_EQ("TestWorldSystem", plugin->Filename()); - EXPECT_EQ("ignition::gazebo::TestWorldSystem", plugin->Name()); + EXPECT_EQ("TestWorldSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestWorldSystem", plugin->Plugin().Name()); plugin = std::next(plugin, 1); EXPECT_EQ("box", plugin->EntityName()); EXPECT_EQ("model", plugin->EntityType()); - EXPECT_EQ("TestModelSystem", plugin->Filename()); - EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Name()); + EXPECT_EQ("TestModelSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Plugin().Name()); plugin = std::next(plugin, 1); EXPECT_EQ("default::box::link_1::camera", plugin->EntityName()); EXPECT_EQ("sensor", plugin->EntityType()); - EXPECT_EQ("TestSensorSystem", plugin->Filename()); - EXPECT_EQ("ignition::gazebo::TestSensorSystem", plugin->Name()); + EXPECT_EQ("TestSensorSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestSensorSystem", plugin->Plugin().Name()); } ////////////////////////////////////////////////// @@ -114,22 +114,22 @@ TEST(ParsePluginsFromFile, Valid) EXPECT_EQ("default", plugin->EntityName()); EXPECT_EQ("world", plugin->EntityType()); - EXPECT_EQ("TestWorldSystem", plugin->Filename()); - EXPECT_EQ("ignition::gazebo::TestWorldSystem", plugin->Name()); + EXPECT_EQ("TestWorldSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestWorldSystem", plugin->Plugin().Name()); plugin = std::next(plugin, 1); EXPECT_EQ("box", plugin->EntityName()); EXPECT_EQ("model", plugin->EntityType()); - EXPECT_EQ("TestModelSystem", plugin->Filename()); - EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Name()); + EXPECT_EQ("TestModelSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Plugin().Name()); plugin = std::next(plugin, 1); EXPECT_EQ("default::box::link_1::camera", plugin->EntityName()); EXPECT_EQ("sensor", plugin->EntityType()); - EXPECT_EQ("TestSensorSystem", plugin->Filename()); - EXPECT_EQ("ignition::gazebo::TestSensorSystem", plugin->Name()); + EXPECT_EQ("TestSensorSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestSensorSystem", plugin->Plugin().Name()); } ////////////////////////////////////////////////// @@ -201,15 +201,15 @@ TEST(LoadPluginInfo, FromValidEnv) EXPECT_EQ("*", plugin->EntityName()); EXPECT_EQ("world", plugin->EntityType()); - EXPECT_EQ("TestWorldSystem", plugin->Filename()); - EXPECT_EQ("ignition::gazebo::TestWorldSystem", plugin->Name()); + EXPECT_EQ("TestWorldSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestWorldSystem", plugin->Plugin().Name()); plugin = std::next(plugin, 1); EXPECT_EQ("box", plugin->EntityName()); EXPECT_EQ("model", plugin->EntityType()); - EXPECT_EQ("TestModelSystem", plugin->Filename()); - EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Name()); + EXPECT_EQ("TestModelSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Plugin().Name()); EXPECT_TRUE(common::unsetenv(gazebo::kServerConfigPathEnv)); } @@ -225,7 +225,7 @@ TEST(ServerConfig, GenerateRecordPlugin) auto plugin = config.LogRecordPlugin(); EXPECT_EQ(plugin.EntityName(), "*"); EXPECT_EQ(plugin.EntityType(), "world"); - EXPECT_EQ(plugin.Name(), "ignition::gazebo::systems::LogRecord"); + EXPECT_EQ(plugin.Plugin().Name(), "ignition::gazebo::systems::LogRecord"); } ////////////////////////////////////////////////// diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index 479448f277..e1f5072531 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -159,6 +159,72 @@ TEST_P(ServerFixture, ServerConfigPluginInfo) } } +///////////////////////////////////////////////// +TEST_P(ServerFixture, ServerConfigSdfPluginInfo) +{ + ServerConfig::PluginInfo pluginInfo; + pluginInfo.SetEntityName("an_entity"); + pluginInfo.SetEntityType("model"); + sdf::Plugin plugin; + plugin.SetFilename("filename"); + plugin.SetName("interface"); + pluginInfo.SetPlugin(plugin); + + ignition::gazebo::ServerConfig serverConfig; + serverConfig.AddPlugin(pluginInfo); + + const std::list &plugins = serverConfig.Plugins(); + ASSERT_FALSE(plugins.empty()); + + EXPECT_EQ("an_entity", plugins.front().EntityName()); + EXPECT_EQ("model", plugins.front().EntityType()); + EXPECT_EQ("filename", plugins.front().Plugin().Filename()); + EXPECT_EQ("interface", plugins.front().Plugin().Name()); + EXPECT_EQ(nullptr, plugins.front().Plugin().Element()); + EXPECT_TRUE(plugins.front().Plugin().Contents().empty()); + + // Test operator= + { + ServerConfig::PluginInfo info; + info = plugins.front(); + + EXPECT_EQ(info.EntityName(), plugins.front().EntityName()); + EXPECT_EQ(info.EntityType(), plugins.front().EntityType()); + EXPECT_EQ(info.Plugin().Filename(), plugins.front().Plugin().Filename()); + EXPECT_EQ(info.Plugin().Name(), plugins.front().Plugin().Name()); + EXPECT_EQ(info.Plugin().ToElement()->ToString(""), + plugins.front().Plugin().ToElement()->ToString("")); + } + + // Test copy constructor + { + ServerConfig::PluginInfo info(plugins.front()); + + EXPECT_EQ(info.EntityName(), plugins.front().EntityName()); + EXPECT_EQ(info.EntityType(), plugins.front().EntityType()); + EXPECT_EQ(info.Plugin().Filename(), plugins.front().Plugin().Filename()); + EXPECT_EQ(info.Plugin().Name(), plugins.front().Plugin().Name()); + EXPECT_EQ(info.Plugin().ToElement()->ToString(""), + plugins.front().Plugin().ToElement()->ToString("")); + } + + // Test server config copy constructor + { + const ServerConfig &cfg(serverConfig); + const std::list &cfgPlugins = cfg.Plugins(); + ASSERT_FALSE(cfgPlugins.empty()); + + EXPECT_EQ(cfgPlugins.front().EntityName(), plugins.front().EntityName()); + EXPECT_EQ(cfgPlugins.front().EntityType(), plugins.front().EntityType()); + EXPECT_EQ(cfgPlugins.front().Plugin().Filename(), + plugins.front().Plugin().Filename()); + EXPECT_EQ(cfgPlugins.front().Plugin().Name(), + plugins.front().Plugin().Name()); + EXPECT_EQ(cfgPlugins.front().Plugin().ToElement()->ToString(""), + plugins.front().Plugin().ToElement()->ToString("")); + } +} + ///////////////////////////////////////////////// TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(ServerConfigRealPlugin)) { diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 895d1e782e..d257a574f1 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -858,15 +858,6 @@ void SimulationRunner::Step(const UpdateInfo &_info) this->entityCompMgr.SetAllComponentsUnchanged(); } -////////////////////////////////////////////////// -void SimulationRunner::LoadPlugin(const Entity _entity, - const std::string &_fname, - const std::string &_name, - const sdf::ElementPtr &_sdf) -{ - this->systemMgr->LoadPlugin(_entity, _fname, _name, _sdf); -} - ////////////////////////////////////////////////// void SimulationRunner::LoadPlugin(const Entity _entity, const sdf::Plugin &_plugin) @@ -997,7 +988,9 @@ void SimulationRunner::LoadPtrPlugins(const Entity _entity, // the console. if (filename != "__default__" && name != "__default__") { - this->LoadPlugin(_entity, filename, name, pluginElem); + sdf::Plugin plugin; + plugin.Load(pluginElem); + this->LoadPlugin(_entity, plugin); } pluginElem = pluginElem->GetNextElement("plugin"); diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index cda6b658ad..6a70affc77 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -136,17 +136,6 @@ namespace ignition /// \brief Publish current world statistics. public: void PublishStats(); - /// \brief Load system plugin for a given entity. - /// \param[in] _entity Entity - /// \param[in] _fname Filename of the plugin library - /// \param[in] _name Name of the plugin - /// \param[in] _sdf SDF element (content of plugin tag) - /// \todo(nkoenig) Remove this in Garden. - public: void LoadPlugin(const Entity _entity, - const std::string &_fname, - const std::string &_name, - const sdf::ElementPtr &_sdf); - /// \brief Load system plugins for a given entity. /// \param[in] _entity Entity /// \param[in] _sdf SDF element diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 17641a873c..3367e39d0d 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -1484,8 +1484,8 @@ TEST_P(SimulationRunnerTest, ASSERT_EQ(1u, rootWith.WorldCount()); // Emit plugin loading event - runner.EventMgr().Emit(boxEntity, - rootWith.WorldByIndex(0)->ModelByIndex(0)->Element()); + runner.EventMgr().Emit(boxEntity, + rootWith.WorldByIndex(0)->ModelByIndex(0)->Plugins()); // Check component registered by model plugin EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(componentId)) @@ -1494,8 +1494,8 @@ TEST_P(SimulationRunnerTest, componentId)) << componentId; // Emit plugin loading event again - runner.EventMgr().Emit(sphereEntity, - rootWith.WorldByIndex(0)->ModelByIndex(0)->Element()); + runner.EventMgr().Emit(sphereEntity, + rootWith.WorldByIndex(0)->ModelByIndex(0)->Plugins()); // Check component for the other model EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(componentId)) @@ -1522,8 +1522,8 @@ TEST_P(SimulationRunnerTest, EXPECT_FALSE(runner.EntityCompMgr().HasEntity(sphereEntity)); // Emit plugin loading event after all previous instances have been removed - runner.EventMgr().Emit(cylinderEntity, - rootWith.WorldByIndex(0)->ModelByIndex(0)->Element()); + runner.EventMgr().Emit(cylinderEntity, + rootWith.WorldByIndex(0)->ModelByIndex(0)->Plugins()); // Check component for the other model EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(componentId)) diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index 40a35bf467..14bbb1f2d6 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -223,26 +223,6 @@ std::optional SystemLoader::LoadPlugin( plugin.SetFilename(_filename); plugin.SetName(_name); return LoadPlugin(plugin); - /*ignition::plugin::PluginPtr plugin; - - if (_filename == "" || _name == "") - { - ignerr << "Failed to instantiate system plugin: empty argument " - "[(filename): " << _filename << "] " << - "[(name): " << _name << "]." << std::endl; - return {}; - } - - auto ret = this->dataPtr->InstantiateSystemPlugin(_filename, - _name, - _sdf, plugin); - if (ret && plugin) - { - return plugin; - } - - return {}; - */ } ////////////////////////////////////////////////// @@ -253,9 +233,9 @@ std::optional SystemLoader::LoadPlugin( { return {}; } - auto filename = _sdf->Get("filename"); - auto pluginName = _sdf->Get("name"); - return LoadPlugin(filename, pluginName, _sdf); + sdf::Plugin plugin; + plugin.Load(_sdf); + return LoadPlugin(plugin); } ////////////////////////////////////////////////// diff --git a/src/SystemLoader_TEST.cc b/src/SystemLoader_TEST.cc index 2a06a7daac..8ca53aff47 100644 --- a/src/SystemLoader_TEST.cc +++ b/src/SystemLoader_TEST.cc @@ -51,7 +51,9 @@ TEST(SystemLoader, Constructor) sdf::ElementPtr pluginElem = worldElem->GetElement("plugin"); while (pluginElem) { - auto system = sm.LoadPlugin(pluginElem); + sdf::Plugin plugin; + plugin.Load(pluginElem); + auto system = sm.LoadPlugin(plugin); ASSERT_TRUE(system.has_value()); pluginElem = pluginElem->GetNextElement("plugin"); } @@ -61,7 +63,7 @@ TEST(SystemLoader, Constructor) TEST(SystemLoader, EmptyNames) { gazebo::SystemLoader sm; - sdf::ElementPtr element; - auto system = sm.LoadPlugin("", "", element); + sdf::Plugin plugin; + auto system = sm.LoadPlugin(plugin); ASSERT_FALSE(system.has_value()); } diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 4374f9be51..33283b4acf 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -47,28 +47,6 @@ void SystemManager::LoadPlugin(const Entity _entity, igndbg << "Loaded system [" << _plugin.Name() << "] for entity [" << _entity << "]" << std::endl; } - -} - -////////////////////////////////////////////////// -void SystemManager::LoadPlugin(const Entity _entity, - const std::string &_fname, - const std::string &_name, - const sdf::ElementPtr &_sdf) -{ - std::optional system; - { - std::lock_guard lock(this->systemLoaderMutex); - system = this->systemLoader->LoadPlugin(_fname, _name, _sdf); - } - - // System correctly loaded from library - if (system) - { - this->AddSystem(system.value(), _entity, _sdf); - igndbg << "Loaded system [" << _name - << "] for entity [" << _entity << "]" << std::endl; - } } ////////////////////////////////////////////////// diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 7cabc15674..9b7a41a618 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -52,16 +52,6 @@ namespace ignition EntityComponentManager *_entityCompMgr = nullptr, EventManager *_eventMgr = nullptr); - /// \brief Load system plugin for a given entity. - /// \param[in] _entity Entity - /// \param[in] _fname Filename of the plugin library - /// \param[in] _name Name of the plugin - /// \param[in] _sdf SDF element (content of plugin tag) - public: void LoadPlugin(const Entity _entity, - const std::string &_fname, - const std::string &_name, - const sdf::ElementPtr &_sdf); - /// \brief Load system plugin for a given entity. /// \param[in] _entity Entity /// \param[in] _plugin Plugin to load diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc index a89a5d8254..09a274d80c 100644 --- a/src/gui/GuiEvents.cc +++ b/src/gui/GuiEvents.cc @@ -59,6 +59,15 @@ class ignition::gazebo::gui::events::VisualPlugin::Implementation public: sdf::ElementPtr element; }; +class ignition::gazebo::gui::events::VisualPluginSdf::Implementation +{ + /// \brief Entity to load the visual plugin for + public: ignition::gazebo::Entity entity; + + /// \brief Sdf plugin of the visual plugin + public: sdf::Plugin plugin; +}; + using namespace ignition; using namespace gazebo; using namespace gui; @@ -162,3 +171,24 @@ sdf::ElementPtr VisualPlugin::Element() const { return this->dataPtr->element; } + +///////////////////////////////////////////////// +VisualPluginSdf::VisualPluginSdf(ignition::gazebo::Entity _entity, + const sdf::Plugin &_plugin) : + QEvent(kType), dataPtr(utils::MakeImpl()) +{ + this->dataPtr->entity = _entity; + this->dataPtr->plugin = _plugin; +} + +///////////////////////////////////////////////// +ignition::gazebo::Entity VisualPluginSdf::Entity() const +{ + return this->dataPtr->entity; +} + +///////////////////////////////////////////////// +sdf::Plugin VisualPluginSdf::Plugin() const +{ + return this->dataPtr->plugin; +} diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 2a70c033ac..3361790e5e 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -82,7 +82,7 @@ class ignition::gazebo::GuiRunner::Implementation public: std::mutex systemLoadMutex; /// \brief Events containing visual plugins to load - public: std::vector> + public: std::vector> visualPlugins; /// \brief Systems implementing PreUpdate @@ -193,6 +193,21 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->node.Request(this->dataPtr->controlService, req, cb); } } + else if (_event->type() == + ignition::gazebo::gui::events::VisualPluginSdf::kType) + { + auto visualPluginEvent = + reinterpret_cast(_event); + if (visualPluginEvent) + { + std::lock_guard lock(this->dataPtr->systemLoadMutex); + + Entity entity = visualPluginEvent->Entity(); + sdf::Plugin plugin = visualPluginEvent->Plugin(); + this->dataPtr->visualPlugins.push_back( + std::make_pair(entity, plugin)); + } + } else if (_event->type() == ignition::gazebo::gui::events::VisualPlugin::kType) { auto visualPluginEvent = @@ -203,11 +218,11 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) Entity entity = visualPluginEvent->Entity(); sdf::ElementPtr pluginElem = visualPluginEvent->Element(); - this->dataPtr->visualPlugins.push_back( - std::make_pair(entity, pluginElem)); + sdf::Plugin plugin; + plugin.Load(pluginElem); + this->dataPtr->visualPlugins.push_back(std::make_pair(entity, plugin)); } } - // Standard event processing return QObject::eventFilter(_obj, _event); } @@ -332,16 +347,13 @@ void GuiRunner::LoadSystems() for (auto &visualPlugin : this->dataPtr->visualPlugins) { Entity entity = visualPlugin.first; - sdf::ElementPtr pluginElem = visualPlugin.second; - auto filename = pluginElem->Get("filename"); - auto name = pluginElem->Get("name"); - if (filename != "__default__" && name != "__default__") + sdf::Plugin plugin = visualPlugin.second; + if (plugin.Filename() != "__default__" && plugin.Name() != "__default__") { std::optional system; if (!this->dataPtr->systemLoader) this->dataPtr->systemLoader = std::make_unique(); - system = this->dataPtr->systemLoader->LoadPlugin( - filename, name, pluginElem); + system = this->dataPtr->systemLoader->LoadPlugin(plugin); if (system) { SystemPluginPtr sys = system.value(); @@ -356,10 +368,10 @@ void GuiRunner::LoadSystems() auto sysConfigure = sys->QueryInterface(); if (sysConfigure) { - sysConfigure->Configure(entity, pluginElem, this->dataPtr->ecm, - this->dataPtr->eventMgr); + sysConfigure->Configure(entity, plugin.ToElement(), + this->dataPtr->ecm, this->dataPtr->eventMgr); } - igndbg << "Loaded system [" << name + igndbg << "Loaded system [" << plugin.Name() << "] for entity [" << entity << "] in GUI" << std::endl; } diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 68d8a666c0..2189525c8c 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -134,39 +134,42 @@ void GzSceneManager::Update(const UpdateInfo &_info, this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); // load visual plugin on gui side - std::map pluginElems; + std::map plugins; if (!this->dataPtr->initializedVisualPlugins) { - _ecm.Each( + _ecm.Each( [&](const Entity &_entity, const components::Visual *, - const components::VisualPlugin *_plugin)->bool + const components::VisualPlugins *_plugins)->bool { - sdf::ElementPtr pluginElem = _plugin->Data(); - pluginElems[_entity] = _plugin->Data(); + plugins[_entity].insert(plugins[_entity].end(), + _plugins->Data().begin(), _plugins->Data().end()); return true; }); this->dataPtr->initializedVisualPlugins = true; } else { - _ecm.EachNew( + _ecm.EachNew( [&](const Entity &_entity, const components::Visual *, - const components::VisualPlugin *_plugin)->bool + const components::VisualPlugins *_plugins)->bool { - sdf::ElementPtr pluginElem = _plugin->Data(); - pluginElems[_entity] = _plugin->Data(); + plugins[_entity].insert(plugins[_entity].end(), + _plugins->Data().begin(), _plugins->Data().end()); return true; }); } - for (const auto &it : pluginElems) + for (const auto &it : plugins) { - ignition::gazebo::gui::events::VisualPlugin visualPluginEvent( - it.first, it.second); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &visualPluginEvent); + for (const sdf::Plugin plugin : it.second) + { + ignition::gazebo::gui::events::VisualPluginSdf visualPluginEvent( + it.first, plugin); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &visualPluginEvent); + } } // Emit entities created / removed event for gui::Plugins which don't have diff --git a/test/integration/battery_plugin.cc b/test/integration/battery_plugin.cc index 78d6ba6e77..15e130fb65 100644 --- a/test/integration/battery_plugin.cc +++ b/test/integration/battery_plugin.cc @@ -53,9 +53,10 @@ class BatteryPluginTest : public InternalFixture<::testing::Test> { InternalFixture::SetUp(); - auto plugin = sm.LoadPlugin("libMockSystem.so", - "ignition::gazebo::MockSystem", - nullptr); + sdf::Plugin sdfPlugin; + sdfPlugin.SetName("ignition::gazebo::MockSystem"); + sdfPlugin.SetFilename("libMockSystem.so"); + auto plugin = sm.LoadPlugin(sdfPlugin); EXPECT_TRUE(plugin.has_value()); this->systemPtr = plugin.value(); diff --git a/test/integration/follow_actor_system.cc b/test/integration/follow_actor_system.cc index befc3b2d2e..a919b970d9 100644 --- a/test/integration/follow_actor_system.cc +++ b/test/integration/follow_actor_system.cc @@ -46,9 +46,10 @@ class Relay { public: Relay() { - auto plugin = loader.LoadPlugin("libMockSystem.so", - "ignition::gazebo::MockSystem", - nullptr); + sdf::Plugin sdfPlugin; + sdfPlugin.SetFilename("libMockSystem.so"); + sdfPlugin.SetName("ignition::gazebo::MockSystem"); + auto plugin = loader.LoadPlugin(sdfPlugin); EXPECT_TRUE(plugin.has_value()); this->systemPtr = plugin.value(); diff --git a/test/integration/network_handshake.cc b/test/integration/network_handshake.cc index 11d42da572..37caded521 100644 --- a/test/integration/network_handshake.cc +++ b/test/integration/network_handshake.cc @@ -136,10 +136,11 @@ TEST_F(NetworkHandshake, IGN_UTILS_TEST_DISABLED_ON_WIN32(Updates)) ServerConfig::PluginInfo primaryPluginInfo; primaryPluginInfo.SetEntityName("default"); primaryPluginInfo.SetEntityType("world"); - primaryPluginInfo.SetFilename( - "libignition-gazebo-scene-broadcaster-system.so"); - primaryPluginInfo.SetName("ignition::gazebo::systems::SceneBroadcaster"); - primaryPluginInfo.SetSdf(pluginElem); + sdf::Plugin plugin; + plugin.SetFilename("libignition-gazebo-scene-broadcaster-system.so"); + plugin.SetName("ignition::gazebo::systems::SceneBroadcaster"); + plugin.InsertContent(pluginElem); + primaryPluginInfo.SetPlugin(plugin); ServerConfig configPrimary; configPrimary.SetNetworkRole("primary"); @@ -158,9 +159,11 @@ TEST_F(NetworkHandshake, IGN_UTILS_TEST_DISABLED_ON_WIN32(Updates)) ServerConfig::PluginInfo secondaryPluginInfo; secondaryPluginInfo.SetEntityName("default"); secondaryPluginInfo.SetEntityType("world"); - secondaryPluginInfo.SetFilename("libignition-gazebo-physics-system.so"); - secondaryPluginInfo.SetName("ignition::gazebo::systems::Physics"); - secondaryPluginInfo.SetSdf(pluginElem); + sdf::Plugin secondPlugin; + secondPlugin.SetFilename("libignition-gazebo-physics-system.so"); + secondPlugin.SetName("ignition::gazebo::systems::Physics"); + secondPlugin.InsertContent(pluginElem); + secondaryPluginInfo.SetPlugin(secondPlugin); ServerConfig configSecondary; configSecondary.SetNetworkRole("secondary"); diff --git a/test/integration/sensors_system.cc b/test/integration/sensors_system.cc index 6165ee9620..a00aa7ec14 100644 --- a/test/integration/sensors_system.cc +++ b/test/integration/sensors_system.cc @@ -53,9 +53,10 @@ class SensorsFixture : public InternalFixture> { InternalFixture::SetUp(); - auto plugin = sm.LoadPlugin("libMockSystem.so", - "ignition::gazebo::MockSystem", - nullptr); + sdf::Plugin sdfPlugin; + sdfPlugin.SetFilename("libMockSystem.so"); + sdfPlugin.SetName("ignition::gazebo::MockSystem"); + auto plugin = sm.LoadPlugin(sdfPlugin); EXPECT_TRUE(plugin.has_value()); this->systemPtr = plugin.value(); this->mockSystem = static_cast( diff --git a/test/integration/wind_effects.cc b/test/integration/wind_effects.cc index ac30ffe240..bd49f777f8 100644 --- a/test/integration/wind_effects.cc +++ b/test/integration/wind_effects.cc @@ -70,8 +70,10 @@ class LinkComponentRecorder public: LinkComponentRecorder(std::string _linkName, bool _createComp = false) : linkName(std::move(_linkName)) { - auto plugin = loader.LoadPlugin("libMockSystem.so", - "ignition::gazebo::MockSystem", nullptr); + sdf::Plugin sdfPlugin; + sdfPlugin.SetName("ignition::gazebo::MockSystem"); + sdfPlugin.SetFilename("libMockSystem.so"); + auto plugin = loader.LoadPlugin(sdfPlugin); EXPECT_TRUE(plugin.has_value()); this->systemPtr = plugin.value(); From f23935c31ae613529889009929e32a8f378d52e6 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 14 Mar 2022 14:28:17 -0700 Subject: [PATCH 09/14] Updated documentation and minor cleanup Signed-off-by: Nate Koenig --- include/ignition/gazebo/Events.hh | 10 ++- include/ignition/gazebo/ServerConfig.hh | 21 ++++-- include/ignition/gazebo/SystemLoader.hh | 6 +- include/ignition/gazebo/components/Visual.hh | 5 +- include/ignition/gazebo/gui/GuiEvents.hh | 10 +-- src/LevelManager.cc | 2 + src/ServerConfig_TEST.cc | 15 ++++ src/SimulationRunner.cc | 34 +-------- src/SimulationRunner.hh | 15 +--- src/SystemLoader.cc | 69 ------------------- src/gui/GuiEvents.cc | 8 +-- src/gui/GuiRunner.cc | 4 +- .../plugins/scene_manager/GzSceneManager.cc | 2 +- 13 files changed, 62 insertions(+), 139 deletions(-) diff --git a/include/ignition/gazebo/Events.hh b/include/ignition/gazebo/Events.hh index 7f3762f6e4..40c8fab64b 100644 --- a/include/ignition/gazebo/Events.hh +++ b/include/ignition/gazebo/Events.hh @@ -54,16 +54,22 @@ namespace ignition /// \endcode using Stop = ignition::common::EventT; - /// \brief Event used to load plugins for an entity into simulation. + /// \brief Please use the LoadSdfPlugins event. The LoadPlugins event + /// will be deprecrated in Gazebo 7 (Garden). Also make sure to + /// connect to only LoadSdfPlugins or LoadPlugins, and not both events. + /// + /// Event used to load plugins for an entity into simulation. /// Pass in the entity which will own the plugins, and an SDF element for /// the entity, which may contain multiple `` tags. - /// \todo(nkoenig) Deprecate this in Garden. + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// sdf::Plugin interface. using LoadPlugins = common::EventT; /// \brief Event used to load plugins for an entity into simulation. /// Pass in the entity which will own the plugins, and an SDF element for /// the entity, which may contain multiple `` tags. + /// Makre sure that you don't also connecto to the LoadPlugins event. using LoadSdfPlugins = common::EventT; } diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index b58a380fae..3a87557b21 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -87,7 +87,8 @@ namespace ignition /// \param[in] _name Name of the interface within the plugin library /// to load. /// \param[in] _sdf Plugin XML elements associated with this plugin. - /// \todo(nkoenig) Deprecate this in Garden. + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// sdf::Plugin interface. public: PluginInfo(const std::string &_entityName, const std::string &_entityType, const std::string &_filename, @@ -141,36 +142,42 @@ namespace ignition /// \brief Get the plugin library filename. /// \return Plugin library filename. - /// \todo(nkoenig) Deprecate this function in Garden. + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// sdf::Plugin interface. public: const std::string &Filename() const; /// \brief Set the type of the entity which should receive this /// plugin. The type is used in conjuction with EntityName to /// uniquely identify an entity. /// \param[in] _filename Entity type string. - /// \todo(nkoenig) Deprecate this function in Garden. + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// sdf::Plugin interface. public: void SetFilename(const std::string &_filename); /// \brief Name of the interface within the plugin library /// to load. /// \return Interface name. - /// \todo(nkoenig) Deprecate this function in Garden. + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// sdf::Plugin interface. public: const std::string &Name() const; /// \brief Set the name of the interface within the plugin library /// to load. /// \param[in] _name Interface name. - /// \todo(nkoenig) Deprecate this function in Garden. + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// sdf::Plugin interface. public: void SetName(const std::string &_name); /// \brief Plugin XML elements associated with this plugin. /// \return SDF pointer. - /// \todo(nkoenig) Deprecate this function in Garden. + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// sdf::Plugin interface. public: const sdf::ElementPtr &Sdf() const; /// \brief Set the plugin XML elements associated with this plugin. /// \param[in] _sdf SDF pointer, it will be cloned. - /// \todo(nkoenig) Deprecate this function in Garden. + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// sdf::Plugin interface. public: void SetSdf(const sdf::ElementPtr &_sdf); /// \brief Get the SDF plugin information. diff --git a/include/ignition/gazebo/SystemLoader.hh b/include/ignition/gazebo/SystemLoader.hh index 719fd825ef..592027394a 100644 --- a/include/ignition/gazebo/SystemLoader.hh +++ b/include/ignition/gazebo/SystemLoader.hh @@ -54,7 +54,8 @@ namespace ignition /// \brief Load and instantiate system plugin from an SDF element. /// \param[in] _sdf SDF Element describing plugin instance to be loaded. /// \returns Shared pointer to system instance or nullptr. - /// \todo(nkoenig) Deprecate this function in Garden + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// sdf::Plugin interface. public: std::optional LoadPlugin( const sdf::ElementPtr &_sdf); @@ -63,7 +64,8 @@ namespace ignition /// \param[in] _name Class name to be instantiated. /// \param[in] _sdf SDF Element describing plugin instance to be loaded. /// \returns Shared pointer to system instance or nullptr. - /// \todo(nkoenig) Deprecate this function in Garden + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// sdf::Plugin interface. public: std::optional LoadPlugin( const std::string &_filename, const std::string &_name, diff --git a/include/ignition/gazebo/components/Visual.hh b/include/ignition/gazebo/components/Visual.hh index dc88a79d2b..0457d69540 100644 --- a/include/ignition/gazebo/components/Visual.hh +++ b/include/ignition/gazebo/components/Visual.hh @@ -81,14 +81,15 @@ namespace components IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Visual", Visual) /// \brief A component that contains a visual plugin's SDF element. - /// \todo(nkoenig) Deprecate this in Garden + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// sdf::Plugins interface. using VisualPlugin = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.VisualPlugin", VisualPlugin) - /// \brief A component that contains a visual plugin's SDF objects. + /// \brief A component that contains a visual plugin's SDF Plugin objects. using VisualPlugins = Component>; diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index dec2f049fc..e05e608b6f 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -216,6 +216,8 @@ namespace events }; /// \brief Event that notifies a visual plugin is to be loaded + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// VisualSdfPlugin class. class IGNITION_GAZEBO_GUI_VISIBLE VisualPlugin: public QEvent { /// \brief Constructor @@ -238,18 +240,18 @@ namespace events }; /// \brief Event that notifies a visual plugin is to be loaded - class IGNITION_GAZEBO_GUI_VISIBLE VisualPluginSdf: public QEvent + class IGNITION_GAZEBO_GUI_VISIBLE VisualSdfPlugin: public QEvent { /// \brief Constructor /// \param[in] _entity Visual entity id - /// \param[in] _elem Visual plugin SDF element - public: explicit VisualPluginSdf(ignition::gazebo::Entity _entity, + /// \param[in] _plguin SDF plugin object + public: explicit VisualSdfPlugin(ignition::gazebo::Entity _entity, const sdf::Plugin &_plugin); /// \brief Get the entity to load the visual plugin for public: ignition::gazebo::Entity Entity() const; - /// \brief Get the sdf plugin of the visual plugin + /// \brief Get the SDF Plugin of the visual plugin public: sdf::Plugin Plugin() const; static const QEvent::Type kType = QEvent::Type(QEvent::User + 8); diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 16a14baf6f..75bc5e2ecf 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -231,6 +231,8 @@ void LevelManager::ReadLevelPerformerInfo() // Load world plugins. this->runner->EventMgr().Emit(this->worldEntity, this->runner->sdfWorld->Plugins()); + this->runner->EventMgr().Emit(this->worldEntity, + this->runner->sdfWorld->Element()); // Store the world's SDF DOM to be used when saving the world to file this->runner->entityCompMgr.CreateComponent( diff --git a/src/ServerConfig_TEST.cc b/src/ServerConfig_TEST.cc index ed8427fce3..7e5df2cd25 100644 --- a/src/ServerConfig_TEST.cc +++ b/src/ServerConfig_TEST.cc @@ -62,21 +62,27 @@ TEST(ParsePluginsFromString, Valid) EXPECT_EQ("default", plugin->EntityName()); EXPECT_EQ("world", plugin->EntityType()); + EXPECT_EQ("TestWorldSystem", plugin->Filename()); EXPECT_EQ("TestWorldSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestWorldSystem", plugin->Name()); EXPECT_EQ("ignition::gazebo::TestWorldSystem", plugin->Plugin().Name()); plugin = std::next(plugin, 1); EXPECT_EQ("box", plugin->EntityName()); EXPECT_EQ("model", plugin->EntityType()); + EXPECT_EQ("TestModelSystem", plugin->Filename()); EXPECT_EQ("TestModelSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Name()); EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Plugin().Name()); plugin = std::next(plugin, 1); EXPECT_EQ("default::box::link_1::camera", plugin->EntityName()); EXPECT_EQ("sensor", plugin->EntityType()); + EXPECT_EQ("TestSensorSystem", plugin->Filename()); EXPECT_EQ("TestSensorSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestSensorSystem", plugin->Name()); EXPECT_EQ("ignition::gazebo::TestSensorSystem", plugin->Plugin().Name()); } @@ -114,21 +120,27 @@ TEST(ParsePluginsFromFile, Valid) EXPECT_EQ("default", plugin->EntityName()); EXPECT_EQ("world", plugin->EntityType()); + EXPECT_EQ("TestWorldSystem", plugin->Filename()); EXPECT_EQ("TestWorldSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestWorldSystem", plugin->Name()); EXPECT_EQ("ignition::gazebo::TestWorldSystem", plugin->Plugin().Name()); plugin = std::next(plugin, 1); EXPECT_EQ("box", plugin->EntityName()); EXPECT_EQ("model", plugin->EntityType()); + EXPECT_EQ("TestModelSystem", plugin->Filename()); EXPECT_EQ("TestModelSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Name()); EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Plugin().Name()); plugin = std::next(plugin, 1); EXPECT_EQ("default::box::link_1::camera", plugin->EntityName()); EXPECT_EQ("sensor", plugin->EntityType()); + EXPECT_EQ("TestSensorSystem", plugin->Filename()); EXPECT_EQ("TestSensorSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestSensorSystem", plugin->Name()); EXPECT_EQ("ignition::gazebo::TestSensorSystem", plugin->Plugin().Name()); } @@ -201,7 +213,9 @@ TEST(LoadPluginInfo, FromValidEnv) EXPECT_EQ("*", plugin->EntityName()); EXPECT_EQ("world", plugin->EntityType()); + EXPECT_EQ("TestWorldSystem", plugin->Filename()); EXPECT_EQ("TestWorldSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestWorldSystem", plugin->Name()); EXPECT_EQ("ignition::gazebo::TestWorldSystem", plugin->Plugin().Name()); plugin = std::next(plugin, 1); @@ -209,6 +223,7 @@ TEST(LoadPluginInfo, FromValidEnv) EXPECT_EQ("box", plugin->EntityName()); EXPECT_EQ("model", plugin->EntityType()); EXPECT_EQ("TestModelSystem", plugin->Plugin().Filename()); + EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Name()); EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Plugin().Name()); EXPECT_TRUE(common::unsetenv(gazebo::kServerConfigPathEnv)); diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index d257a574f1..08de7a1bda 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -126,11 +126,6 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, std::bind(&SimulationRunner::LoadPlugins, this, std::placeholders::_1, std::placeholders::_2)); - /// \todo(nkoenig) Deprecate/remove this event in Garden - this->loadPtrPluginsConn = this->eventMgr.Connect( - std::bind(&SimulationRunner::LoadPtrPlugins, this, std::placeholders::_1, - std::placeholders::_2)); - // Create the level manager this->levelMgr = std::make_unique(this, _config.UseLevels()); @@ -972,44 +967,17 @@ void SimulationRunner::LoadLoggingPlugins(const ServerConfig &_config) this->LoadServerPlugins(plugins); } -////////////////////////////////////////////////// -/// \todo(nkoenig) Remove this function in Garden. -void SimulationRunner::LoadPtrPlugins(const Entity _entity, - const sdf::ElementPtr &_sdf) -{ - sdf::ElementPtr pluginElem = _sdf->FindElement("plugin"); - while (pluginElem) - { - auto filename = pluginElem->Get("filename"); - auto name = pluginElem->Get("name"); - // No error message for the 'else' case of the following 'if' statement - // because SDF create a default element even if it's not - // specified. An error message would result in spamming - // the console. - if (filename != "__default__" && name != "__default__") - { - sdf::Plugin plugin; - plugin.Load(pluginElem); - this->LoadPlugin(_entity, plugin); - } - - pluginElem = pluginElem->GetNextElement("plugin"); - } -} - ////////////////////////////////////////////////// void SimulationRunner::LoadPlugins(const Entity _entity, const sdf::Plugins &_plugins) { for (const sdf::Plugin plugin : _plugins) { - auto filename = plugin.Filename(); - auto name = plugin.Name(); // No error message for the 'else' case of the following 'if' statement // because SDF create a default element even if it's not // specified. An error message would result in spamming // the console. - if (filename != "__default__" && name != "__default__") + if (plugin.Filename() != "__default__" && plugin.Name() != "__default__") { this->LoadPlugin(_entity, plugin); } diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 6a70affc77..99b66c1c95 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -136,20 +136,13 @@ namespace ignition /// \brief Publish current world statistics. public: void PublishStats(); - /// \brief Load system plugins for a given entity. - /// \param[in] _entity Entity - /// \param[in] _sdf SDF element - /// \todo(nkoenig) Remove this in Garden. - public: void LoadPtrPlugins(const Entity _entity, - const sdf::ElementPtr &_sdf); - /// \brief Load system plugin for a given entity. - /// \param[in] _entity Entity + /// \param[in] _entity The plugins will be associated with this Entity /// \param[in] _plugin SDF Plugin to load public: void LoadPlugin(const Entity _entity, const sdf::Plugin &_plugin); /// \brief Load system plugins for a given entity. - /// \param[in] _entity Entity + /// \param[in] _entity The plugins will be associated with this Entity /// \param[in] _plugins SDF Plugins to load public: void LoadPlugins(const Entity _entity, const sdf::Plugins &_plugins); @@ -468,10 +461,6 @@ namespace ignition /// \brief Connection to the load plugins event. private: common::ConnectionPtr loadPluginsConn; - /// \brief Connection to the sdf element pointer load plugins event. - /// \todo(nkoenig) Remove this in Garden. - private: common::ConnectionPtr loadPtrPluginsConn; - /// \brief Pointer to the sdf::World object of this runner private: const sdf::World *sdfWorld; diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index 14bbb1f2d6..86328cf00f 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -40,75 +40,6 @@ class ignition::gazebo::SystemLoaderPrivate ////////////////////////////////////////////////// public: explicit SystemLoaderPrivate() = default; - ////////////////////////////////////////////////// - public: bool InstantiateSystemPlugin(const std::string &_filename, - const std::string &_name, - const sdf::ElementPtr &/*_sdf*/, - ignition::plugin::PluginPtr &_plugin) - { - ignition::common::SystemPaths systemPaths; - systemPaths.SetPluginPathEnv(pluginPathEnv); - - for (const auto &path : this->systemPluginPaths) - systemPaths.AddPluginPaths(path); - - std::string homePath; - ignition::common::env(IGN_HOMEDIR, homePath); - systemPaths.AddPluginPaths(homePath + "/.ignition/gazebo/plugins"); - systemPaths.AddPluginPaths(IGN_GAZEBO_PLUGIN_INSTALL_DIR); - - auto pathToLib = systemPaths.FindSharedLibrary(_filename); - if (pathToLib.empty()) - { - // We assume ignition::gazebo corresponds to the levels feature - if (_name != "ignition::gazebo") - { - ignerr << "Failed to load system plugin [" << _filename << - "] : couldn't find shared library." << std::endl; - } - return false; - } - - auto pluginNames = this->loader.LoadLib(pathToLib); - if (pluginNames.empty()) - { - ignerr << "Failed to load system plugin [" << _filename << - "] : couldn't load library on path [" << pathToLib << - "]." << std::endl; - return false; - } - - auto pluginName = *pluginNames.begin(); - if (pluginName.empty()) - { - ignerr << "Failed to load system plugin [" << _filename << - "] : couldn't load library on path [" << pathToLib << - "]." << std::endl; - return false; - } - - _plugin = this->loader.Instantiate(_name); - if (!_plugin) - { - ignerr << "Failed to load system plugin [" << _name << - "] : could not instantiate from library [" << _filename << - "] from path [" << pathToLib << "]." << std::endl; - return false; - } - - if (!_plugin->HasInterface()) - { - ignerr << "Failed to load system plugin [" << _name << - "] : system not found in library [" << _filename << - "] from path [" << pathToLib << "]." << std::endl; - - return false; - } - - this->systemPluginsAdded.insert(_plugin); - return true; - } - ////////////////////////////////////////////////// public: bool InstantiateSystemPlugin(const sdf::Plugin &_plugin, ignition::plugin::PluginPtr &_ignPlugin) diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc index 09a274d80c..98ca90dfb7 100644 --- a/src/gui/GuiEvents.cc +++ b/src/gui/GuiEvents.cc @@ -59,7 +59,7 @@ class ignition::gazebo::gui::events::VisualPlugin::Implementation public: sdf::ElementPtr element; }; -class ignition::gazebo::gui::events::VisualPluginSdf::Implementation +class ignition::gazebo::gui::events::VisualSdfPlugin::Implementation { /// \brief Entity to load the visual plugin for public: ignition::gazebo::Entity entity; @@ -173,7 +173,7 @@ sdf::ElementPtr VisualPlugin::Element() const } ///////////////////////////////////////////////// -VisualPluginSdf::VisualPluginSdf(ignition::gazebo::Entity _entity, +VisualSdfPlugin::VisualSdfPlugin(ignition::gazebo::Entity _entity, const sdf::Plugin &_plugin) : QEvent(kType), dataPtr(utils::MakeImpl()) { @@ -182,13 +182,13 @@ VisualPluginSdf::VisualPluginSdf(ignition::gazebo::Entity _entity, } ///////////////////////////////////////////////// -ignition::gazebo::Entity VisualPluginSdf::Entity() const +ignition::gazebo::Entity VisualSdfPlugin::Entity() const { return this->dataPtr->entity; } ///////////////////////////////////////////////// -sdf::Plugin VisualPluginSdf::Plugin() const +sdf::Plugin VisualSdfPlugin::Plugin() const { return this->dataPtr->plugin; } diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 3361790e5e..caeaa0617c 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -194,10 +194,10 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::VisualPluginSdf::kType) + ignition::gazebo::gui::events::VisualSdfPlugin::kType) { auto visualPluginEvent = - reinterpret_cast(_event); + reinterpret_cast(_event); if (visualPluginEvent) { std::lock_guard lock(this->dataPtr->systemLoadMutex); diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 2189525c8c..ab584b127a 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -164,7 +164,7 @@ void GzSceneManager::Update(const UpdateInfo &_info, { for (const sdf::Plugin plugin : it.second) { - ignition::gazebo::gui::events::VisualPluginSdf visualPluginEvent( + ignition::gazebo::gui::events::VisualSdfPlugin visualPluginEvent( it.first, plugin); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), From d0695a417483c3ddb5c31c311206f39bf54c39a8 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Mar 2022 05:10:58 -0700 Subject: [PATCH 10/14] use sdf 12.4 Signed-off-by: Nate Koenig --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index df48d9915a..9a1d2999cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,7 +59,7 @@ set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # as protobuf could be find transitively by any dependency set(protobuf_MODULE_COMPATIBLE TRUE) -ign_find_package(sdformat12 REQUIRED VERSION 12.3) +ign_find_package(sdformat12 REQUIRED VERSION 12.4) set(SDF_VER ${sdformat12_VERSION_MAJOR}) #-------------------------------------- From fd2b9b19c1d93e62e87d0d4ed9532876671db5e9 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 13 May 2022 05:46:45 -0700 Subject: [PATCH 11/14] resolve some comments Signed-off-by: Nate Koenig --- include/ignition/gazebo/Events.hh | 2 +- include/ignition/gazebo/gui/GuiEvents.hh | 2 +- src/ServerConfig.cc | 17 ++++++----- src/SimulationRunner.cc | 2 +- src/SystemLoader.cc | 30 +++++++++---------- src/gui/GuiEvents.cc | 2 +- .../plugins/scene_manager/GzSceneManager.cc | 2 +- 7 files changed, 29 insertions(+), 28 deletions(-) diff --git a/include/ignition/gazebo/Events.hh b/include/ignition/gazebo/Events.hh index 40c8fab64b..c2d1dbfa89 100644 --- a/include/ignition/gazebo/Events.hh +++ b/include/ignition/gazebo/Events.hh @@ -69,7 +69,7 @@ namespace ignition /// \brief Event used to load plugins for an entity into simulation. /// Pass in the entity which will own the plugins, and an SDF element for /// the entity, which may contain multiple `` tags. - /// Makre sure that you don't also connecto to the LoadPlugins event. + /// Makre sure that you don't also connect to the LoadPlugins event. using LoadSdfPlugins = common::EventT; } diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index e05e608b6f..c6f9a96d90 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -252,7 +252,7 @@ namespace events public: ignition::gazebo::Entity Entity() const; /// \brief Get the SDF Plugin of the visual plugin - public: sdf::Plugin Plugin() const; + public: const sdf::Plugin &Plugin() const; static const QEvent::Type kType = QEvent::Type(QEvent::User + 8); diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 5e6864555b..9482ed4f12 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -42,9 +42,9 @@ class ignition::gazebo::ServerConfig::PluginInfoPrivate const std::unique_ptr &_info) : entityName(_info->entityName), entityType(_info->entityType), + plugin(_info->plugin), filename(_info->filename), - name(_info->name), - plugin(_info->plugin) + name(_info->name) { this->sdf = plugin.Element(); } @@ -65,11 +65,11 @@ class ignition::gazebo::ServerConfig::PluginInfoPrivate sdf::Plugin _plugin) : entityName(std::move(_entityName)), entityType(std::move(_entityType)), - plugin(std::move(_plugin)) + plugin(std::move(_plugin)), + filename(plugin.Filename()), + name(plugin.Name()), + sdf(plugin.Element()) { - this->filename = this->plugin.Filename(); - this->name = this->plugin.Name(); - this->sdf = this->plugin.Element(); } /// \brief The name of the entity. @@ -78,6 +78,9 @@ class ignition::gazebo::ServerConfig::PluginInfoPrivate /// \brief The type of entity. public: std::string entityType = ""; + /// \brief SDF plugin information. + public: sdf::Plugin plugin; + /// \brief _filename The plugin library. public: std::string filename = ""; @@ -87,8 +90,6 @@ class ignition::gazebo::ServerConfig::PluginInfoPrivate /// \brief XML elements associated with this plugin public: sdf::ElementPtr sdf = nullptr; - /// \brief SDF plugin information. - public: sdf::Plugin plugin; }; ////////////////////////////////////////////////// diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 7d25746fc5..f4616bbcb4 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -972,7 +972,7 @@ void SimulationRunner::LoadLoggingPlugins(const ServerConfig &_config) void SimulationRunner::LoadPlugins(const Entity _entity, const sdf::Plugins &_plugins) { - for (const sdf::Plugin plugin : _plugins) + for (const sdf::Plugin &plugin : _plugins) { // No error message for the 'else' case of the following 'if' statement // because SDF create a default element even if it's not diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index 86328cf00f..fbf7ae020e 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -41,8 +41,8 @@ class ignition::gazebo::SystemLoaderPrivate public: explicit SystemLoaderPrivate() = default; ////////////////////////////////////////////////// - public: bool InstantiateSystemPlugin(const sdf::Plugin &_plugin, - ignition::plugin::PluginPtr &_ignPlugin) + public: bool InstantiateSystemPlugin(const sdf::Plugin &_sdfPlugin, + ignition::plugin::PluginPtr &_gzPlugin) { ignition::common::SystemPaths systemPaths; systemPaths.SetPluginPathEnv(pluginPathEnv); @@ -55,13 +55,13 @@ class ignition::gazebo::SystemLoaderPrivate systemPaths.AddPluginPaths(homePath + "/.ignition/gazebo/plugins"); systemPaths.AddPluginPaths(IGN_GAZEBO_PLUGIN_INSTALL_DIR); - auto pathToLib = systemPaths.FindSharedLibrary(_plugin.Filename()); + auto pathToLib = systemPaths.FindSharedLibrary(_sdfPlugin.Filename()); if (pathToLib.empty()) { // We assume ignition::gazebo corresponds to the levels feature - if (_plugin.Name() != "ignition::gazebo") + if (_sdfPlugin.Name() != "ignition::gazebo") { - ignerr << "Failed to load system plugin [" << _plugin.Filename() << + ignerr << "Failed to load system plugin [" << _sdfPlugin.Filename() << "] : couldn't find shared library." << std::endl; } return false; @@ -70,7 +70,7 @@ class ignition::gazebo::SystemLoaderPrivate auto pluginNames = this->loader.LoadLib(pathToLib); if (pluginNames.empty()) { - ignerr << "Failed to load system plugin [" << _plugin.Filename() << + ignerr << "Failed to load system plugin [" << _sdfPlugin.Filename() << "] : couldn't load library on path [" << pathToLib << "]." << std::endl; return false; @@ -79,31 +79,31 @@ class ignition::gazebo::SystemLoaderPrivate auto pluginName = *pluginNames.begin(); if (pluginName.empty()) { - ignerr << "Failed to load system plugin [" << _plugin.Filename() << + ignerr << "Failed to load system plugin [" << _sdfPlugin.Filename() << "] : couldn't load library on path [" << pathToLib << "]." << std::endl; return false; } - _ignPlugin = this->loader.Instantiate(_plugin.Name()); - if (!_ignPlugin) + _gzPlugin = this->loader.Instantiate(_sdfPlugin.Name()); + if (!_gzPlugin) { - ignerr << "Failed to load system plugin [" << _plugin.Name() << - "] : could not instantiate from library [" << _plugin.Filename() << + ignerr << "Failed to load system plugin [" << _sdfPlugin.Name() << + "] : could not instantiate from library [" << _sdfPlugin.Filename() << "] from path [" << pathToLib << "]." << std::endl; return false; } - if (!_ignPlugin->HasInterface()) + if (!_gzPlugin->HasInterface()) { - ignerr << "Failed to load system plugin [" << _plugin.Name() << - "] : system not found in library [" << _plugin.Filename() << + ignerr << "Failed to load system plugin [" << _sdfPlugin.Name() << + "] : system not found in library [" << _sdfPlugin.Filename() << "] from path [" << pathToLib << "]." << std::endl; return false; } - this->systemPluginsAdded.insert(_ignPlugin); + this->systemPluginsAdded.insert(_gzPlugin); return true; } diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc index 98ca90dfb7..356c7c3465 100644 --- a/src/gui/GuiEvents.cc +++ b/src/gui/GuiEvents.cc @@ -188,7 +188,7 @@ ignition::gazebo::Entity VisualSdfPlugin::Entity() const } ///////////////////////////////////////////////// -sdf::Plugin VisualSdfPlugin::Plugin() const +const sdf::Plugin &VisualSdfPlugin::Plugin() const { return this->dataPtr->plugin; } diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index ab584b127a..0190e92e4e 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -162,7 +162,7 @@ void GzSceneManager::Update(const UpdateInfo &_info, } for (const auto &it : plugins) { - for (const sdf::Plugin plugin : it.second) + for (const sdf::Plugin &plugin : it.second) { ignition::gazebo::gui::events::VisualSdfPlugin visualPluginEvent( it.first, plugin); From 61fcf059bdda7db75e59e12058eec67f713d5fed Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 13 May 2022 09:01:42 -0700 Subject: [PATCH 12/14] Resolve issue #1363 Signed-off-by: Nate Koenig --- include/ignition/gazebo/gui/GuiEvents.hh | 12 ++++++------ src/gui/GuiEvents.cc | 18 +++++++++--------- src/gui/GuiRunner.cc | 11 ++++++----- .../plugins/scene_manager/GzSceneManager.cc | 12 ++++++++++-- 4 files changed, 31 insertions(+), 22 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index c6f9a96d90..c9807b3915 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -217,7 +217,7 @@ namespace events /// \brief Event that notifies a visual plugin is to be loaded /// \note This will be deprecated in Gazebo 7 (Garden), please the use - /// VisualSdfPlugin class. + /// VisualPlugins class. class IGNITION_GAZEBO_GUI_VISIBLE VisualPlugin: public QEvent { /// \brief Constructor @@ -240,19 +240,19 @@ namespace events }; /// \brief Event that notifies a visual plugin is to be loaded - class IGNITION_GAZEBO_GUI_VISIBLE VisualSdfPlugin: public QEvent + class IGNITION_GAZEBO_GUI_VISIBLE VisualPlugins: public QEvent { /// \brief Constructor /// \param[in] _entity Visual entity id - /// \param[in] _plguin SDF plugin object - public: explicit VisualSdfPlugin(ignition::gazebo::Entity _entity, - const sdf::Plugin &_plugin); + /// \param[in] _plugins SDF plugin object + public: explicit VisualPlugins(ignition::gazebo::Entity _entity, + const sdf::Plugins &_plugins); /// \brief Get the entity to load the visual plugin for public: ignition::gazebo::Entity Entity() const; /// \brief Get the SDF Plugin of the visual plugin - public: const sdf::Plugin &Plugin() const; + public: const sdf::Plugins &Plugins() const; static const QEvent::Type kType = QEvent::Type(QEvent::User + 8); diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc index 356c7c3465..b9ce7e8812 100644 --- a/src/gui/GuiEvents.cc +++ b/src/gui/GuiEvents.cc @@ -59,13 +59,13 @@ class ignition::gazebo::gui::events::VisualPlugin::Implementation public: sdf::ElementPtr element; }; -class ignition::gazebo::gui::events::VisualSdfPlugin::Implementation +class ignition::gazebo::gui::events::VisualPlugins::Implementation { /// \brief Entity to load the visual plugin for public: ignition::gazebo::Entity entity; - /// \brief Sdf plugin of the visual plugin - public: sdf::Plugin plugin; + /// \brief Sdf plugins for the visual plugin + public: sdf::Plugins plugins; }; using namespace ignition; @@ -173,22 +173,22 @@ sdf::ElementPtr VisualPlugin::Element() const } ///////////////////////////////////////////////// -VisualSdfPlugin::VisualSdfPlugin(ignition::gazebo::Entity _entity, - const sdf::Plugin &_plugin) : +VisualPlugins::VisualPlugins(ignition::gazebo::Entity _entity, + const sdf::Plugins &_plugins) : QEvent(kType), dataPtr(utils::MakeImpl()) { this->dataPtr->entity = _entity; - this->dataPtr->plugin = _plugin; + this->dataPtr->plugins = _plugins; } ///////////////////////////////////////////////// -ignition::gazebo::Entity VisualSdfPlugin::Entity() const +ignition::gazebo::Entity VisualPlugins::Entity() const { return this->dataPtr->entity; } ///////////////////////////////////////////////// -const sdf::Plugin &VisualSdfPlugin::Plugin() const +const sdf::Plugins &VisualPlugins::Plugins() const { - return this->dataPtr->plugin; + return this->dataPtr->plugins; } diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index caeaa0617c..ab8a8d43e9 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -194,18 +194,19 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::VisualSdfPlugin::kType) + ignition::gazebo::gui::events::VisualPlugins::kType) { auto visualPluginEvent = - reinterpret_cast(_event); + reinterpret_cast(_event); if (visualPluginEvent) { std::lock_guard lock(this->dataPtr->systemLoadMutex); Entity entity = visualPluginEvent->Entity(); - sdf::Plugin plugin = visualPluginEvent->Plugin(); - this->dataPtr->visualPlugins.push_back( - std::make_pair(entity, plugin)); + for (const sdf::Plugin &plugin : visualPluginEvent->Plugins()) + { + this->dataPtr->visualPlugins.push_back(std::make_pair(entity, plugin)); + } } } else if (_event->type() == ignition::gazebo::gui::events::VisualPlugin::kType) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 0190e92e4e..4fa3b551c4 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -162,10 +162,18 @@ void GzSceneManager::Update(const UpdateInfo &_info, } for (const auto &it : plugins) { + // Send the new VisualPlugins event + ignition::gazebo::gui::events::VisualPlugins visualPluginsEvent( + it.first, it.second); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &visualPluginsEvent); + + // Send the old VisualPlugin event for (const sdf::Plugin &plugin : it.second) { - ignition::gazebo::gui::events::VisualSdfPlugin visualPluginEvent( - it.first, plugin); + ignition::gazebo::gui::events::VisualPlugin visualPluginEvent( + it.first, plugin.ToElement()); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &visualPluginEvent); From c3dbeb77ce46723301faf05927c9069dc1c6a1be Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 8 Jun 2022 09:42:08 -0700 Subject: [PATCH 13/14] Remove components::VisualPlugins Signed-off-by: Nate Koenig --- include/ignition/gazebo/Conversions.hh | 50 +++++++++++++++++++ include/ignition/gazebo/components/Visual.hh | 10 ---- src/Conversions.cc | 32 ++++++++++++ src/SdfEntityCreator.cc | 4 +- .../plugins/scene_manager/GzSceneManager.cc | 15 +++--- 5 files changed, 94 insertions(+), 17 deletions(-) diff --git a/include/ignition/gazebo/Conversions.hh b/include/ignition/gazebo/Conversions.hh index e39b579340..5168d67f78 100644 --- a/include/ignition/gazebo/Conversions.hh +++ b/include/ignition/gazebo/Conversions.hh @@ -27,6 +27,8 @@ #include #include #include +#include +#include #include #include #include @@ -742,6 +744,54 @@ namespace ignition /// \return Plugin message. template<> msgs::Plugin convert(const sdf::Plugin &_in); + + /// \brief Generic conversion from an SDF plugins to another type. + /// \param[in] _in SDF plugins. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const sdf::Plugins &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from an SDF plugins to a plugin_v message. + /// \param[in] _in SDF plugins. + /// \return Plugin_V message. + template<> + msgs::Plugin_V convert(const sdf::Plugins &_in); + + /// \brief Generic conversion from a msgs::Plugin to another type. + /// \param[in] _in msgs::Plugin. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const msgs::Plugin &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from a msgs::Plugin to an sdf::Plugin. + /// \param[in] _in msgs::Plugin. + /// \return sdf::Plugin. + template<> + sdf::Plugin convert(const sdf::Plugin &_in); + + /// \brief Generic conversion from a msgs::Plugin_V to another type. + /// \param[in] _in msgs::Plugin_V. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const msgs::Plugin_V &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from a msgs::Plugin_V to an sdf::Plugins. + /// \param[in] _in msgs::Plugin_V. + /// \return sdf::Plugins. + template<> + sdf::Plugins convert(const msgs::Plugin_V &_in); } } } diff --git a/include/ignition/gazebo/components/Visual.hh b/include/ignition/gazebo/components/Visual.hh index 0457d69540..de94818059 100644 --- a/include/ignition/gazebo/components/Visual.hh +++ b/include/ignition/gazebo/components/Visual.hh @@ -81,21 +81,11 @@ namespace components IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Visual", Visual) /// \brief A component that contains a visual plugin's SDF element. - /// \note This will be deprecated in Gazebo 7 (Garden), please the use - /// sdf::Plugins interface. using VisualPlugin = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.VisualPlugin", VisualPlugin) - - /// \brief A component that contains a visual plugin's SDF Plugin objects. - using VisualPlugins = Component>; - IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.VisualPlugins", - VisualPlugins) - } } } diff --git a/src/Conversions.cc b/src/Conversions.cc index 28f07167d3..ac2bda406a 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -1792,3 +1792,35 @@ msgs::Plugin ignition::gazebo::convert(const sdf::Plugin &_in) return result; } + +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +msgs::Plugin_V ignition::gazebo::convert(const sdf::Plugins &_in) +{ + msgs::Plugin_V result; + for (const sdf::Plugin &plugin : _in) + { + result.add_plugins()->CopyFrom(convert(plugin)); + } + return result; +} + +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +sdf::Plugin ignition::gazebo::convert(const msgs::Plugin &_in) +{ + return sdf::Plugin(_in.filename(), _in.name(), _in.innerxml()); +} + +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +sdf::Plugins ignition::gazebo::convert(const msgs::Plugin_V &_in) +{ + sdf::Plugins result; + for (int i = 0; i < _in.plugins_size(); ++i) + result.push_back(convert(_in.plugins(i))); + return result; +} diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index cf1ea82830..ab273111df 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -71,6 +71,7 @@ #include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/Static.hh" +#include "ignition/gazebo/components/SystemPluginInfo.hh" #include "ignition/gazebo/components/ThermalCamera.hh" #include "ignition/gazebo/components/ThreadPitch.hh" #include "ignition/gazebo/components/Transparency.hh" @@ -757,7 +758,8 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) if (!_visual->Plugins().empty()) { this->dataPtr->ecm->CreateComponent(visualEntity, - components::VisualPlugins(_visual->Plugins())); + components::SystemPluginInfo( + convert(_visual->Plugins()))); } // Keep track of visuals so we can load their plugins after loading the diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 4fa3b551c4..f04c61d07a 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -36,6 +36,7 @@ #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/SystemPluginInfo.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/gui/GuiEvents.hh" @@ -137,26 +138,28 @@ void GzSceneManager::Update(const UpdateInfo &_info, std::map plugins; if (!this->dataPtr->initializedVisualPlugins) { - _ecm.Each( + _ecm.Each( [&](const Entity &_entity, const components::Visual *, - const components::VisualPlugins *_plugins)->bool + const components::SystemPluginInfo *_plugins)->bool { + sdf::Plugins convertedPlugins = convert(_plugins->Data()); plugins[_entity].insert(plugins[_entity].end(), - _plugins->Data().begin(), _plugins->Data().end()); + convertedPlugins.begin(), convertedPlugins.end()); return true; }); this->dataPtr->initializedVisualPlugins = true; } else { - _ecm.EachNew( + _ecm.EachNew( [&](const Entity &_entity, const components::Visual *, - const components::VisualPlugins *_plugins)->bool + const components::SystemPluginInfo *_plugins)->bool { + sdf::Plugins convertedPlugins = convert(_plugins->Data()); plugins[_entity].insert(plugins[_entity].end(), - _plugins->Data().begin(), _plugins->Data().end()); + convertedPlugins.begin(), convertedPlugins.end()); return true; }); } From afd30fbb79c7c81cf9d7bb056179fa6cd92a0d94 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 23 Jun 2022 09:46:34 -0700 Subject: [PATCH 14/14] Address comments Signed-off-by: Nate Koenig --- include/ignition/gazebo/Conversions.hh | 2 +- include/ignition/gazebo/gui/GuiEvents.hh | 2 +- src/Conversions_TEST.cc | 47 ++++++++++++++++++++++++ src/LevelManager.cc | 1 + src/SdfEntityCreator.cc | 35 ++++++++++++++++++ src/ServerConfig.cc | 11 +++--- 6 files changed, 91 insertions(+), 7 deletions(-) diff --git a/include/ignition/gazebo/Conversions.hh b/include/ignition/gazebo/Conversions.hh index 5168d67f78..3b37d2cae1 100644 --- a/include/ignition/gazebo/Conversions.hh +++ b/include/ignition/gazebo/Conversions.hh @@ -775,7 +775,7 @@ namespace ignition /// \param[in] _in msgs::Plugin. /// \return sdf::Plugin. template<> - sdf::Plugin convert(const sdf::Plugin &_in); + sdf::Plugin convert(const msgs::Plugin &_in); /// \brief Generic conversion from a msgs::Plugin_V to another type. /// \param[in] _in msgs::Plugin_V. diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index c9807b3915..a0f861088e 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -254,7 +254,7 @@ namespace events /// \brief Get the SDF Plugin of the visual plugin public: const sdf::Plugins &Plugins() const; - static const QEvent::Type kType = QEvent::Type(QEvent::User + 8); + static const QEvent::Type kType = QEvent::Type(QEvent::User + 9); /// \internal /// \brief Private data pointer diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 4dc334fd6f..f1876943b4 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -1109,3 +1109,50 @@ TEST(Conversions, Plugin) EXPECT_NE(pluginMsg.innerxml().find("0.5"), std::string::npos) << pluginMsg.innerxml(); } + +///////////////////////////////////////////////// +TEST(Conversions, MsgsPluginToSdf) +{ + std::string innerXml ="another_test\n"; + std::string innerXml2 ="butter\n"; + + msgs::Plugin msgPlugin; + msgPlugin.set_name("foo"); + msgPlugin.set_filename("bar"); + msgPlugin.set_innerxml(innerXml); + + // Test conversion of a single msgs::Plugin to sdf::Plugin + sdf::Plugin sdfPlugin = convert(msgPlugin); + EXPECT_EQ("foo", sdfPlugin.Name()); + EXPECT_EQ("bar", sdfPlugin.Filename()); + ASSERT_EQ(1u, sdfPlugin.Contents().size()); + EXPECT_EQ(innerXml, sdfPlugin.Contents()[0]->ToString("")); + + // Test conversion of a msgs::Plugin_V with 1 plugin to sdf::Plugins + msgs::Plugin_V msgsPlugin; + msgsPlugin.add_plugins()->CopyFrom(msgPlugin); + sdf::Plugins sdfPlugins = convert(msgsPlugin); + ASSERT_EQ(1u, sdfPlugins.size()); + EXPECT_EQ("foo", sdfPlugins[0].Name()); + EXPECT_EQ("bar", sdfPlugins[0].Filename()); + ASSERT_EQ(1u, sdfPlugins[0].Contents().size()); + EXPECT_EQ(innerXml, sdfPlugins[0].Contents()[0]->ToString("")); + + // Add another plugin the msgs::Plugin_V + msgs::Plugin anotherPlugin; + anotherPlugin.set_name("sandwich"); + anotherPlugin.set_filename("time"); + anotherPlugin.set_innerxml(innerXml + innerXml2); + msgsPlugin.add_plugins()->CopyFrom(anotherPlugin); + sdfPlugins = convert(msgsPlugin); + ASSERT_EQ(2u, sdfPlugins.size()); + EXPECT_EQ("foo", sdfPlugins[0].Name()); + EXPECT_EQ("bar", sdfPlugins[0].Filename()); + ASSERT_EQ(1u, sdfPlugins[0].Contents().size()); + EXPECT_EQ(innerXml, sdfPlugins[0].Contents()[0]->ToString("")); + EXPECT_EQ("sandwich", sdfPlugins[1].Name()); + EXPECT_EQ("time", sdfPlugins[1].Filename()); + ASSERT_EQ(2u, sdfPlugins[1].Contents().size()); + EXPECT_EQ(innerXml, sdfPlugins[1].Contents()[0]->ToString("")); + EXPECT_EQ(innerXml2, sdfPlugins[1].Contents()[1]->ToString("")); +} diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 75bc5e2ecf..ffb53ad74a 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -231,6 +231,7 @@ void LevelManager::ReadLevelPerformerInfo() // Load world plugins. this->runner->EventMgr().Emit(this->worldEntity, this->runner->sdfWorld->Plugins()); + // Deprecate this in Garden this->runner->EventMgr().Emit(this->worldEntity, this->runner->sdfWorld->Element()); diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index ab273111df..0fbcdceb50 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -324,6 +324,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) this->dataPtr->eventManager->Emit(worldEntity, _world->Plugins()); + for (const sdf::Plugin &p : _world->Plugins()) + { + this->dataPtr->eventManager->Emit(worldEntity, + p.ToElement()); + } // Store the world's SDF DOM to be used when saving the world to file this->dataPtr->ecm->CreateComponent( @@ -343,6 +348,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model) for (const auto &[entity, plugins] : this->dataPtr->newModels) { this->dataPtr->eventManager->Emit(entity, plugins); + for (const sdf::Plugin &p : plugins) + { + this->dataPtr->eventManager->Emit(entity, + p.ToElement()); + } } this->dataPtr->newModels.clear(); @@ -350,6 +360,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model) for (const auto &[entity, plugins] : this->dataPtr->newSensors) { this->dataPtr->eventManager->Emit(entity, plugins); + for (const sdf::Plugin &p : plugins) + { + this->dataPtr->eventManager->Emit(entity, + p.ToElement()); + } } this->dataPtr->newSensors.clear(); @@ -357,6 +372,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model) for (const auto &[entity, plugins] : this->dataPtr->newVisuals) { this->dataPtr->eventManager->Emit(entity, plugins); + for (const sdf::Plugin &p : plugins) + { + this->dataPtr->eventManager->Emit(entity, + p.ToElement()); + } } this->dataPtr->newVisuals.clear(); @@ -490,6 +510,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Actor *_actor) // Actor plugins this->dataPtr->eventManager->Emit(actorEntity, _actor->Plugins()); + for (const sdf::Plugin &p : _actor->Plugins()) + { + this->dataPtr->eventManager->Emit(actorEntity, + p.ToElement()); + } return actorEntity; } @@ -761,6 +786,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) components::SystemPluginInfo( convert(_visual->Plugins()))); } + // Deprecate this in Garden + if (_visual->Element()) + { + sdf::ElementPtr pluginElem = _visual->Element()->FindElement("plugin"); + if (pluginElem) + { + this->dataPtr->ecm->CreateComponent(visualEntity, + components::VisualPlugin(pluginElem)); + } + } // Keep track of visuals so we can load their plugins after loading the // entire model and having its full scoped name. diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 9482ed4f12..00e31e9e8b 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -82,6 +82,8 @@ class ignition::gazebo::ServerConfig::PluginInfoPrivate public: sdf::Plugin plugin; /// \brief _filename The plugin library. + // Remove this in Garden, and rely solely on the plugin variable. + // Requires: https://github.com/gazebosim/sdformat/pull/1055 public: std::string filename = ""; /// \brief Name of the plugin implementation. @@ -89,12 +91,11 @@ class ignition::gazebo::ServerConfig::PluginInfoPrivate /// \brief XML elements associated with this plugin public: sdf::ElementPtr sdf = nullptr; - }; ////////////////////////////////////////////////// ServerConfig::PluginInfo::PluginInfo() -: dataPtr(new ServerConfig::PluginInfoPrivate) +: dataPtr(std::make_unique()) { } @@ -107,7 +108,7 @@ ServerConfig::PluginInfo::PluginInfo(const std::string &_entityName, const std::string &_filename, const std::string &_name, const sdf::ElementPtr &_sdf) - : dataPtr(new ServerConfig::PluginInfoPrivate()) + : dataPtr(std::make_unique()) { if (_sdf) { @@ -126,8 +127,8 @@ ServerConfig::PluginInfo::PluginInfo(const std::string &_entityName, ServerConfig::PluginInfo::PluginInfo(const std::string &_entityName, const std::string &_entityType, const sdf::Plugin &_plugin) - : dataPtr(new ServerConfig::PluginInfoPrivate(_entityName, _entityType, - _plugin)) + : dataPtr(std::make_unique(_entityName, + _entityType, _plugin)) { }