Implement system Reset interface for Sensors and SceneBroadcaster

src/SimulationRunner.cc

@@ -538,8 +538,7 @@ void SimulationRunner::UpdateSystems()
   if (this->resetInitiated)
   {
     GZ_PROFILE("Reset");
-    for (auto &system : this->systemMgr->SystemsReset())
-      system->Reset(this->currentInfo, this->entityCompMgr);
+    this->systemMgr->Reset(this->currentInfo, this->entityCompMgr);
     return;
   }
 

src/SystemInternal.hh

@@ -19,6 +19,7 @@
 
 #include <chrono>
 #include <memory>
+#include <string>
 #include <utility>
 #include <vector>
 
@@ -105,6 +106,14 @@ namespace gz
       /// system during the `Configure` call.
       public: Entity parentEntity = {kNullEntity};
 
+      /// \brief Cached filename of the plugin used when system was loaded.
+      /// Used for reloading a system at runtime.
+      public: std::string fname = "";
+
+      /// \brief Cached plugin name of the plugin used when system was loaded.
+      /// Used for reloading a system at runtime.
+      public: std::string name = "";
+
       /// \brief Cached sdf that was used to call `Configure` on the system
       /// Useful for if a system needs to be reconfigured at runtime
       public: std::shared_ptr<const sdf::Element> configureSdf = nullptr;
@@ -116,4 +125,3 @@ namespace gz
   }  // namespace sim
 }  // namespace gz
 #endif  // GZ_SIM_SYSTEMINTERNAL_HH_
-

src/SystemLoader.cc

@@ -136,7 +136,6 @@ class gz::sim::SystemLoaderPrivate
       return false;
     }
 
-    this->systemPluginsAdded.insert(_plugin);
     return true;
   }
 
@@ -151,7 +150,6 @@ class gz::sim::SystemLoaderPrivate
   public: std::unordered_set<std::string> systemPluginPaths;
 
   /// \brief System plugins that have instances loaded via the manager.
-  public: std::unordered_set<SystemPluginPtr> systemPluginsAdded;
 };
 
 //////////////////////////////////////////////////

src/SystemManager.cc

@@ -47,7 +47,11 @@ void SystemManager::LoadPlugin(const Entity _entity,
   // System correctly loaded from library
   if (system)
   {
-    this->AddSystem(system.value(), _entity, _sdf);
+    SystemInternal ss(system.value(), _entity);
+    ss.fname = _fname;
+    ss.name = _name;
+    ss.configureSdf = _sdf;
+    this->AddSystemImpl(ss, ss.configureSdf);
     gzdbg << "Loaded system [" << _name
            << "] for entity [" << _entity << "]" << std::endl;
   }
@@ -103,6 +107,82 @@ size_t SystemManager::ActivatePendingSystems()
   return count;
 }
 
+//////////////////////////////////////////////////
+/// \brief Structure to temporarily store plugin information for reset
+struct PluginInfo {
+  /// \brief Entity plugin is attached to
+  Entity entity;
+  /// \brief Filename of the plugin library
+  std::string fname;
+  /// \brief Name of the plugin
+  std::string name;
+  /// \brief SDF element (content of the plugin tag)
+  sdf::ElementPtr sdf;
+};
+
+//////////////////////////////////////////////////
+void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm)
+{
+  {
+    std::lock_guard<std::mutex> lock(this->pendingSystemsMutex);
+    this->pendingSystems.clear();
+  }
+
+  // Clear all iterable collections of systems
+  this->systemsConfigure.clear();
+  this->systemsReset.clear();
+  this->systemsPreupdate.clear();
+  this->systemsUpdate.clear();
+  this->systemsPostupdate.clear();
+
+  std::vector<PluginInfo> pluginsToBeLoaded;
+
+  for (auto &system : this->systems)
+  {
+    if (nullptr != system.reset)
+    {
+      // If implemented, call reset and add to pending systems.
+      system.reset->Reset(_info, _ecm);
+
+      {
+        std::lock_guard<std::mutex> lock(this->pendingSystemsMutex);
+        this->pendingSystems.push_back(system);
+      }
+    }
+    else
+    {
+      // Cannot reset systems that were created in memory rather than
+      // from a plugin, because there isn't access to the constructor.
+      if (nullptr != system.systemShared)
+      {
+        ignwarn << "In-memory without ISystemReset detected: ["
+          << system.name << "]\n"
+          << "Systems created without plugins that do not implement Reset"
+          << " will not be reloaded. Reset may not work correctly\n";
+        continue;
+      }
+      PluginInfo info = {
+        system.parentEntity, system.fname, system.name,
+        system.configureSdf->Clone()
+      };
+
+      pluginsToBeLoaded.push_back(info);
+    }
+  }
+
+  this->systems.clear();
+
+  // Load plugins which do not implement reset after clearing this->systems
+  // to ensure the previous instance is destroyed before the new one is created
+  // and configured.
+  for (const auto &pluginInfo : pluginsToBeLoaded)
+  {
+    this->LoadPlugin(pluginInfo.entity, pluginInfo.fname, pluginInfo.name,
+        pluginInfo.sdf);
+  }
+  this->ActivatePendingSystems();
+}
+
 //////////////////////////////////////////////////
 void SystemManager::AddSystem(const SystemPluginPtr &_system,
       Entity _entity,

src/SystemManager.hh

@@ -92,12 +92,26 @@ namespace gz
       /// \return The number of newly-active systems
       public: size_t ActivatePendingSystems();
 
-      /// \brief Get an vector of all active systems implementing "Configure"
-      /// \return Vector of systems's configure interfaces.
+      /// \brief Perform a reset on all systems
+      ///
+      /// If a system implements the ISystemReset interface, it will be called.
+      //
+      /// Otherwise, if a system does not have the ISystemReset interface
+      /// implemented, and was created via loading a plugin,
+      /// that plugin will be reloaded.
+      ///
+      /// Otherwise, if a system is created from in-memory rather than a plugin,
+      /// that system will remain unaffected.
+      /// \param[in] _info Update info corresponding to the update time
+      /// \param[in] _ecm Version of the ECM reset to an initial state
+      public: void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm);
+
+      /// \brief Get a vector of all systems implementing "Configure"
+      /// \return Vector of systems' configure interfaces.
       public: const std::vector<ISystemConfigure *>& SystemsConfigure();
 
       /// \brief Get an vector of all active systems implementing "Reset"
