gazebosim · arjo129 · Aug 3, 2023 · May 31, 2023 · May 31, 2023 · May 31, 2023
@@ -601,6 +601,17 @@ namespace ignition
       public: std::unordered_set<ComponentTypeId>
           ComponentTypesWithPeriodicChanges() const;
 
+      /// \brief Get a cache of components with periodic changes.
+      /// \param[inout] _changes A list of components with the latest periodic
+      /// changes. If a component has a periodic change, it is added to the
+      /// hash map. It the component or entity was removed, it is removed from
+      /// the hashmap. This way the hashmap stores a list of components and
+      /// entities which have had periodic changes in the past and still
+      /// exist within the ECM.
+      /// \sa EntityComponentManager::PeriodicStateFromCache
+      public: void UpdatePeriodicChangeCache(std::unordered_map<ComponentTypeId,
+        std::unordered_set<Entity>>&) 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.
@@ -628,6 +639,19 @@ namespace ignition
                   const std::unordered_set<ComponentTypeId> &_types = {},
                   bool _full = false) const;
 
+      /// \brief Populate a message with relevant changes to the state given
+      /// a periodic change cache.
+      /// \details The header of the message will not be populated, it is the
+      /// responsibility of the caller to timestamp it before use. Additionally,
+      /// changes such as addition or removal will not be populated.
+      /// \param[inout] _state The serialized state message to populate.
+      /// \param[in] _cache A map of entities and components to serialize.
+      /// \sa EntityComponenetManager::UpdatePeriodicChangeCache
+      public: void PeriodicStateFromCache(
+                  msgs::SerializedStateMap &_state,
+                  const std::unordered_map<ComponentTypeId,
+                        std::unordered_set<Entity>> &_cache) const;
+
       /// \brief Get a message with the serialized state of all entities and
       /// components that are changing in the current iteration
       ///

@@ -963,6 +963,42 @@ std::unordered_set<ComponentTypeId>
   return periodicComponents;
 }
 
+/////////////////////////////////////////////////
+void EntityComponentManager::UpdatePeriodicChangeCache(
+  std::unordered_map<ComponentTypeId,
+  std::unordered_set<Entity>> &_changes) const
+{
+  // Get all changes
+  for (const auto &[componentType, entities] :
+    this->dataPtr->periodicChangedComponents)
+  {
+    _changes[componentType].insert(
+      entities.begin(), entities.end());
+  }
+
+  // Get all removed components
+  for (const auto &[entity, components] :
+    this->dataPtr->componentsMarkedAsRemoved)
+  {
+    for (const auto &comp : components)
+    {
+      _changes[comp].erase(entity);
+    }
+  }
+
+  // Get all removed entities
+  for (const auto &entity : this->dataPtr->toRemoveEntities) {
+    for (
+      auto components = _changes.begin();
+      components != _changes.end(); components++) {
+      // Its ok to leave component entries empty, the serialization
+      // code will simply ignore it. In any case the number of components
+      // is limited, so this cache will never grow too large.
+      components->second.erase(entity);
+    }
+  }
+}
+
 /////////////////////////////////////////////////
 bool EntityComponentManager::HasEntity(const Entity _entity) const
 {
@@ -1678,6 +1714,48 @@ void EntityComponentManager::State(
   });
 }
 
+//////////////////////////////////////////////////
+void EntityComponentManager::PeriodicStateFromCache(
+  msgs::SerializedStateMap &_state,
+  const std::unordered_map<ComponentTypeId,
+    std::unordered_set<Entity>> &_cache) const
+{
+  for (auto &[typeId, entities] : _cache) {
+    // Serialize components that have changed
+    for (auto &entity : entities) {
+      // Add entity to message if it does not exist
+      auto entIter = _state.mutable_entities()->find(entity);
+      if (entIter == _state.mutable_entities()->end())
+      {
+        msgs::SerializedEntityMap ent;
+        ent.set_id(entity);
+        (*_state.mutable_entities())[static_cast<uint64_t>(entity)] = ent;
+        entIter = _state.mutable_entities()->find(entity);
+      }
+
+      // 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;
+      }
+
+      auto compIdx = this->dataPtr->componentTypeIndex[entity][typeId];
+      auto &comp = this->dataPtr->componentStorage[entity][compIdx];
+
+      // 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;
+    }
+  }
+}
+
 //////////////////////////////////////////////////
 void EntityComponentManager::SetState(
     const msgs::SerializedState &_stateMsg)

@@ -2204,6 +2204,72 @@ TEST_P(EntityComponentManagerFixture, Descendants)
   }
 }
 
