From 0d102682f8cc44c087b02acac1ff694b0564bf2d Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Thu, 1 Apr 2021 16:53:37 -0700 Subject: [PATCH 1/3] ecm's ChangedState to contain modified components Signed-off-by: Jenn Nguyen --- .../ignition/gazebo/EntityComponentManager.hh | 8 +-- src/EntityComponentManager.cc | 49 ++++++++++++- src/EntityComponentManager_TEST.cc | 70 ++++++++++++++++++- 3 files changed, 118 insertions(+), 9 deletions(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 768432498d..17f125775b 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/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 diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 4c1089f1aa..b4d412cbd3 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -72,6 +72,12 @@ class ignition::gazebo::EntityComponentManagerPrivate msgs::SerializedStateMap &_msg, const std::unordered_set &_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 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 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 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 lock(this->dataPtr->removedComponentsMutex); @@ -529,6 +544,8 @@ ComponentKey EntityComponentManager::CreateComponentImplementation( } } + this->dataPtr->AddModifiedComponent(_entity); + // Instantiate the new component. std::pair 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); +} diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index cf42727bdd..3293eeb15d 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/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(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(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) { From e6ffee0526b0fe96e63ef3c78b430deb4412e482 Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Tue, 6 Apr 2021 12:01:15 -0700 Subject: [PATCH 2/3] updated log_system test Signed-off-by: Jenn Nguyen --- test/integration/log_system.cc | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index df16897a4e..c8b6c40882 100644 --- a/test/integration/log_system.cc +++ b/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}; From 70c432050ff7c18d5cb3f79de5cb697ce950ff14 Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Wed, 7 Apr 2021 16:28:15 -0700 Subject: [PATCH 3/3] removed unnecessary calls Signed-off-by: Jenn Nguyen --- src/EntityComponentManager.cc | 8 +++++--- src/EntityComponentManager_TEST.cc | 10 +++++----- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index b4d412cbd3..acb11faaf9 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -211,7 +211,6 @@ void EntityComponentManager::ClearNewlyCreatedEntities() { std::lock_guard lock(this->dataPtr->entityCreatedMutex); this->dataPtr->newlyCreatedEntities.clear(); - this->dataPtr->modifiedComponents.clear(); for (auto &view : this->dataPtr->views) { @@ -1282,7 +1281,10 @@ void EntityComponentManager::SetState( // values. // igndbg << *comp << " " << *newComp.get() << std::endl; // *comp = *newComp.get(); - this->dataPtr->AddModifiedComponent(entity); + + // When above TODO is addressed, uncomment AddModifiedComponent below + // unless calling SetChanged (which already calls AddModifiedComponent) + // this->dataPtr->AddModifiedComponent(entity); } } } @@ -1393,7 +1395,6 @@ void EntityComponentManager::SetState( } } this->SetChanged(entity, compIter.first, flag); - this->dataPtr->AddModifiedComponent(entity); } } } @@ -1429,6 +1430,7 @@ void EntityComponentManager::SetAllComponentsUnchanged() { this->dataPtr->periodicChangedComponents.clear(); this->dataPtr->oneTimeChangedComponents.clear(); + this->dataPtr->modifiedComponents.clear(); } ///////////////////////////////////////////////// diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 3293eeb15d..04e9b86a25 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -1917,14 +1917,14 @@ TEST_P(EntityComponentManagerFixture, ChangedStateComponents) manager.CreateComponent(e1, StringComponent(e1c1)); changedStateMsg = manager.ChangedState(); EXPECT_EQ(1, changedStateMsg.entities_size()); + manager.State(stateMsg); - // Mark entities/components as not new - manager.RunClearNewlyCreatedEntities(); + // Mark components as not new + manager.RunSetAllComponentsUnchanged(); 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()); @@ -1943,8 +1943,8 @@ TEST_P(EntityComponentManagerFixture, ChangedStateComponents) changedStateMsg = manager.ChangedState(); EXPECT_EQ(1, changedStateMsg.entities_size()); - // Mark entities/components as not new - manager.RunClearNewlyCreatedEntities(); + // Mark components as not new + manager.RunSetAllComponentsUnchanged(); changedStateMsg = manager.ChangedState(); EXPECT_EQ(0, changedStateMsg.entities_size());