-      /// \return Vector of systems's reset interfaces.
+      /// \return Vector of systems' reset interfaces.
       public: const std::vector<ISystemReset *>& SystemsReset();
 
       /// \brief Get an vector of all active systems implementing "PreUpdate"

src/rendering/RenderUtil.cc

@@ -1661,7 +1661,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate(
         return true;
       });
 
-
   _ecm.Each<components::Model, components::Name, components::Pose,
             components::ParentEntity>(
       [&](const Entity &_entity,

src/systems/log/LogPlayback.cc

@@ -453,6 +453,16 @@ bool LogPlaybackPrivate::ExtractStateAndResources()
   }
 }
 
+//////////////////////////////////////////////////
+void LogPlayback::Reset(const UpdateInfo &, EntityComponentManager &)
+{
+  // In this case, Reset is a noop
+  // LogPlayback already has handling for jumps in time as part of the
+  // Update method.
+  // Leaving this function implemented but empty prevents the SystemManager
+  // from trying to destroy and recreate the plugin.
+}
+
 //////////////////////////////////////////////////
 void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm)
 {
@@ -621,6 +631,7 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm)
 GZ_ADD_PLUGIN(gz::sim::systems::LogPlayback,
                     gz::sim::System,
                     LogPlayback::ISystemConfigure,
+                    LogPlayback::ISystemReset,
                     LogPlayback::ISystemUpdate)
 
 GZ_ADD_PLUGIN_ALIAS(LogPlayback,

src/systems/log/LogPlayback.hh

@@ -39,6 +39,7 @@ namespace systems
   class LogPlayback:
     public System,
     public ISystemConfigure,
+    public ISystemReset,
     public ISystemUpdate
   {
     /// \brief Constructor
@@ -53,6 +54,10 @@ namespace systems
                            EntityComponentManager &_ecm,
                            EventManager &_eventMgr) final;
 
+    /// Documentation inherited
+    public: void Reset(const UpdateInfo &_info,
+                       EntityComponentManager &_ecm) final;
+
     /// Documentation inherited
     public: void Update(const UpdateInfo &_info,
                         EntityComponentManager &_ecm) final;

src/systems/scene_broadcaster/SceneBroadcaster.cc

@@ -399,6 +399,28 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
   }
 }
 
+//////////////////////////////////////////////////
+void SceneBroadcaster::Reset(const UpdateInfo &_info,
+                             EntityComponentManager &_manager)
+{
+  // Update scene graph with added entities before populating pose message
+  this->dataPtr->SceneGraphAddEntities(_manager);
+
+  this->dataPtr->PoseUpdate(_info, _manager);
+
+  // call SceneGraphRemoveEntities at the end of this update cycle so that
+  // removed entities are removed from the scene graph for the next update cycle
+  this->dataPtr->SceneGraphRemoveEntities(_manager);
+
+  std::unique_lock<std::mutex> lock(this->dataPtr->stateMutex);
+  this->dataPtr->stepMsg.Clear();
+
+  set(this->dataPtr->stepMsg.mutable_stats(), _info);
+  _manager.State(*this->dataPtr->stepMsg.mutable_state(), {}, {}, true);
+  this->dataPtr->statePub.Publish(this->dataPtr->stepMsg);
+  this->dataPtr->lastStatePubTime = std::chrono::system_clock::now();
+}
+
 //////////////////////////////////////////////////
 void SceneBroadcasterPrivate::PoseUpdate(const UpdateInfo &_info,
     const EntityComponentManager &_manager)
@@ -1201,7 +1223,8 @@ void SceneBroadcasterPrivate::RemoveFromGraph(const Entity _entity,
 GZ_ADD_PLUGIN(SceneBroadcaster,
                     gz::sim::System,
                     SceneBroadcaster::ISystemConfigure,
-                    SceneBroadcaster::ISystemPostUpdate)
+                    SceneBroadcaster::ISystemPostUpdate,
+                    SceneBroadcaster::ISystemReset)
 
 // Add plugin alias so that we can refer to the plugin without the version
 // namespace

src/systems/scene_broadcaster/SceneBroadcaster.hh

@@ -40,7 +40,8 @@ namespace systems
   class SceneBroadcaster final:
     public System,
     public ISystemConfigure,
-    public ISystemPostUpdate
+    public ISystemPostUpdate,
+    public ISystemReset
   {
     /// \brief Constructor
     public: SceneBroadcaster();
@@ -58,6 +59,10 @@ namespace systems
     public: void PostUpdate(const UpdateInfo &_info,
                 const EntityComponentManager &_ecm) final;
 
+    // Documentation inherited
+    public: void Reset(const UpdateInfo &_info,
+                       EntityComponentManager &_ecm) final;
+
     /// \brief Private data pointer
     private: std::unique_ptr<SceneBroadcasterPrivate> dataPtr;
   };