+//////////////////////////////////////////////////
+TEST_P(EntityComponentManagerFixture,
+       IGN_UTILS_TEST_DISABLED_ON_WIN32(UpdatePeriodicChangeCache))
+{
+  Entity e1 = manager.CreateEntity();
+  auto c1 = manager.CreateComponent<IntComponent>(e1, IntComponent(123));
+
+  std::unordered_map<ComponentTypeId,
+                        std::unordered_set<Entity>> changeTracker;
+
+  // No periodic changes keep cache empty.
+  manager.UpdatePeriodicChangeCache(changeTracker);
+  EXPECT_EQ(changeTracker.size(), 0u);
+
+  // Create a periodic change.
+  manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange);
+
+  // 1 periodic change, should be reflected in cache.
+  manager.UpdatePeriodicChangeCache(changeTracker);
+  EXPECT_EQ(changeTracker.size(), 1u);
+  EXPECT_EQ(changeTracker[c1->TypeId()].count(e1), 1u);
+
+  manager.RunSetAllComponentsUnchanged();
+
+  // Has periodic change. Cache should be full.
+  manager.UpdatePeriodicChangeCache(changeTracker);
+  EXPECT_EQ(changeTracker.size(), 1u);
+  EXPECT_EQ(changeTracker[c1->TypeId()].count(e1), 1u);
+  EXPECT_EQ(changeTracker[c1->TypeId()].size(), 1u);
+
+  // Serialize state
+  msgs::SerializedStateMap state;
+  manager.PeriodicStateFromCache(state, changeTracker);
+  EXPECT_EQ(state.entities().size(), 1u);
+  EXPECT_EQ(
+    state.entities().find(e1)->second.components().size(), 1u);
+  EXPECT_NE(state.entities().find(e1)->second
+      .components().find(c1->TypeId()),
+    state.entities().find(e1)->second.components().end());
+
+  // Component removed cache should be updated.
+  manager.RemoveComponent<IntComponent>(e1);
+  manager.UpdatePeriodicChangeCache(changeTracker);
+  EXPECT_EQ(changeTracker.size(), 1u);
+  EXPECT_EQ(changeTracker[c1->TypeId()].size(), 0u);
+
+  manager.RunSetAllComponentsUnchanged();
+
+  // Add another component to the entity
+  auto c2 = manager.CreateComponent<IntComponent>(e1, IntComponent(123));
+  manager.UpdatePeriodicChangeCache(changeTracker);
+
+  // Cache does not track additions, only PeriodicChanges
+  EXPECT_EQ(changeTracker[c2->TypeId()].size(), 0u);
+
+  // Track change
+  manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange);
+  manager.UpdatePeriodicChangeCache(changeTracker);
+  EXPECT_EQ(changeTracker[c2->TypeId()].size(), 1u);
+
+  // Entity removed cache should be updated.
+  manager.RequestRemoveEntity(e1);
+  manager.UpdatePeriodicChangeCache(changeTracker);
+  EXPECT_EQ(changeTracker[c2->TypeId()].size(), 0u);
+}
+
 //////////////////////////////////////////////////
 TEST_P(EntityComponentManagerFixture,
        IGN_UTILS_TEST_DISABLED_ON_WIN32(SetChanged))

@@ -57,6 +57,9 @@ class ignition::gazebo::systems::JointPositionControllerPrivate
   /// \brief mutex to protect joint commands
   public: std::mutex jointCmdMutex;
 
+  /// \brief Is the maximum PID gain set.
+  public: bool isMaxSet {false};
+
   /// \brief Model interface
   public: Model model{kNullEntity};
 
@@ -149,6 +152,7 @@ void JointPositionController::Configure(const Entity &_entity,
   if (_sdf->HasElement("cmd_max"))
   {
     cmdMax = _sdf->Get<double>("cmd_max");
+    this->dataPtr->isMaxSet = true;
   }
   if (_sdf->HasElement("cmd_min"))
   {
@@ -306,14 +310,14 @@ void JointPositionController::PreUpdate(
     auto maxMovement = this->dataPtr->posPid.CmdMax() * dt;
 
     // Limit the maximum change to maxMovement
-    if (abs(error) > maxMovement)
+    if (abs(error) > maxMovement && this->dataPtr->isMaxSet)
     {
       targetVel = (error < 0) ? this->dataPtr->posPid.CmdMax() :
         -this->dataPtr->posPid.CmdMax();
     }
     else
     {
-      targetVel = -error;
+      targetVel = - error / dt;
     }
 
     // Set velocity and return

@@ -24,6 +24,7 @@
 #include <condition_variable>
 #include <map>
 #include <string>
+#include <unordered_map>
 #include <unordered_set>
 
 #include <gz/common/Profiler.hh>
@@ -257,8 +258,15 @@ 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 updated
+  ///  frequently enough)
+  public: std::unordered_map<ComponentTypeId,
+    std::unordered_set<Entity>> changedComponents;
 };
 
+
 //////////////////////////////////////////////////
 SceneBroadcaster::SceneBroadcaster()
   : System(), dataPtr(std::make_unique<SceneBroadcasterPrivate>())
@@ -341,6 +349,9 @@ 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.UpdatePeriodicChangeCache(this->dataPtr->changedComponents);
+
   // Publish state only if there are subscribers and
   // * throttle rate to 60 Hz
   // * also publish off-rate if there are change events:
@@ -383,15 +394,7 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
     else if (!_info.paused)
     {
       IGN_PROFILE("SceneBroadcast::PostUpdate UpdateState");
-
-      if (_manager.HasPeriodicComponentChanges())
-      {
-        auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges();
-        _manager.State(*this->dataPtr->stepMsg.mutable_state(),
-             {}, periodicComponents);
-        this->dataPtr->pubPeriodicChanges = false;
-      }
-      else
+      if (!_manager.HasPeriodicComponentChanges())
       {
         // log files may be recorded at lower rate than sim time step. So in
         // playback mode, the scene broadcaster may not see any periodic
@@ -416,6 +419,12 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
         // we may be able to remove this in the future and update tests
         this->dataPtr->stepMsg.mutable_state();
       }
+
+      // Apply changes that were caught by the periodic state tracker and then
+      // clear the change tracker.
+      _manager.PeriodicStateFromCache(*this->dataPtr->stepMsg.mutable_state(),
+        this->dataPtr->changedComponents);
+      this->dataPtr->changedComponents.clear();
     }
 
     // Full state on demand