From ccbda485c8b65ec465b3d66108eb28f2edc07f35 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 23 Jun 2022 12:47:09 -0700 Subject: [PATCH 01/10] Use more sdf::Plugin instead of sdf::ElementPtr (#1352) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Michael Carroll Co-authored-by: Alejandro Hernández Cordero Co-authored-by: Ian Chen --- include/ignition/gazebo/Conversions.hh | 50 +++++++ include/ignition/gazebo/Events.hh | 16 ++- include/ignition/gazebo/ServerConfig.hh | 39 ++++++ include/ignition/gazebo/SystemLoader.hh | 11 ++ include/ignition/gazebo/gui/GuiEvents.hh | 25 ++++ src/Conversions.cc | 32 +++++ src/Conversions_TEST.cc | 47 +++++++ src/LevelManager.cc | 3 + src/SdfEntityCreator.cc | 79 ++++++++--- src/ServerConfig.cc | 128 +++++++++++------- src/ServerConfig_TEST.cc | 19 ++- src/ServerPrivate.cc | 90 +++++------- src/Server_TEST.cc | 67 +++++++++ src/SimulationRunner.cc | 23 ++-- src/SimulationRunner.hh | 17 +-- src/SimulationRunner_TEST.cc | 12 +- src/SystemLoader.cc | 77 ++++++----- src/SystemLoader_TEST.cc | 8 +- src/SystemManager.cc | 10 +- src/SystemManager.hh | 10 +- src/gui/GuiEvents.cc | 30 ++++ src/gui/GuiRunner.cc | 39 ++++-- .../plugins/scene_manager/GzSceneManager.cc | 38 ++++-- 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 +- 28 files changed, 661 insertions(+), 253 deletions(-) diff --git a/include/ignition/gazebo/Conversions.hh b/include/ignition/gazebo/Conversions.hh index e39b579340..3b37d2cae1 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 msgs::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/Events.hh b/include/ignition/gazebo/Events.hh index 672c264c03..c2d1dbfa89 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 @@ -53,11 +54,24 @@ 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. + /// \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 connect to the LoadPlugins event. + using LoadSdfPlugins = common::EventT; } } // namespace events } // namespace gazebo diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index 2afae36d32..74927d6cef 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -86,12 +87,26 @@ 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. + /// \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, 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); @@ -127,32 +142,56 @@ namespace ignition /// \brief Get the plugin library filename. /// \return Plugin library filename. + /// \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. + /// \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. + /// \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. + /// \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. + /// \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. + /// \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. + /// \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..592027394a 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,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. + /// \note This will be deprecated in Gazebo 7 (Garden), please the use + /// sdf::Plugin interface. public: std::optional LoadPlugin( const sdf::ElementPtr &_sdf); @@ -61,11 +64,19 @@ 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. + /// \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, 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/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index afbb751879..a0f861088e 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" @@ -215,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 + /// VisualPlugins class. class IGNITION_GAZEBO_GUI_VISIBLE VisualPlugin: public QEvent { /// \brief Constructor @@ -235,6 +238,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 VisualPlugins: public QEvent + { + /// \brief Constructor + /// \param[in] _entity Visual entity id + /// \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::Plugins &Plugins() const; + + static const QEvent::Type kType = QEvent::Type(QEvent::User + 9); + + /// \internal + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) + }; } // namespace events } } // namespace gui 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/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 81fa242484..ffb53ad74a 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -229,6 +229,9 @@ void LevelManager::ReadLevelPerformerInfo() this->ConfigureDefaultLevel(); // 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 afb49a05cb..39dd0105d1 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -72,6 +72,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" @@ -90,15 +91,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; @@ -322,8 +323,13 @@ 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()); + 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( @@ -340,23 +346,38 @@ 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); + for (const sdf::Plugin &p : plugins) + { + this->dataPtr->eventManager->Emit(entity, + p.ToElement()); + } } 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); + for (const sdf::Plugin &p : plugins) + { + this->dataPtr->eventManager->Emit(entity, + p.ToElement()); + } } 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); + for (const sdf::Plugin &p : plugins) + { + this->dataPtr->eventManager->Emit(entity, + p.ToElement()); + } } this->dataPtr->newVisuals.clear(); @@ -383,8 +404,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. @@ -451,7 +475,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"; @@ -463,7 +487,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; } @@ -484,8 +509,13 @@ 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()); + for (const sdf::Plugin &p : _actor->Plugins()) + { + this->dataPtr->eventManager->Emit(actorEntity, + p.ToElement()); + } return actorEntity; } @@ -751,9 +781,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) } // store the plugin in a component + if (!_visual->Plugins().empty()) + { + this->dataPtr->ecm->CreateComponent(visualEntity, + components::SystemPluginInfo( + convert(_visual->Plugins()))); + } + // Deprecate this in Garden if (_visual->Element()) { - sdf::ElementPtr pluginElem = _visual->Element()->FindElement("plugin"); + sdf::ElementPtr pluginElem = _visual->Element()->FindElement("plugin"); if (pluginElem) { this->dataPtr->ecm->CreateComponent(visualEntity, @@ -763,7 +800,8 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) // 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; } @@ -965,7 +1003,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/ServerConfig.cc b/src/ServerConfig.cc index d410168d09..00e31e9e8b 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -42,11 +42,11 @@ 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) { - if (_info->sdf) - this->sdf = _info->sdf->Clone(); + this->sdf = plugin.Element(); } /// \brief Constructor based on values. @@ -56,22 +56,19 @@ 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)), + filename(plugin.Filename()), + name(plugin.Name()), + sdf(plugin.Element()) { } @@ -81,7 +78,12 @@ 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. + // 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. @@ -93,7 +95,7 @@ class ignition::gazebo::ServerConfig::PluginInfoPrivate ////////////////////////////////////////////////// ServerConfig::PluginInfo::PluginInfo() -: dataPtr(new ServerConfig::PluginInfoPrivate) +: dataPtr(std::make_unique()) { } @@ -106,11 +108,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(std::make_unique()) { 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(std::make_unique(_entityName, + _entityType, _plugin)) +{ } ////////////////////////////////////////////////// @@ -161,6 +180,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 +193,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 +207,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 { @@ -573,45 +617,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; @@ -619,37 +651,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 @@ -657,19 +685,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); } ///////////////////////////////////////////////// @@ -838,9 +863,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; } @@ -863,7 +890,6 @@ ignition::gazebo::parsePluginsFromString(const std::string &_str) return parsePluginsFromDoc(doc); } - ///////////////////////////////////////////////// std::list ignition::gazebo::loadPluginInfo(bool _isPlayback) diff --git a/src/ServerConfig_TEST.cc b/src/ServerConfig_TEST.cc index bc93120ef2..7e5df2cd25 100644 --- a/src/ServerConfig_TEST.cc +++ b/src/ServerConfig_TEST.cc @@ -63,21 +63,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()); } ////////////////////////////////////////////////// @@ -115,21 +121,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()); } ////////////////////////////////////////////////// @@ -202,14 +214,17 @@ 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); 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()); EXPECT_TRUE(common::unsetenv(gazebo::kServerConfigPathEnv)); } @@ -225,7 +240,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/ServerPrivate.cc b/src/ServerPrivate.cc index 55f8784154..ef792687dc 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 619e08b2b1..e0944c8095 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)) { @@ -423,6 +489,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 715df805d3..f4616bbcb4 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -122,7 +122,7 @@ 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)); @@ -856,11 +856,9 @@ void SimulationRunner::Step(const UpdateInfo &_info) ////////////////////////////////////////////////// void SimulationRunner::LoadPlugin(const Entity _entity, - const std::string &_fname, - const std::string &_name, - const sdf::ElementPtr &_sdf) + const sdf::Plugin &_plugin) { - this->systemMgr->LoadPlugin(_entity, _fname, _name, _sdf); + this->systemMgr->LoadPlugin(_entity, _plugin); } ////////////////////////////////////////////////// @@ -940,7 +938,7 @@ void SimulationRunner::LoadServerPlugins( if (kNullEntity != entity) { - this->LoadPlugin(entity, plugin.Filename(), plugin.Name(), plugin.Sdf()); + this->LoadPlugin(entity, plugin.Plugin()); } } } @@ -972,23 +970,18 @@ void SimulationRunner::LoadLoggingPlugins(const ServerConfig &_config) ////////////////////////////////////////////////// void SimulationRunner::LoadPlugins(const Entity _entity, - const sdf::ElementPtr &_sdf) + const sdf::Plugins &_plugins) { - sdf::ElementPtr pluginElem = _sdf->FindElement("plugin"); - while (pluginElem) + for (const sdf::Plugin &plugin : _plugins) { - 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__") + if (plugin.Filename() != "__default__" && plugin.Name() != "__default__") { - this->LoadPlugin(_entity, filename, name, pluginElem); + this->LoadPlugin(_entity, plugin); } - - pluginElem = pluginElem->GetNextElement("plugin"); } } diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 351dfe6fa0..99b66c1c95 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -137,20 +137,15 @@ namespace ignition 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) - public: void LoadPlugin(const Entity _entity, - const std::string &_fname, - const std::string &_name, - const sdf::ElementPtr &_sdf); + /// \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] _sdf SDF element + /// \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::ElementPtr &_sdf); + const sdf::Plugins &_plugins); /// \brief Load server plugins for a given entity. /// \param[in] _config Configuration to load plugins from. diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 3d525f0f23..5f0e91e26c 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 3a78dda509..fbf7ae020e 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -41,10 +41,8 @@ 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) + public: bool InstantiateSystemPlugin(const sdf::Plugin &_sdfPlugin, + ignition::plugin::PluginPtr &_gzPlugin) { ignition::common::SystemPaths systemPaths; systemPaths.SetPluginPathEnv(pluginPathEnv); @@ -57,13 +55,13 @@ class ignition::gazebo::SystemLoaderPrivate systemPaths.AddPluginPaths(homePath + "/.ignition/gazebo/plugins"); systemPaths.AddPluginPaths(IGN_GAZEBO_PLUGIN_INSTALL_DIR); - auto pathToLib = systemPaths.FindSharedLibrary(_filename); + auto pathToLib = systemPaths.FindSharedLibrary(_sdfPlugin.Filename()); if (pathToLib.empty()) { // We assume ignition::gazebo corresponds to the levels feature - if (_name != "ignition::gazebo") + if (_sdfPlugin.Name() != "ignition::gazebo") { - ignerr << "Failed to load system plugin [" << _filename << + ignerr << "Failed to load system plugin [" << _sdfPlugin.Filename() << "] : couldn't find shared library." << std::endl; } return false; @@ -72,7 +70,7 @@ class ignition::gazebo::SystemLoaderPrivate auto pluginNames = this->loader.LoadLib(pathToLib); if (pluginNames.empty()) { - ignerr << "Failed to load system plugin [" << _filename << + ignerr << "Failed to load system plugin [" << _sdfPlugin.Filename() << "] : couldn't load library on path [" << pathToLib << "]." << std::endl; return false; @@ -81,31 +79,31 @@ class ignition::gazebo::SystemLoaderPrivate auto pluginName = *pluginNames.begin(); if (pluginName.empty()) { - ignerr << "Failed to load system plugin [" << _filename << + ignerr << "Failed to load system plugin [" << _sdfPlugin.Filename() << "] : couldn't load library on path [" << pathToLib << "]." << std::endl; return false; } - _plugin = this->loader.Instantiate(_name); - if (!_plugin) + _gzPlugin = this->loader.Instantiate(_sdfPlugin.Name()); + if (!_gzPlugin) { - ignerr << "Failed to load system plugin [" << _name << - "] : could not instantiate from library [" << _filename << - "] from path [" << pathToLib << "]." << std::endl; + ignerr << "Failed to load system plugin [" << _sdfPlugin.Name() << + "] : could not instantiate from library [" << _sdfPlugin.Filename() << + "] from path [" << pathToLib << "]." << std::endl; return false; } - if (!_plugin->HasInterface()) + if (!_gzPlugin->HasInterface()) { - ignerr << "Failed to load system plugin [" << _name << - "] : system not found in library [" << _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(_plugin); + this->systemPluginsAdded.insert(_gzPlugin); return true; } @@ -143,8 +141,6 @@ 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 " @@ -153,15 +149,11 @@ std::optional SystemLoader::LoadPlugin( return {}; } - auto ret = this->dataPtr->InstantiateSystemPlugin(_filename, - _name, - _sdf, plugin); - if (ret && plugin) - { - return plugin; - } - - return {}; + sdf::Plugin plugin; + plugin.Load(_sdf); + plugin.SetFilename(_filename); + plugin.SetName(_name); + return LoadPlugin(plugin); } ////////////////////////////////////////////////// @@ -172,9 +164,30 @@ 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); +} + +////////////////////////////////////////////////// +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 {}; } ////////////////////////////////////////////////// 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 902804a59d..fbba44d71f 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -34,21 +34,19 @@ SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader, ////////////////////////////////////////////////// void SystemManager::LoadPlugin(const Entity _entity, - const std::string &_fname, - const std::string &_name, - const sdf::ElementPtr &_sdf) + const sdf::Plugin &_plugin) { std::optional system; { std::lock_guard lock(this->systemLoaderMutex); - system = this->systemLoader->LoadPlugin(_fname, _name, _sdf); + system = this->systemLoader->LoadPlugin(_plugin); } // System correctly loaded from library if (system) { - this->AddSystem(system.value(), _entity, _sdf); - igndbg << "Loaded system [" << _name + this->AddSystem(system.value(), _entity, _plugin.ToElement()); + igndbg << "Loaded system [" << _plugin.Name() << "] for entity [" << _entity << "]" << std::endl; } } diff --git a/src/SystemManager.hh b/src/SystemManager.hh index bb6db6e593..e267c19c93 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" @@ -52,13 +54,9 @@ namespace ignition /// \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) + /// \param[in] _plugin Plugin to load public: void LoadPlugin(const Entity _entity, - const std::string &_fname, - const std::string &_name, - const sdf::ElementPtr &_sdf); + const sdf::Plugin &_plugin); /// \brief Add a system to the manager /// \param[in] _system SystemPluginPtr to be added diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc index a89a5d8254..b9ce7e8812 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::VisualPlugins::Implementation +{ + /// \brief Entity to load the visual plugin for + public: ignition::gazebo::Entity entity; + + /// \brief Sdf plugins for the visual plugin + public: sdf::Plugins plugins; +}; + using namespace ignition; using namespace gazebo; using namespace gui; @@ -162,3 +171,24 @@ sdf::ElementPtr VisualPlugin::Element() const { return this->dataPtr->element; } + +///////////////////////////////////////////////// +VisualPlugins::VisualPlugins(ignition::gazebo::Entity _entity, + const sdf::Plugins &_plugins) : + QEvent(kType), dataPtr(utils::MakeImpl()) +{ + this->dataPtr->entity = _entity; + this->dataPtr->plugins = _plugins; +} + +///////////////////////////////////////////////// +ignition::gazebo::Entity VisualPlugins::Entity() const +{ + return this->dataPtr->entity; +} + +///////////////////////////////////////////////// +const sdf::Plugins &VisualPlugins::Plugins() const +{ + return this->dataPtr->plugins; +} diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 2a70c033ac..ab8a8d43e9 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,22 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->node.Request(this->dataPtr->controlService, req, cb); } } + else if (_event->type() == + ignition::gazebo::gui::events::VisualPlugins::kType) + { + auto visualPluginEvent = + reinterpret_cast(_event); + if (visualPluginEvent) + { + std::lock_guard lock(this->dataPtr->systemLoadMutex); + + Entity entity = visualPluginEvent->Entity(); + 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) { auto visualPluginEvent = @@ -203,11 +219,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 +348,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 +369,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..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" @@ -134,39 +135,52 @@ 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::SystemPluginInfo *_plugins)->bool { - sdf::ElementPtr pluginElem = _plugin->Data(); - pluginElems[_entity] = _plugin->Data(); + sdf::Plugins convertedPlugins = convert(_plugins->Data()); + plugins[_entity].insert(plugins[_entity].end(), + convertedPlugins.begin(), convertedPlugins.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::SystemPluginInfo *_plugins)->bool { - sdf::ElementPtr pluginElem = _plugin->Data(); - pluginElems[_entity] = _plugin->Data(); + sdf::Plugins convertedPlugins = convert(_plugins->Data()); + plugins[_entity].insert(plugins[_entity].end(), + convertedPlugins.begin(), convertedPlugins.end()); return true; }); } - for (const auto &it : pluginElems) + for (const auto &it : plugins) { - ignition::gazebo::gui::events::VisualPlugin visualPluginEvent( + // Send the new VisualPlugins event + ignition::gazebo::gui::events::VisualPlugins visualPluginsEvent( it.first, it.second); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), - &visualPluginEvent); + &visualPluginsEvent); + + // Send the old VisualPlugin event + for (const sdf::Plugin &plugin : it.second) + { + ignition::gazebo::gui::events::VisualPlugin visualPluginEvent( + it.first, plugin.ToElement()); + 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 f640add611..789d57fefd 100644 --- a/test/integration/sensors_system.cc +++ b/test/integration/sensors_system.cc @@ -101,9 +101,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 4f8c91b90361e600d7f3986347ab1b5a4a7ff5d8 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 23 Jun 2022 14:41:44 -0700 Subject: [PATCH 02/10] Added particle emitters to scene broadcaster (#1516) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Carlos Agüero Co-authored-by: Ian Chen --- .../scene_broadcaster/SceneBroadcaster.cc | 51 +++++++++++++++++ test/integration/scene_broadcaster_system.cc | 57 +++++++++++++++++++ test/worlds/particle_emitter2.sdf | 4 ++ 3 files changed, 112 insertions(+) diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 91cdf15e34..a8c5d2d857 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -49,6 +49,7 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParticleEmitter.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RgbdCamera.hh" #include "ignition/gazebo/components/Scene.hh" @@ -156,6 +157,15 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate public: static void AddSensors(msgs::Link *_msg, const Entity _entity, const SceneGraphType &_graph); + /// \brief Adds particle emitters to a msgs::Link object based on the + /// contents of the scene graph + /// \param[inout] _msg Pointer to msg object to which the particle + /// emitters will be added. + /// \param[in] _entity Parent entity in the graph + /// \param[in] _graph Scene graph + public: static void AddParticleEmitters(msgs::Link *_msg, + const Entity _entity, const SceneGraphType &_graph); + /// \brief Recursively remove entities from the graph /// \param[in] _entity Entity /// \param[in/out] _graph Scene graph @@ -995,6 +1005,26 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( return true; }); + // Particle emitters + _manager.EachNew( + [&](const Entity &_entity, + const components::ParticleEmitter *_emitterComp, + const components::ParentEntity *_parentComp, + const components::Pose *_poseComp) -> bool + { + auto emitterMsg = std::make_shared(); + emitterMsg->CopyFrom(_emitterComp->Data()); + emitterMsg->set_id(_entity); + emitterMsg->mutable_pose()->CopyFrom(msgs::Convert(_poseComp->Data())); + + // Add to graph + newGraph.AddVertex(emitterMsg->name(), emitterMsg, _entity); + newGraph.AddEdge({_parentComp->Data(), _entity}, true); + newEntity = true; + return true; + }); + // Update the whole scene graph from the new graph { std::lock_guard lock(this->graphMutex); @@ -1158,6 +1188,24 @@ void SceneBroadcasterPrivate::AddSensors(msgs::Link *_msg, const Entity _entity, } } +////////////////////////////////////////////////// +void SceneBroadcasterPrivate::AddParticleEmitters(msgs::Link *_msg, + const Entity _entity, const SceneGraphType &_graph) +{ + if (!_msg) + return; + + for (const auto &vertex : _graph.AdjacentsFrom(_entity)) + { + auto emitterMsg = std::dynamic_pointer_cast( + vertex.second.get().Data()); + if (!emitterMsg) + continue; + + _msg->add_particle_emitter()->CopyFrom(*emitterMsg); + } +} + ////////////////////////////////////////////////// void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity, const SceneGraphType &_graph) @@ -1183,6 +1231,9 @@ void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity, // Sensors AddSensors(msgOut, vertex.second.get().Id(), _graph); + + // Particle emitters + AddParticleEmitters(msgOut, vertex.second.get().Id(), _graph); } } diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 41ca64e113..878e200e73 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -952,6 +952,63 @@ TEST_P(SceneBroadcasterTest, EXPECT_FALSE(res.has_sky()); } +TEST_P(SceneBroadcasterTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SceneInfoHasParticleEmitter)) +{ + // Start server + ignition::gazebo::ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + common::joinPaths("/", "test", "worlds", "particle_emitter2.sdf")); + + gazebo::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Run server + server.Run(true, 1, false); + + // Create requester + transport::Node node; + + bool result{false}; + unsigned int timeout{5000}; + ignition::msgs::Scene res; + + EXPECT_TRUE(node.Request("/world/particle_emitters/scene/info", + timeout, res, result)); + ASSERT_TRUE(result); + + ASSERT_EQ(3, res.model_size()); + int count = 0; + for (int i = 0; i < res.model_size(); ++i) + { + if (res.model(i).name() == "smoke_generator_demo_model") + { + count++; + // There should be one link + ASSERT_EQ(1, res.model(i).link_size()); + // The link should have one particle emitter + ASSERT_EQ(1, res.model(i).link(0).particle_emitter_size()); + + // Check a few parameter values to make sure we have the correct + // particle emittter + const msgs::ParticleEmitter &emitter = + res.model(i).link(0).particle_emitter(0); + EXPECT_EQ("smoke_emitter", emitter.name()); + EXPECT_EQ(math::Pose3d(0, 1, 0, 0, 0, 0), msgs::Convert(emitter.pose())); + EXPECT_EQ(math::Vector3d(2, 2, 2), msgs::Convert(emitter.size())); + EXPECT_DOUBLE_EQ(5.0, emitter.rate().data()); + EXPECT_DOUBLE_EQ(1.0, emitter.duration().data()); + EXPECT_EQ(math::Vector3d(3, 3, 3), + msgs::Convert(emitter.particle_size())); + EXPECT_DOUBLE_EQ(2.0, emitter.lifetime().data()); + } + } + + // Should have found 1 particle emitter. + EXPECT_EQ(1, count); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, SceneBroadcasterTest, ::testing::Range(1, 2)); diff --git a/test/worlds/particle_emitter2.sdf b/test/worlds/particle_emitter2.sdf index ca643ae13e..8d374ceff7 100644 --- a/test/worlds/particle_emitter2.sdf +++ b/test/worlds/particle_emitter2.sdf @@ -11,6 +11,10 @@ filename="ignition-gazebo-particle-emitter2-system" name="ignition::gazebo::systems::ParticleEmitter2"> + + true From 255e1e0d417fc3122b31df8bd3cfb52aab9fa091 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 23 Jun 2022 16:03:38 -0700 Subject: [PATCH 03/10] New service for adding systems to an entity (#1524) Signed-off-by: Ian Chen Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel --- CMakeLists.txt | 2 +- src/SimulationRunner.cc | 43 ++--- src/SystemManager.cc | 48 +++++- src/SystemManager.hh | 28 +++- test/integration/CMakeLists.txt | 1 + test/integration/entity_system.cc | 163 +++++++++++++++++++ test/worlds/diff_drive_no_plugin.sdf | 226 +++++++++++++++++++++++++++ 7 files changed, 488 insertions(+), 23 deletions(-) create mode 100644 test/integration/entity_system.cc create mode 100644 test/worlds/diff_drive_no_plugin.sdf diff --git a/CMakeLists.txt b/CMakeLists.txt index a566322711..3e1dd1e979 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,7 +74,7 @@ set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) #-------------------------------------- # Find ignition-msgs -ign_find_package(ignition-msgs8 REQUIRED VERSION 8.3) +ign_find_package(ignition-msgs8 REQUIRED VERSION 8.5) set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) #-------------------------------------- diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index f4616bbcb4..3f817dd1c5 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -112,9 +112,28 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, static_cast(this->stepSize.count() / this->desiredRtf)); } + // World control + transport::NodeOptions opts; + std::string ns{"/world/" + this->worldName}; + if (this->networkMgr) + { + ns = this->networkMgr->Namespace() + ns; + } + + auto validNs = transport::TopicUtils::AsValidTopic(ns); + if (validNs.empty()) + { + ignerr << "Invalid namespace [" << ns + << "], not initializing runner transport." << std::endl; + return; + } + opts.SetNameSpace(validNs); + + this->node = std::make_unique(opts); + // Create the system manager this->systemMgr = std::make_unique(_systemLoader, - &this->entityCompMgr, &this->eventMgr); + &this->entityCompMgr, &this->eventMgr, validNs); this->pauseConn = this->eventMgr.Connect( std::bind(&SimulationRunner::SetPaused, this, std::placeholders::_1)); @@ -185,25 +204,6 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, this->LoadLoggingPlugins(this->serverConfig); - // World control - transport::NodeOptions opts; - std::string ns{"/world/" + this->worldName}; - if (this->networkMgr) - { - ns = this->networkMgr->Namespace() + ns; - } - - auto validNs = transport::TopicUtils::AsValidTopic(ns); - if (validNs.empty()) - { - ignerr << "Invalid namespace [" << ns - << "], not initializing runner transport." << std::endl; - return; - } - opts.SetNameSpace(validNs); - - this->node = std::make_unique(opts); - // TODO(louise) Combine both messages into one. this->node->Advertise("control", &SimulationRunner::OnWorldControl, this); this->node->Advertise("control/state", &SimulationRunner::OnWorldControlState, @@ -807,6 +807,9 @@ void SimulationRunner::Step(const UpdateInfo &_info) // so that we can recreate entities with the same name. this->ProcessRecreateEntitiesRemove(); + // handle systems that need to be added + this->systemMgr->ProcessPendingEntitySystems(); + // Update all the systems. this->UpdateSystems(); diff --git a/src/SystemManager.cc b/src/SystemManager.cc index fbba44d71f..8440364fc8 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -25,11 +25,20 @@ using namespace gazebo; ////////////////////////////////////////////////// SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader, EntityComponentManager *_entityCompMgr, - EventManager *_eventMgr) + EventManager *_eventMgr, + const std::string &_namespace) : systemLoader(_systemLoader), entityCompMgr(_entityCompMgr), eventMgr(_eventMgr) { + transport::NodeOptions opts; + opts.SetNameSpace(_namespace); + this->node = std::make_unique(opts); + std::string entitySystemService{"entity/system/add"}; + this->node->Advertise(entitySystemService, + &SystemManager::EntitySystemAddService, this); + ignmsg << "Serving entity system service on [" + << "/" << entitySystemService << "]" << std::endl; } ////////////////////////////////////////////////// @@ -195,3 +204,40 @@ std::vector SystemManager::TotalByEntity(Entity _entity) std::back_inserter(result), checkEntity); return result; } + +////////////////////////////////////////////////// +bool SystemManager::EntitySystemAddService(const msgs::EntityPlugin_V &_req, + msgs::Boolean &_res) +{ + std::lock_guard lock(this->systemsMsgMutex); + this->systemsToAdd.push_back(_req); + _res.set_data(true); + return true; +} + +////////////////////////////////////////////////// +void SystemManager::ProcessPendingEntitySystems() +{ + std::lock_guard lock(this->systemsMsgMutex); + for (auto &req : this->systemsToAdd) + { + Entity entity = req.entity().id(); + + if (req.plugins().empty()) + { + ignwarn << "Unable to add plugins to Entity: '" << entity + << "'. No plugins specified." << std::endl; + continue; + } + + for (auto &pluginMsg : req.plugins()) + { + std::string fname = pluginMsg.filename(); + std::string name = pluginMsg.name(); + std::string innerxml = pluginMsg.innerxml(); + sdf::Plugin pluginSDF(fname, name, innerxml); + this->LoadPlugin(entity, pluginSDF); + } + } + this->systemsToAdd.clear(); +} diff --git a/src/SystemManager.hh b/src/SystemManager.hh index e267c19c93..c23ee38cee 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -17,11 +17,14 @@ #ifndef IGNITION_GAZEBO_SYSTEMMANAGER_HH_ #define IGNITION_GAZEBO_SYSTEMMANAGER_HH_ +#include + #include #include #include #include +#include #include "ignition/gazebo/config.hh" #include "ignition/gazebo/EntityComponentManager.hh" @@ -48,9 +51,11 @@ namespace ignition /// be used when configuring new systems /// \param[in] _eventMgr Pointer to the event manager to be used when /// configuring new systems + /// \param[in] _namespace Namespace to use for the transport node public: explicit SystemManager(const SystemLoaderPtr &_systemLoader, EntityComponentManager *_entityCompMgr = nullptr, - EventManager *_eventMgr = nullptr); + EventManager *_eventMgr = nullptr, + const std::string &_namespace = std::string()); /// \brief Load system plugin for a given entity. /// \param[in] _entity Entity @@ -110,6 +115,9 @@ namespace ignition /// \return Vector of systems. public: std::vector TotalByEntity(Entity _entity); + /// \brief Process system messages and add systems to entities + public: void ProcessPendingEntitySystems(); + /// \brief Implementation for AddSystem functions that takes an SDF /// element. This calls the AddSystemImpl that accepts an SDF Plugin. /// \param[in] _system Generic representation of a system. @@ -125,6 +133,15 @@ namespace ignition private: void AddSystemImpl(SystemInternal _system, const sdf::Plugin &_sdf); + /// \brief Callback for entity add system service. + /// \param[in] _req Request message containing the entity id and plugins + /// to add to that entity + /// \param[out] _res Response containing a boolean value indicating if + /// service request is received or not + /// \return True if request received. + private: bool EntitySystemAddService(const msgs::EntityPlugin_V &_req, + msgs::Boolean &_res); + /// \brief All the systems. private: std::vector systems; @@ -157,6 +174,15 @@ namespace ignition /// \brief Pointer to associated event manager private: EventManager *eventMgr; + + /// \brief A list of entity systems to add + private: std::vector systemsToAdd; + + /// \brief Mutex to protect systemsToAdd list + private: std::mutex systemsMsgMutex; + + /// \brief Node for communication. + private: std::unique_ptr node{nullptr}; }; } } // namespace gazebo diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 5b234d4d4e..4b1e1e7358 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -16,6 +16,7 @@ set(tests diff_drive_system.cc each_new_removed.cc entity_erase.cc + entity_system.cc events.cc examples_build.cc follow_actor_system.cc diff --git a/test/integration/entity_system.cc b/test/integration/entity_system.cc new file mode 100644 index 0000000000..d4ea7ce57c --- /dev/null +++ b/test/integration/entity_system.cc @@ -0,0 +1,163 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/EnvTestFixture.hh" +#include "../helpers/Relay.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test DiffDrive system +class EntitySystemTest : public InternalFixture<::testing::TestWithParam> +{ + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + protected: void TestPublishCmd(const std::string &_sdfFile, + const std::string &_cmdVelTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + Entity vehicleEntity = kNullEntity; + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, id); + vehicleEntity = id; + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + EXPECT_EQ(1000u, poses.size()); + + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + poses.clear(); + + // Publish command and check that vehicle still does not move + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + msgs::Twist msg; + const double desiredLinVel = 10.5; + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, 0)); + pub.Publish(msg); + server.Run(true, 1000, false); + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + poses.clear(); + + // send request to add diff_drive system + EXPECT_NE(kNullEntity, vehicleEntity); + msgs::EntityPlugin_V req; + auto ent = req.mutable_entity(); + ent->set_id(vehicleEntity); + auto plugin = req.add_plugins(); + plugin->set_name("ignition::gazebo::systems::DiffDrive"); + plugin->set_filename("ignition-gazebo-diff-drive-system"); + std::stringstream innerxml; + innerxml + << "left_wheel_joint\n" + << "right_wheel_joint\n" + << "1.25\n" + << "0.3\n" + << "1\n" + << "-1\n" + << "2\n" + << "-2\n" + << "0.5\n" + << "-0.5\n" + << "1\n" + << "-1\n"; + plugin->set_innerxml(innerxml.str()); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/diff_drive/entity/system/add"}; + + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // run once for the system to be added + server.Run(true, 1, false); + poses.clear(); + + // publish twist msg and verify that the vehicle now moves forward + pub.Publish(msg); + server.Run(true, 1000, false); + for (unsigned int i = 1; i < poses.size(); ++i) + { + EXPECT_GT(poses[i].Pos().X(), poses[i-1].Pos().X()); + } + } +}; + +///////////////////////////////////////////////// +// See https://github.com/gazebosim/gz-sim/issues/1175 +TEST_P(EntitySystemTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(PublishCmd)) +{ + TestPublishCmd( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/diff_drive_no_plugin.sdf", + "/model/vehicle/cmd_vel"); +} + +// Run multiple times +INSTANTIATE_TEST_SUITE_P(ServerRepeat, EntitySystemTest, + ::testing::Range(1, 2)); diff --git a/test/worlds/diff_drive_no_plugin.sdf b/test/worlds/diff_drive_no_plugin.sdf new file mode 100644 index 0000000000..1792199086 --- /dev/null +++ b/test/worlds/diff_drive_no_plugin.sdf @@ -0,0 +1,226 @@ + + + + + + 0.001 + 0 + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + + + From 6739800821af5e39767fe22e8da4b3e27fbaa284 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Thu, 23 Jun 2022 19:31:39 -0400 Subject: [PATCH 04/10] Fix various Protobuf Windows warnings on ign-gazebo6 (#1299) Signed-off-by: Jorge Perez Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel --- .../gazebo/components/Serialization.hh | 10 +++++++ src/CMakeLists.txt | 29 +++++++++++-------- src/gui/plugins/CMakeLists.txt | 10 +++++++ .../KineticEnergyMonitor.cc | 10 +++++++ src/systems/physics/CMakeLists.txt | 9 ++++++ .../triggered_publisher/TriggeredPublisher.cc | 9 ++++++ src/systems/user_commands/UserCommands.cc | 10 +++++++ src/systems/wind_effects/WindEffects.cc | 10 +++++++ test/integration/scene_broadcaster_system.cc | 10 +++++++ test/performance/CMakeLists.txt | 20 +++++++++++-- test/regression/CMakeLists.txt | 9 ++++++ 11 files changed, 121 insertions(+), 15 deletions(-) diff --git a/include/ignition/gazebo/components/Serialization.hh b/include/ignition/gazebo/components/Serialization.hh index 7ef0dd6639..ad062393ca 100644 --- a/include/ignition/gazebo/components/Serialization.hh +++ b/include/ignition/gazebo/components/Serialization.hh @@ -17,7 +17,17 @@ #ifndef IGNITION_GAZEBO_COMPONENTS_SERIALIZATION_HH_ #define IGNITION_GAZEBO_COMPONENTS_SERIALIZATION_HH_ +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + #include + +#ifdef _MSC_VER +#pragma warning(pop) +#endif + #include #include diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 2aef8d0996..ff178007c3 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -37,17 +37,9 @@ set(gui_sources PARENT_SCOPE ) -ign_add_component(ign - SOURCES - ign.cc - cmd/ModelCommandAPI.cc - GET_TARGET_NAME ign_lib_target) -target_link_libraries(${ign_lib_target} - PRIVATE - ${PROJECT_LIBRARY_TARGET_NAME} - ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} - ignition-gazebo${PROJECT_VERSION_MAJOR} - ignition-gazebo${PROJECT_VERSION_MAJOR}-gui +set(cli_sources + ign.cc + cmd/ModelCommandAPI.cc ) set (sources @@ -132,9 +124,22 @@ if (MSVC) # members that don't get exported, so they trigger this warning. However, the # warning is not important since those members do not need to be interfaced # with. - set_source_files_properties(${sources} ${gtest_sources} COMPILE_FLAGS "/wd4251 /wd4146") + set_source_files_properties(${sources} ${gtest_sources} ${cli_sources} COMPILE_FLAGS "/wd4251 /wd4146") endif() +# CLI +ign_add_component(ign + SOURCES + ${cli_sources} + GET_TARGET_NAME ign_lib_target) +target_link_libraries(${ign_lib_target} + PRIVATE + ${PROJECT_LIBRARY_TARGET_NAME} + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-gazebo${PROJECT_VERSION_MAJOR} + ignition-gazebo${PROJECT_VERSION_MAJOR}-gui +) + # Create the library target ign_create_core_library(SOURCES ${sources} CXX_STANDARD 17) target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 62fcf2d096..ba88ac4487 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -26,6 +26,16 @@ function(gz_add_gui_library library_name) QT5_WRAP_CPP(${library_name}_headers_MOC ${gz_add_gui_library_QT_HEADERS}) QT5_ADD_RESOURCES(${library_name}_RCC ${library_name}.qrc) + if(MSVC) + # Warning #4251 is the "dll-interface" warning that tells you when types + # used by a class are not being exported. These generated source files have + # private members that don't get exported, so they trigger this warning. + # However, the warning is not important since those members do not need to + # be interfaced with. + set_source_files_properties(${${library_name}_headers_MOC} COMPILE_FLAGS "/wd4251") + set_source_files_properties(${gz_add_gui_library_SOURCES} COMPILE_FLAGS "/wd4251") + endif() + add_library(${library_name} SHARED ${gz_add_gui_library_SOURCES} ${${library_name}_headers_MOC} diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc index 00978cc26f..fa36aea43c 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc @@ -15,7 +15,17 @@ * */ +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + #include + +#ifdef _MSC_VER +#pragma warning(pop) +#endif + #include #include diff --git a/src/systems/physics/CMakeLists.txt b/src/systems/physics/CMakeLists.txt index 8e4cfb072e..ce4d5bea4f 100644 --- a/src/systems/physics/CMakeLists.txt +++ b/src/systems/physics/CMakeLists.txt @@ -11,6 +11,15 @@ set (gtest_sources EntityFeatureMap_TEST.cc ) +if (MSVC) + # Warning #4251 is the "dll-interface" warning that tells you when types used + # by a class are not being exported. These generated source files have private + # members that don't get exported, so they trigger this warning. However, the + # warning is not important since those members do not need to be interfaced + # with. + set_source_files_properties(${gtest_sources} COMPILE_FLAGS "/wd4251 /wd4146") +endif() + ign_build_tests(TYPE UNIT SOURCES ${gtest_sources} diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index d817d55861..94a63b7ac8 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -17,10 +17,19 @@ #include "TriggeredPublisher.hh" +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + #include #include #include +#ifdef _MSC_VER +#pragma warning(pop) +#endif + #include #include diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 9ac7892eaa..25dfb42c20 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -17,7 +17,17 @@ #include "UserCommands.hh" +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + #include + +#ifdef _MSC_VER +#pragma warning(pop) +#endif + #include #include #include diff --git a/src/systems/wind_effects/WindEffects.cc b/src/systems/wind_effects/WindEffects.cc index 5389ba9c17..39eee5e760 100644 --- a/src/systems/wind_effects/WindEffects.cc +++ b/src/systems/wind_effects/WindEffects.cc @@ -17,7 +17,17 @@ #include "WindEffects.hh" +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + #include + +#ifdef _MSC_VER +#pragma warning(pop) +#endif + #include #include diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 878e200e73..13c1945c6e 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -16,8 +16,18 @@ */ #include + +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + #include +#ifdef _MSC_VER +#pragma warning(pop) +#endif + #include #include diff --git a/test/performance/CMakeLists.txt b/test/performance/CMakeLists.txt index 74219c4224..a9889b9c07 100644 --- a/test/performance/CMakeLists.txt +++ b/test/performance/CMakeLists.txt @@ -5,8 +5,22 @@ set(tests level_manager.cc ) +set(exec + sdf_runner +) + link_directories(${PROJECT_BINARY_DIR}/test) +if (MSVC) + # Warning #4251 is the "dll-interface" warning that tells you when types used + # by a class are not being exported. These generated source files have private + # members that don't get exported, so they trigger this warning. However, the + # warning is not important since those members do not need to be interfaced + # with. + set_source_files_properties(${tests} COMPILE_FLAGS "/wd4251 /wd4146") + set_source_files_properties(${exec}.cc COMPILE_FLAGS "/wd4251 /wd4146") +endif() + ign_build_tests(TYPE PERFORMANCE SOURCES ${tests} @@ -15,12 +29,12 @@ ign_build_tests(TYPE PERFORMANCE ) add_executable( - PERFORMANCE_sdf_runner - sdf_runner.cc + PERFORMANCE_${exec} + ${exec}.cc ) target_link_libraries( - PERFORMANCE_sdf_runner + PERFORMANCE_${exec} ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ignition-gazebo${PROJECT_VERSION_MAJOR} ignition-gazebo${PROJECT_VERSION_MAJOR}-gui diff --git a/test/regression/CMakeLists.txt b/test/regression/CMakeLists.txt index 9371b25e72..5d9dd5a880 100644 --- a/test/regression/CMakeLists.txt +++ b/test/regression/CMakeLists.txt @@ -5,6 +5,15 @@ set(tests link_directories(${PROJECT_BINARY_DIR}/test) +if (MSVC) + # Warning #4251 is the "dll-interface" warning that tells you when types used + # by a class are not being exported. These generated source files have private + # members that don't get exported, so they trigger this warning. However, the + # warning is not important since those members do not need to be interfaced + # with. + set_source_files_properties(${tests} COMPILE_FLAGS "/wd4251 /wd4146") +endif() + ign_build_tests(TYPE REGRESSION SOURCES ${tests} From 5fc968dd611317f848c188162e47f6b7bb8b3b24 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 24 Jun 2022 16:12:47 -0700 Subject: [PATCH 05/10] Expose the ability to stop a server (#1551) Signed-off-by: Nate Koenig Signed-off-by: Louise Poubel Co-authored-by: Nate Koenig Co-authored-by: Louise Poubel --- include/ignition/gazebo/Server.hh | 3 ++ src/Server.cc | 6 ++++ src/Server_TEST.cc | 57 ++++++++++++++++++++++--------- 3 files changed, 50 insertions(+), 16 deletions(-) diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index 088fda99d8..3a70bc02d8 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -285,6 +285,9 @@ namespace ignition bool _recursive = true, const unsigned int _worldIndex = 0); + /// \brief Stop the server. This will stop all running simulations. + public: void Stop(); + /// \brief Private data private: std::unique_ptr dataPtr; }; diff --git a/src/Server.cc b/src/Server.cc index 87381ee516..d6edfd0cba 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -377,3 +377,9 @@ bool Server::RequestRemoveEntity(const Entity _entity, return false; } + +////////////////////////////////////////////////// +void Server::Stop() +{ + this->dataPtr->Stop(); +} diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index e0944c8095..f18e16c8a4 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -231,8 +231,8 @@ TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(ServerConfigRealPlugin)) // Start server ServerConfig serverConfig; serverConfig.SetUpdateRate(10000); - serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/shapes.sdf"); + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "shapes.sdf")); sdf::ElementPtr sdf(new sdf::Element); sdf->SetName("plugin"); @@ -338,8 +338,8 @@ TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(SdfServerConfig)) EXPECT_FALSE(serverConfig.SdfString().empty()); // Setting the SDF file should override the string. - serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/shapes.sdf"); + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "shapes.sdf")); EXPECT_FALSE(serverConfig.SdfFile().empty()); EXPECT_TRUE(serverConfig.SdfString().empty()); @@ -407,8 +407,7 @@ TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(SdfRootServerConfig)) ///////////////////////////////////////////////// TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(ServerConfigLogRecord)) { - auto logPath = common::joinPaths( - std::string(PROJECT_BINARY_PATH), "test_log_path"); + auto logPath = common::joinPaths(PROJECT_BINARY_PATH, "test_log_path"); auto logFile = common::joinPaths(logPath, "state.tlog"); auto compressedFile = logPath + ".zip"; @@ -447,8 +446,7 @@ TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(ServerConfigLogRecord)) TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(ServerConfigLogRecordCompress)) { - auto logPath = common::joinPaths( - std::string(PROJECT_BINARY_PATH), "test_log_path"); + auto logPath = common::joinPaths(PROJECT_BINARY_PATH, "test_log_path"); auto logFile = common::joinPaths(logPath, "state.tlog"); auto compressedFile = logPath + ".zip"; @@ -480,8 +478,8 @@ TEST_P(ServerFixture, SdfStringServerConfig) { ignition::gazebo::ServerConfig serverConfig; - serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/shapes.sdf"); + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "shapes.sdf")); EXPECT_FALSE(serverConfig.SdfFile().empty()); EXPECT_TRUE(serverConfig.SdfString().empty()); @@ -796,8 +794,8 @@ TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(AddSystemWhileRunning)) { ignition::gazebo::ServerConfig serverConfig; - serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/shapes.sdf"); + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "shapes.sdf")); gazebo::Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -844,8 +842,8 @@ TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(AddSystemAfterLoad)) { ignition::gazebo::ServerConfig serverConfig; - serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/shapes.sdf"); + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "shapes.sdf")); gazebo::Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -917,8 +915,9 @@ TEST_P(ServerFixture, Seed) TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(ResourcePath)) { ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", - (std::string(PROJECT_SOURCE_PATH) + "/test/worlds:" + - std::string(PROJECT_SOURCE_PATH) + "/test/worlds/models").c_str()); + (common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds:") + + common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "models")).c_str()); ServerConfig serverConfig; serverConfig.SetSdfFile("resource_paths.sdf"); @@ -1172,6 +1171,32 @@ TEST_P(ServerFixture, ResolveResourcePaths) "include_nested", "model.sdf"), true); } +///////////////////////////////////////////////// +TEST_P(ServerFixture, Stop) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetUpdateRate(10000); + serverConfig.SetSdfFile(common::joinPaths((PROJECT_SOURCE_PATH), + "test", "worlds", "shapes.sdf")); + + gazebo::Server server(serverConfig); + + // The simulation runner should not be running. + EXPECT_FALSE(*server.Running(0)); + EXPECT_FALSE(server.Running()); + + // Run the server. + EXPECT_TRUE(server.Run(false, 0, false)); + EXPECT_TRUE(*server.Running(0)); + EXPECT_TRUE(server.Running()); + + // Stop the server + server.Stop(); + EXPECT_FALSE(*server.Running(0)); + EXPECT_FALSE(server.Running()); +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(ServerRepeat, ServerFixture, ::testing::Range(1, 2)); From 1f6c1263fea66cd1736f420741acbe030a3f3089 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 24 Jun 2022 16:15:52 -0700 Subject: [PATCH 06/10] =?UTF-8?q?=F0=9F=8E=88=206.10.0=20(#1552)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- Changelog.md | 131 +++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 132 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3e1dd1e979..53942b80db 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo6 VERSION 6.9.0) +project(ignition-gazebo6 VERSION 6.10.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 79ee4ec6a0..325bb7338d 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,136 @@ ## Ignition Gazebo 6.x +### Gazebo Sim 6.10.0 (2022-06-24) + +1. Expose the ability to stop a server from C++ + * [Pull request #1551](https://github.com/gazebosim/gz-sim/pull/1551) + +1. Fix various Protobuf Windows warnings + * [Pull request #1299](https://github.com/gazebosim/gz-sim/pull/1299) + +1. New service for adding systems to an entity + * [Pull request #1524](https://github.com/gazebosim/gz-sim/pull/1524) + +1. Added particle emitters to scene broadcaster + * [Pull request #1516](https://github.com/gazebosim/gz-sim/pull/1516) + +1. Use more `sdf::Plugin` instead of `sdf::ElementPtr` + * [Pull request #1352](https://github.com/gazebosim/gz-sim/pull/1352) + +1. Depend on common 4.5.1 + * [Pull request #1547](https://github.com/gazebosim/gz-sim/pull/1547) + +1. Update README links + * [Pull request #1546](https://github.com/gazebosim/gz-sim/pull/1546) + +1. Add bounding boxes into the label system plugin + * [Pull request #1040](https://github.com/gazebosim/gz-sim/pull/1040) + +1. Odometry publisher: also publish `Pose_V` (TF) + * [Pull request #1534](https://github.com/gazebosim/gz-sim/pull/1534) + +1. Fix clang warning from Thruster plugin + * [Pull request #1540](https://github.com/gazebosim/gz-sim/pull/1540) + +1. Fix locks in Visualize Lidar GUI plugin + * [Pull request #1538](https://github.com/gazebosim/gz-sim/pull/1538) + +1. Bash completion for flags + * [Pull request #1504](https://github.com/gazebosim/gz-sim/pull/1504) + +1. Fix sensors battery state test + * [Pull request #1529](https://github.com/gazebosim/gz-sim/pull/1529) + +1. Add new `GZ_GUI_RESOURCE_PATH` to help message + * [Pull request #1470](https://github.com/gazebosim/gz-sim/pull/1470) + +1. Fix regression with camera sensors not using the background color set in `` + * [Pull request #1515](https://github.com/gazebosim/gz-sim/pull/1515) + +1. Check RGBD camera sensor connection + * [Pull request #1513](https://github.com/gazebosim/gz-sim/pull/1513) + +1. Optimize sensor updates + * [Pull request #1480](https://github.com/gazebosim/gz-sim/pull/1480) + +1. System inspector GUI widget + * [Pull request #1404](https://github.com/gazebosim/gz-sim/pull/1404) + +1. Scene update resource finder + * [Pull request #1508](https://github.com/gazebosim/gz-sim/pull/1508) + +1. Updating hydrodynamics plugin description + * [Pull request #1502](https://github.com/gazebosim/gz-sim/pull/1502) + +1. Makes thruster stop when battery runs out. + * [Pull request #1495](https://github.com/gazebosim/gz-sim/pull/1495) + +1. Fix Documentation Header. + * [Pull request #1501](https://github.com/gazebosim/gz-sim/pull/1501) + +1. Adding rssi + * [Pull request #1482](https://github.com/gazebosim/gz-sim/pull/1482) + +1. Delete unused gazebo.hh.in + * [Pull request #1490](https://github.com/gazebosim/gz-sim/pull/1490) + +1. :books: Fixed broken URL link to gazebo documentation + * [Pull request #1486](https://github.com/gazebosim/gz-sim/pull/1486) + +1. View polyline collisions on the GUI + * [Pull request #1481](https://github.com/gazebosim/gz-sim/pull/1481) + +1. Extruded 2D polyline geometries + * [Pull request #1456](https://github.com/gazebosim/gz-sim/pull/1456) + +1. Fix fuel url + * [Pull request #1479](https://github.com/gazebosim/gz-sim/pull/1479) + +1. Camera trigger integration test + * [Pull request #1384](https://github.com/gazebosim/gz-sim/pull/1384) + +1. Extend Multicoptor Control system to include nested model inertial params + * [Pull request #1450](https://github.com/gazebosim/gz-sim/pull/1450) + +1. Remove dead ign.cc file + * [Pull request #1474](https://github.com/gazebosim/gz-sim/pull/1474) + +1. Test case to check if velocity limits are applied to joints + * [Pull request #1445](https://github.com/gazebosim/gz-sim/pull/1445) + +1. SceneBroadcaster: Use double for state publish frequency instead of int + * [Pull request #1417](https://github.com/gazebosim/gz-sim/pull/1417) + +1. Revert format change + * [Pull request #1468](https://github.com/gazebosim/gz-sim/pull/1468) + +1. Fix finding DART on macOS + * [Pull request #1469](https://github.com/gazebosim/gz-sim/pull/1469) + +1. Skip serializing nested model with `//pose/@relative_to` attribute + * [Pull request #1454](https://github.com/gazebosim/gz-sim/pull/1454) + +1. Fix running simulation with no world specified on the command line + * [Pull request #1463](https://github.com/gazebosim/gz-sim/pull/1463) + +1. Add repo specific issue templates + * [Pull request #1461](https://github.com/gazebosim/gz-sim/pull/1461) + +1. python: release GIL when running server + * [Pull request #1458](https://github.com/gazebosim/gz-sim/pull/1458) + +1. python: remove semicolons + * [Pull request #1459](https://github.com/gazebosim/gz-sim/pull/1459) + +1. Bump rendering dependency version + * [Pull request #1455](https://github.com/gazebosim/gz-sim/pull/1455) + +1. Improve contact sensor / visualization performance + * [Pull request #1452](https://github.com/gazebosim/gz-sim/pull/1452) + +1. Set simulation time to Rendering + * [Pull request #1415](https://github.com/gazebosim/gz-sim/pull/1415) + ### Ignition Gazebo 6.9.0 (2022-04-14) 1. Add new `RFComms` system From e826c71b2a196511100a8a985db33e55b588c8ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20Berk=20T=C3=B6re?= Date: Fri, 1 Jul 2022 23:21:13 +0300 Subject: [PATCH 07/10] Utilizes function to sequence trajectories (#1565) Signed-off-by: Onur Berk Tore --- .../ignition/gazebo/rendering/SceneManager.hh | 7 ++++++ src/rendering/SceneManager.cc | 23 +++++++++++++------ 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index 1ea41fb348..60aefb24d3 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -200,6 +200,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return Pointer to requested visual public: rendering::VisualPtr VisualById(Entity _id); + /// \brief Sequences Trajectories + /// \param[in] _trajectories Actor trajectories + /// \param[in] _time Actor trajectory delay start time (miliseconds) + public: void SequenceTrajectories( + std::vector& _trajectories, + std::chrono::steady_clock::time_point _time); + /// \brief Create an actor /// \param[in] _id Unique actor id /// \param[in] _actor Actor sdf dom diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 3caf84c4ed..7924bc61f2 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -910,6 +910,20 @@ rendering::MaterialPtr SceneManager::LoadMaterial( return material; } +void SceneManager::SequenceTrajectories( + std::vector& _trajectories, + TP _time) +{ + // sequencing all trajectories + for (auto &trajectory : _trajectories) + { + auto duration = trajectory.Duration(); + trajectory.SetStartTime(_time); + _time += duration; + trajectory.SetEndTime(_time); + } +} + ///////////////////////////////////////////////// rendering::VisualPtr SceneManager::CreateActor(Entity _id, const sdf::Actor &_actor, const std::string &_name, Entity _parentId) @@ -1164,13 +1178,8 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, auto delayStartTime = std::chrono::milliseconds( static_cast(_actor.ScriptDelayStart() * 1000)); TP time(delayStartTime); - for (auto &trajectory : trajectories) - { - auto dura = trajectory.Duration(); - trajectory.SetStartTime(time); - time += dura; - trajectory.SetEndTime(time); - } + this->SequenceTrajectories(trajectories, time); + // loop flag: add a placeholder trajectory to indicate no loop if (!_actor.ScriptLoop()) From d6e69d0c4ad764e6a69e952d5a129e48c19ea159 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20Berk=20T=C3=B6re?= Date: Mon, 4 Jul 2022 19:07:40 +0300 Subject: [PATCH 08/10] Refactor: Utilizes function to load animations (#1568) Signed-off-by: Onur Berk Tore --- .../ignition/gazebo/rendering/SceneManager.hh | 7 + src/rendering/SceneManager.cc | 228 ++++++++++-------- 2 files changed, 130 insertions(+), 105 deletions(-) diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index 60aefb24d3..d99a904a51 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -200,6 +201,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return Pointer to requested visual public: rendering::VisualPtr VisualById(Entity _id); + /// \brief Load Actor animations + /// \param[in] _actor Actor + /// \return Animation name to ID map + public: std::unordered_map + LoadAnimations(const sdf::Actor &_actor); + /// \brief Sequences Trajectories /// \param[in] _trajectories Actor trajectories /// \param[in] _time Actor trajectory delay start time (miliseconds) diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 7924bc61f2..3b53f5b9b0 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -18,7 +18,6 @@ #include #include -#include #include #include @@ -982,112 +981,11 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, return rendering::VisualPtr(); } - unsigned int numAnims = 0; - std::unordered_map mapAnimNameId; - mapAnimNameId[descriptor.meshName] = numAnims++; - // Load all animations - for (unsigned i = 0; i < _actor.AnimationCount(); ++i) - { - std::string animName = _actor.AnimationByIndex(i)->Name(); - std::string animFilename = asFullPath( - _actor.AnimationByIndex(i)->Filename(), - _actor.AnimationByIndex(i)->FilePath()); - - double animScale = _actor.AnimationByIndex(i)->Scale(); - - std::string extension = animFilename.substr(animFilename.rfind('.') + 1, - animFilename.size()); - std::transform(extension.begin(), extension.end(), - extension.begin(), ::tolower); - - if (extension == "bvh") - { - // do not add duplicate animation - // start checking from index 1 since index 0 is reserved by skin mesh - bool addAnim = true; - for (unsigned int a = 1; a < meshSkel->AnimationCount(); ++a) - { - if (meshSkel->Animation(a)->Name() == animFilename) - { - addAnim = false; - break; - } - } - if (addAnim) - meshSkel->AddBvhAnimation(animFilename, animScale); - mapAnimNameId[animName] = numAnims++; - } - else if (extension == "dae") - { - // Load the mesh if it has not been loaded before - const common::Mesh *animMesh = nullptr; - if (!meshManager->HasMesh(animFilename)) - { - animMesh = meshManager->Load(animFilename); - if (animMesh->MeshSkeleton()->AnimationCount() > 1) - { - ignwarn << "File [" << animFilename - << "] has more than one animation, but only the 1st one is used." - << std::endl; - } - } - animMesh = meshManager->MeshByName(animFilename); - - // add the first animation - auto firstAnim = animMesh->MeshSkeleton()->Animation(0); - if (nullptr == firstAnim) - { - ignerr << "Failed to get animations from [" << animFilename - << "]" << std::endl; - return nullptr; - } - // do not add duplicate animation - // start checking from index 1 since index 0 is reserved by skin mesh - bool addAnim = true; - for (unsigned int a = 1; a < meshSkel->AnimationCount(); ++a) - { - if (meshSkel->Animation(a)->Name() == animName) - { - addAnim = false; - break; - } - } - if (addAnim) - { - // collada loader loads animations with name that defaults to - // "animation0", "animation"1", etc causing conflicts in names - // when multiple animations are added to meshSkel. - // We have to clone the skeleton animation before giving it a unique - // name otherwise if mulitple instances of the same animation were added - // to meshSkel, changing the name that would also change the name of - // other instances of the animation - // todo(anyone) cloning is inefficient and error-prone. We should - // add a copy constructor to animation classes in ign-common. - // The proper fix is probably to update ign-rendering to allow it to - // load multiple animations of the same name - common::SkeletonAnimation *skelAnim = - new common::SkeletonAnimation(animName); - for (unsigned int j = 0; j < meshSkel->NodeCount(); ++j) - { - common::SkeletonNode *node = meshSkel->NodeByHandle(j); - common::NodeAnimation *nodeAnim = firstAnim->NodeAnimationByName( - node->Name()); - if (!nodeAnim) - continue; - for (unsigned int k = 0; k < nodeAnim->FrameCount(); ++k) - { - std::pair keyFrame = nodeAnim->KeyFrame(k); - skelAnim->AddKeyFrame( - node->Name(), keyFrame.first, keyFrame.second); - } - } + auto mapAnimNameId = this->LoadAnimations(_actor); + if (mapAnimNameId.size() == 0) + return nullptr; - meshSkel->AddAnimation(skelAnim); - } - mapAnimNameId[animName] = numAnims++; - } - } this->dataPtr->actorSkeletons[_id] = meshSkel; std::vector trajectories; @@ -2336,3 +2234,123 @@ AnimationUpdateData SceneManagerPrivate::ActorTrajectoryAt( animData.valid = true; return animData; } + +std::unordered_map +SceneManager::LoadAnimations(const sdf::Actor &_actor) +{ + rendering::MeshDescriptor descriptor; + descriptor.meshName = asFullPath(_actor.SkinFilename(), _actor.FilePath()); + common::MeshManager *meshManager = common::MeshManager::Instance(); + descriptor.mesh = meshManager->Load(descriptor.meshName); + common::SkeletonPtr meshSkel = descriptor.mesh->MeshSkeleton(); + + unsigned int numAnims = 0; + std::unordered_map mapAnimNameId; + mapAnimNameId[descriptor.meshName] = numAnims++; + + // Load all animations + for (unsigned i = 0; i < _actor.AnimationCount(); ++i) + { + std::string animName = _actor.AnimationByIndex(i)->Name(); + std::string animFilename = asFullPath( + _actor.AnimationByIndex(i)->Filename(), + _actor.AnimationByIndex(i)->FilePath()); + + double animScale = _actor.AnimationByIndex(i)->Scale(); + + std::string extension = animFilename.substr(animFilename.rfind('.') + 1, + animFilename.size()); + std::transform(extension.begin(), extension.end(), + extension.begin(), ::tolower); + + if (extension == "bvh") + { + // do not add duplicate animation + // start checking from index 1 since index 0 is reserved by skin mesh + bool addAnim = true; + for (unsigned int a = 1; a < meshSkel->AnimationCount(); ++a) + { + if (meshSkel->Animation(a)->Name() == animFilename) + { + addAnim = false; + break; + } + } + if (addAnim) + meshSkel->AddBvhAnimation(animFilename, animScale); + mapAnimNameId[animName] = numAnims++; + } + else if (extension == "dae") + { + // Load the mesh if it has not been loaded before + const common::Mesh *animMesh = nullptr; + if (!meshManager->HasMesh(animFilename)) + { + animMesh = meshManager->Load(animFilename); + if (animMesh->MeshSkeleton()->AnimationCount() > 1) + { + ignwarn << "File [" << animFilename + << "] has more than one animation, " + << "but only the 1st one is used." + << std::endl; + } + } + animMesh = meshManager->MeshByName(animFilename); + + // add the first animation + auto firstAnim = animMesh->MeshSkeleton()->Animation(0); + if (nullptr == firstAnim) + { + ignerr << "Failed to get animations from [" << animFilename + << "]" << std::endl; + mapAnimNameId.clear(); + break; + } + // do not add duplicate animation + // start checking from index 1 since index 0 is reserved by skin mesh + bool addAnim = true; + for (unsigned int a = 1; a < meshSkel->AnimationCount(); ++a) + { + if (meshSkel->Animation(a)->Name() == animName) + { + addAnim = false; + break; + } + } + if (addAnim) + { + // collada loader loads animations with name that defaults to + // "animation0", "animation"1", etc causing conflicts in names + // when multiple animations are added to meshSkel. + // We have to clone the skeleton animation before giving it a unique + // name otherwise if mulitple instances of the same animation were added + // to meshSkel, changing the name that would also change the name of + // other instances of the animation + // todo(anyone) cloning is inefficient and error-prone. We should + // add a copy constructor to animation classes in ign-common. + // The proper fix is probably to update ign-rendering to allow it to + // load multiple animations of the same name + common::SkeletonAnimation *skelAnim = + new common::SkeletonAnimation(animName); + for (unsigned int j = 0; j < meshSkel->NodeCount(); ++j) + { + common::SkeletonNode *node = meshSkel->NodeByHandle(j); + common::NodeAnimation *nodeAnim = firstAnim->NodeAnimationByName( + node->Name()); + if (!nodeAnim) + continue; + for (unsigned int k = 0; k < nodeAnim->FrameCount(); ++k) + { + std::pair keyFrame = nodeAnim->KeyFrame(k); + skelAnim->AddKeyFrame( + node->Name(), keyFrame.first, keyFrame.second); + } + } + + meshSkel->AddAnimation(skelAnim); + } + mapAnimNameId[animName] = numAnims++; + } + } + return mapAnimNameId; +} From 199237ddc8c436a5064472855a74a19c89b512d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 5 Jul 2022 19:25:04 +0200 Subject: [PATCH 09/10] Use pytest to generate junit xml files for python tests (#1562) Signed-off-by: ahcorde --- .github/ci/packages.apt | 1 + python/CMakeLists.txt | 20 ++++++++++++++++++-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index 0481d197ea..555e4d2446 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -23,6 +23,7 @@ libxi-dev libxmu-dev python3-distutils python3-pybind11 +python3-pytest qml-module-qt-labs-folderlistmodel qml-module-qt-labs-settings qml-module-qtqml-models2 diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index bd08e206db..13ee16b004 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -86,9 +86,25 @@ if (BUILD_TESTING) testFixture_TEST ) + execute_process(COMMAND "${PYTHON_EXECUTABLE}" -m pytest --version + OUTPUT_VARIABLE PYTEST_output + ERROR_VARIABLE PYTEST_error + RESULT_VARIABLE PYTEST_result) + if(${PYTEST_result} EQUAL 0) + set(pytest_FOUND TRUE) + else() + message("") + message(WARNING "Pytest package not available: ${PYTEST_error}") + endif() + foreach (test ${python_tests}) - add_test(NAME ${test}.py COMMAND - "${PYTHON_EXECUTABLE}" "${CMAKE_SOURCE_DIR}/python/test/${test}.py") + if (pytest_FOUND) + add_test(NAME ${test}.py COMMAND + "${PYTHON_EXECUTABLE}" -m pytest "${CMAKE_SOURCE_DIR}/python/test/${test}.py" --junitxml "${CMAKE_BINARY_DIR}/test_results/UNIT_${test}.xml") + else() + add_test(NAME ${test}.py COMMAND + "${PYTHON_EXECUTABLE}" "${CMAKE_SOURCE_DIR}/python/test/${test}.py") + endif() set(_env_vars) list(APPEND _env_vars "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/python/") From f39bc25da4a93a0b2ccf5fca8c3e1b6863185a9d Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 6 Jul 2022 18:58:50 +0200 Subject: [PATCH 10/10] Fix UNIT_Server_TEST on Windows (#1577) Signed-off-by: Silvio --- src/Server_TEST.cc | 14 ++++++++++---- src/Util.cc | 14 +++++++------- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index f18e16c8a4..6023c036ea 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -1004,7 +1004,9 @@ TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(ResourcePath)) TEST_P(ServerFixture, GetResourcePaths) { ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", - "/tmp/some/path:/home/user/another_path"); + std::string("/tmp/some/path") + + common::SystemPaths::Delimiter() + + std::string("/home/user/another_path")); ServerConfig serverConfig; gazebo::Server server(serverConfig); @@ -1034,7 +1036,9 @@ TEST_P(ServerFixture, GetResourcePaths) TEST_P(ServerFixture, AddResourcePaths) { ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", - "/tmp/some/path:/home/user/another_path"); + std::string("/tmp/some/path") + + common::SystemPaths::Delimiter() + + std::string("/home/user/another_path")); ignition::common::setenv("SDF_PATH", ""); ignition::common::setenv("IGN_FILE_PATH", ""); @@ -1063,7 +1067,9 @@ TEST_P(ServerFixture, AddResourcePaths) // Add path msgs::StringMsg_V req; req.add_data("/tmp/new_path"); - req.add_data("/tmp/more:/tmp/even_more"); + req.add_data(std::string("/tmp/more") + + common::SystemPaths::Delimiter() + + std::string("/tmp/even_more")); req.add_data("/tmp/some/path"); bool executed = node.Request("/gazebo/resource_paths/add", req); EXPECT_TRUE(executed); @@ -1082,7 +1088,7 @@ TEST_P(ServerFixture, AddResourcePaths) { char *pathCStr = std::getenv(env); - auto paths = common::Split(pathCStr, ':'); + auto paths = common::Split(pathCStr, common::SystemPaths::Delimiter()); paths.erase(std::remove_if(paths.begin(), paths.end(), [](std::string const &_path) { diff --git a/src/Util.cc b/src/Util.cc index e24718bfac..ca545bf39f 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -408,7 +408,7 @@ std::vector resourcePaths() char *gzPathCStr = std::getenv(kResourcePathEnv.c_str()); if (gzPathCStr && *gzPathCStr != '\0') { - gzPaths = common::Split(gzPathCStr, ':'); + gzPaths = common::Split(gzPathCStr, common::SystemPaths::Delimiter()); } gzPaths.erase(std::remove_if(gzPaths.begin(), gzPaths.end(), @@ -428,7 +428,7 @@ void addResourcePaths(const std::vector &_paths) char *sdfPathCStr = std::getenv(kSdfPathEnv.c_str()); if (sdfPathCStr && *sdfPathCStr != '\0') { - sdfPaths = common::Split(sdfPathCStr, ':'); + sdfPaths = common::Split(sdfPathCStr, common::SystemPaths::Delimiter()); } // Ignition file paths (for s) @@ -437,7 +437,7 @@ void addResourcePaths(const std::vector &_paths) char *ignPathCStr = std::getenv(systemPaths->FilePathEnv().c_str()); if (ignPathCStr && *ignPathCStr != '\0') { - ignPaths = common::Split(ignPathCStr, ':'); + ignPaths = common::Split(ignPathCStr, common::SystemPaths::Delimiter()); } // Gazebo resource paths @@ -445,7 +445,7 @@ void addResourcePaths(const std::vector &_paths) char *gzPathCStr = std::getenv(kResourcePathEnv.c_str()); if (gzPathCStr && *gzPathCStr != '\0') { - gzPaths = common::Split(gzPathCStr, ':'); + gzPaths = common::Split(gzPathCStr, common::SystemPaths::Delimiter()); } // Add new paths to gzPaths @@ -474,20 +474,20 @@ void addResourcePaths(const std::vector &_paths) // Update the vars std::string sdfPathsStr; for (const auto &path : sdfPaths) - sdfPathsStr += ':' + path; + sdfPathsStr += common::SystemPaths::Delimiter() + path; ignition::common::setenv(kSdfPathEnv.c_str(), sdfPathsStr.c_str()); std::string ignPathsStr; for (const auto &path : ignPaths) - ignPathsStr += ':' + path; + ignPathsStr += common::SystemPaths::Delimiter() + path; ignition::common::setenv( systemPaths->FilePathEnv().c_str(), ignPathsStr.c_str()); std::string gzPathsStr; for (const auto &path : gzPaths) - gzPathsStr += ':' + path; + gzPathsStr += common::SystemPaths::Delimiter() + path; ignition::common::setenv(kResourcePathEnv.c_str(), gzPathsStr.c_str());