Tracks periodic changes in scene broadcaster

This commit proposes a change to the scene broadcaster which enables
tracking of all components with periodic changes. This way if a
component has a periodic change the scene broadcaster does not miss it.

For more info see this discussion #2001 (comment) where @azeey proposes this solution.

This commit is WIP and I still need to handle entity/component removal
and add tests.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

include/ignition/gazebo/EntityComponentManager.hh

@@ -601,6 +601,13 @@ namespace ignition
       public: std::unordered_set<ComponentTypeId>
           ComponentTypesWithPeriodicChanges() const;
 
+      /// \brief Get all components with periodic changes.
+      /// \param[in] _f Callback function to be called for each matching entity
+      /// and component that has experienced a change.
+      public: void EachPeriodicChange(const std::function<
+                  void(const Entity &_entity,
+                       components::BaseComponent *_component)> _f) const; 
+
       /// \brief Set the absolute state of the ECM from a serialized message.
       /// Entities / components that are in the new state but not in the old
       /// one will be created.

src/EntityComponentManager.cc

@@ -963,6 +963,27 @@ std::unordered_set<ComponentTypeId>
   return periodicComponents;
 }
 
+/////////////////////////////////////////////////
+void EntityComponentManager::EachPeriodicChange(const std::function<
+                  void(const Entity &_entity,
+                       components::BaseComponent *_type)> _f) const
+{
+  for (const auto &[componentType, entities] : this->dataPtr->periodicChangedComponents)
+  {
+
+    for (const auto entity: entities) {
+
+      const components::BaseComponent* implementation =  
+        this->ComponentImplementation(entity, componentType);
+
+      if(!implementation)
+        continue;
+
+      _f(entity, const_cast<components::BaseComponent*>(implementation));
+    }
+  }
+}
+
 /////////////////////////////////////////////////
 bool EntityComponentManager::HasEntity(const Entity _entity) const
 {

src/systems/scene_broadcaster/SceneBroadcaster.cc

@@ -257,8 +257,54 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate
   /// \brief Flag used to indicate if periodic changes need to be published
   /// This is currently only used in playback mode.
   public: bool pubPeriodicChanges{false};
+
+  /// \brief Stores a cache of components that are changed. (This prevents dropping of
+  /// periodic change components which may not be frequent enough)
+  public: std::unordered_map<Entity,
+    std::unordered_map<ComponentTypeId,
+    std::unique_ptr<components::BaseComponent>>> changedComponents;
+
+  void ApplyPeriodicCacheChanges(msgs::SerializedStateMap &_stateMsg) {
+    for (auto &[entity, components] : changedComponents) {
+
+      // Add entity to message if it does not exist
+      auto entIter = _stateMsg.mutable_entities()->find(entity);
+      if (entIter == _stateMsg.mutable_entities()->end())
+      {
+        msgs::SerializedEntityMap ent;
+        ent.set_id(entity);
+        (*_stateMsg.mutable_entities())[static_cast<uint64_t>(entity)] = ent;
+        entIter = _stateMsg.mutable_entities()->find(entity);
+      }
+
+      // Serialize components that have changed
+      for (auto &[typeId, comp]: components) {
+
+        // Find the component in the message
+        auto compIter = entIter->second.mutable_components()->find(typeId);
+
+        if (compIter != entIter->second.mutable_components()->end())
+        {
+          // If the component is present we don't need to update it.
+          continue;
+        }
+
+        // Add the component to the message
+        msgs::SerializedComponent cmp;
+        cmp.set_type(comp->TypeId());
+        std::ostringstream ostr;
+        comp->Serialize(ostr);
+        cmp.set_component(ostr.str());
+        (*(entIter->second.mutable_components()))[
+        static_cast<int64_t>(typeId)] = cmp;
+      }  
+    }
+    this->changedComponents.clear();
+  }
+
 };
 
+
 //////////////////////////////////////////////////
 SceneBroadcaster::SceneBroadcaster()
   : System(), dataPtr(std::make_unique<SceneBroadcasterPrivate>())
@@ -341,6 +387,16 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
   // removed entities are removed from the scene graph for the next update cycle
   this->dataPtr->SceneGraphRemoveEntities(_manager);
 
+  // Iterate through entities and their changes to cache them.
+  _manager.EachPeriodicChange([&](const Entity &_entity,
+                      components::BaseComponent *_component) {
+    //ignerr << "got change" <<"\n";
+    this->dataPtr->changedComponents[_entity].emplace(_component->TypeId(),
+      _component->Clone());
+  });
+  // TODO(arjo): Remove items in the periodic change cache that have been removed.
+
+
   // Publish state only if there are subscribers and
   // * throttle rate to 60 Hz
   // * also publish off-rate if there are change events:
@@ -386,6 +442,7 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
 
       if (_manager.HasPeriodicComponentChanges())
       {
+        // TODO(arjo): We probably don't need this with the new cache method?
         auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges();
         _manager.State(*this->dataPtr->stepMsg.mutable_state(),
              {}, periodicComponents);
@@ -418,6 +475,10 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
       }
     }
 
+    // Apply changes that were caught by the periodic state tracker
+    this->dataPtr->ApplyPeriodicCacheChanges(
+      *this->dataPtr->stepMsg.mutable_state());
+
     // Full state on demand
     if (this->dataPtr->stateServiceRequest)
     {