ECM's ChangedState gets message with modified components

include/ignition/gazebo/EntityComponentManager.hh

@@ -474,11 +474,9 @@ namespace ignition
       /// \brief Get a message with the serialized state of all entities and
       /// components that are changing in the current iteration
       ///
-      /// Currently supported:
+      /// This includes:
       /// * New entities and all of their components
       /// * Removed entities and all of their components
-      ///
-      /// Future work:
       /// * Entities which had a component added
       /// * Entities which had a component removed
       /// * Entities which had a component modified
@@ -535,11 +533,9 @@ namespace ignition
       /// \brief Get a message with the serialized state of all entities and
       /// components that are changing in the current iteration
       ///
-      /// Currently supported:
+      /// This includes:
       /// * New entities and all of their components
       /// * Removed entities and all of their components
-      ///
-      /// Future work:
       /// * Entities which had a component added
       /// * Entities which had a component removed
       /// * Entities which had a component modified

src/EntityComponentManager.cc

@@ -72,6 +72,12 @@ class ignition::gazebo::EntityComponentManagerPrivate
       msgs::SerializedStateMap &_msg,
       const std::unordered_set<ComponentTypeId> &_types = {});
 
+  /// \brief Add newly modified (created/modified/removed) components to
+  /// modifiedComponents list. The entity is added to the list when it is not
+  /// a newly created entity or is not an entity to be removed
+  /// \param[in] _entity Entity that has component newly modified
+  public: void AddModifiedComponent(const Entity &_entity);
+
   /// \brief Map of component storage classes. The key is a component
   /// type id, and the value is a pointer to the component storage.
   public: std::unordered_map<ComponentTypeId,
@@ -93,6 +99,12 @@ class ignition::gazebo::EntityComponentManagerPrivate
   /// \brief Entities that need to be removed.
   public: std::unordered_set<Entity> toRemoveEntities;
 
+  /// \brief Entities that have components newly modified
+  /// (created/modified/removed) but are not entities that have been
+  /// newly created or removed (ie. newlyCreatedEntities or toRemoveEntities).
+  /// This is used for the ChangedState functions
+  public: std::unordered_set<Entity> modifiedComponents;
+
   /// \brief Flag that indicates if all entities should be removed.
   public: bool removeAllEntities{false};
 
@@ -199,6 +211,7 @@ void EntityComponentManager::ClearNewlyCreatedEntities()
 {
   std::lock_guard<std::mutex> lock(this->dataPtr->entityCreatedMutex);
   this->dataPtr->newlyCreatedEntities.clear();
+  this->dataPtr->modifiedComponents.clear();
 
   for (auto &view : this->dataPtr->views)
   {
@@ -354,6 +367,8 @@ bool EntityComponentManager::RemoveComponent(
 
   this->UpdateViews(_entity);
 
+  this->dataPtr->AddModifiedComponent(_entity);
+
   // Add component to map of removed components
   {
     std::lock_guard<std::mutex> lock(this->dataPtr->removedComponentsMutex);
@@ -529,6 +544,8 @@ ComponentKey EntityComponentManager::CreateComponentImplementation(
     }
   }
 
+  this->dataPtr->AddModifiedComponent(_entity);
+
   // Instantiate the new component.
   std::pair<ComponentId, bool> componentIdPair =
     this->dataPtr->components[_componentTypeId]->Create(_data);
@@ -1021,7 +1038,11 @@ ignition::msgs::SerializedState EntityComponentManager::ChangedState() const
     this->AddEntityToMessage(stateMsg, entity);
   }
 
-  // TODO(anyone) New / removed / changed components
+  // New / removed / changed components
+  for (const auto &entity : this->dataPtr->modifiedComponents)
+  {
+    this->AddEntityToMessage(stateMsg, entity);
+  }
 
   return stateMsg;
 }
@@ -1042,7 +1063,11 @@ void EntityComponentManager::ChangedState(
     this->AddEntityToMessage(_state, entity);
   }
 
-  // TODO(anyone) New / removed / changed components
+  // New / removed / changed components
+  for (const auto &entity : this->dataPtr->modifiedComponents)
+  {
+    this->AddEntityToMessage(_state, entity);
+  }
 }
 
 //////////////////////////////////////////////////
@@ -1257,6 +1282,7 @@ void EntityComponentManager::SetState(
         // values.
         // igndbg << *comp << "  " << *newComp.get() << std::endl;
         // *comp = *newComp.get();
+        this->dataPtr->AddModifiedComponent(entity);
       }
     }
   }
@@ -1367,6 +1393,7 @@ void EntityComponentManager::SetState(
           }
         }
         this->SetChanged(entity, compIter.first, flag);
+        this->dataPtr->AddModifiedComponent(entity);
       }
     }
   }
@@ -1433,6 +1460,8 @@ void EntityComponentManager::SetChanged(
     this->dataPtr->periodicChangedComponents.erase(typeIter->second);
     this->dataPtr->oneTimeChangedComponents.erase(typeIter->second);
   }
+
+  this->dataPtr->AddModifiedComponent(_entity);
 }
 
 /////////////////////////////////////////////////
@@ -1465,3 +1494,19 @@ void EntityComponentManager::SetEntityCreateOffset(uint64_t _offset)
 
   this->dataPtr->entityCount = _offset;
 }
+
+/////////////////////////////////////////////////
+void EntityComponentManagerPrivate::AddModifiedComponent(const Entity &_entity)
+{
+  if (this->newlyCreatedEntities.find(_entity)
+        != this->newlyCreatedEntities.end() ||
+      this->toRemoveEntities.find(_entity) != this->toRemoveEntities.end() ||
+      this->modifiedComponents.find(_entity) != this->modifiedComponents.end())
+  {
+    // modified component is already in newlyCreatedEntities
+    // or toRemoveEntities list
+    return;
+  }
+
+  this->modifiedComponents.insert(_entity);
+}

src/EntityComponentManager_TEST.cc

@@ -1805,7 +1805,7 @@ TEST_P(EntityComponentManagerFixture, State)
   {
     // Check the changed state
     auto changedStateMsg = manager.ChangedState();
-    EXPECT_EQ(2, changedStateMsg.entities_size());
+    EXPECT_EQ(4, changedStateMsg.entities_size());
 
     const auto &e4Msg = changedStateMsg.entities(0);
     EXPECT_EQ(e4, e4Msg.id());
@@ -1887,6 +1887,74 @@ TEST_P(EntityComponentManagerFixture, State)
   }
 }
 
+/////////////////////////////////////////////////
+TEST_P(EntityComponentManagerFixture, ChangedStateComponents)
+{
+  // Entity and component
+  Entity e1{1};
+  int e1c0{123};
+  std::string e1c1{"string"};
+
+  // Fill manager with entity
+  EXPECT_EQ(e1, manager.CreateEntity());
+  EXPECT_EQ(1u, manager.EntityCount());
+
+  // Component
+  manager.CreateComponent<IntComponent>(e1, IntComponent(e1c0));
+
+  // Serialize into a message
+  msgs::SerializedStateMap stateMsg;
+  manager.State(stateMsg);
+  ASSERT_EQ(1, stateMsg.entities_size());
+
+  // Mark entities/components as not new
+  manager.RunClearNewlyCreatedEntities();
+  auto changedStateMsg = manager.ChangedState();
+  EXPECT_EQ(0, changedStateMsg.entities_size());
+
+  // create component
+  auto compKey =
+      manager.CreateComponent<StringComponent>(e1, StringComponent(e1c1));
+  changedStateMsg = manager.ChangedState();
+  EXPECT_EQ(1, changedStateMsg.entities_size());
+
+  // Mark entities/components as not new
+  manager.RunClearNewlyCreatedEntities();
+  changedStateMsg = manager.ChangedState();
+  EXPECT_EQ(0, changedStateMsg.entities_size());
+
+  // modify component
+  manager.State(stateMsg);
+  auto iter = stateMsg.mutable_entities()->find(e1);
+  ASSERT_TRUE(iter != stateMsg.mutable_entities()->end());
+
+  msgs::SerializedEntityMap &e1Msg = iter->second;
+
+  auto compIter = e1Msg.mutable_components()->find(compKey.first);
+  ASSERT_TRUE(compIter != e1Msg.mutable_components()->end());
+
+  msgs::SerializedComponent &e1c1Msg = compIter->second;
+  EXPECT_EQ(e1c1, e1c1Msg.component());
+  e1c1Msg.set_component("test");
+  EXPECT_EQ("test", e1c1Msg.component());
+  (*e1Msg.mutable_components())[e1c1Msg.type()] = e1c1Msg;
+
+  manager.SetState(stateMsg);
+  changedStateMsg = manager.ChangedState();
+  EXPECT_EQ(1, changedStateMsg.entities_size());
+
+  // Mark entities/components as not new
+  manager.RunClearNewlyCreatedEntities();
+  changedStateMsg = manager.ChangedState();
+  EXPECT_EQ(0, changedStateMsg.entities_size());
+
+  // remove component
+  manager.RemoveComponent(e1, StringComponent::typeId);
+
+  changedStateMsg = manager.ChangedState();
+  EXPECT_EQ(1, changedStateMsg.entities_size());
+}
+
 /////////////////////////////////////////////////
 TEST_P(EntityComponentManagerFixture, Descendants)
 {

test/integration/log_system.cc

@@ -780,7 +780,27 @@ TEST_F(LogSystemTest, RecordAndPlayback)
   stateMsg.ParseFromString(recordedIter->Data());
   // entity size = 28 in dbl pendulum + 4 in nested model
   EXPECT_EQ(32, stateMsg.entities_size());
-  EXPECT_EQ(batch.end(), ++recordedIter);
+  EXPECT_NE(batch.end(), ++recordedIter);
+
+  // check rest of recordIter (state message) for poses
+  while (batch.end() != recordedIter)
+  {
+    EXPECT_EQ("ignition.msgs.SerializedStateMap", recordedIter->Type());
+    EXPECT_EQ(recordedIter->Topic(), "/world/log_pendulum/changed_state");
+
+    stateMsg.ParseFromString(recordedIter->Data());
+
+    for (const auto &entityIter : stateMsg.entities())
+    {
+      for (const auto &compIter : entityIter.second.components())
+      {
+        EXPECT_EQ(components::Pose::typeId, compIter.second.type());
+      }
+    }
+    ++recordedIter;
+  }
+
+  EXPECT_EQ(batch.end(), recordedIter);
 
   // Keep track of total number of pose comparisons
   int nTotal{0};