diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index da7a25bb4f..106f9ce536 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -534,6 +534,10 @@ namespace ignition /// is protected to facilitate testing. protected: void ClearNewlyCreatedEntities(); + /// \brief Clear the list of removed components so that a call to + /// RemoveComponent doesn't make the list grow indefinitely. + protected: void ClearRemovedComponents(); + /// \brief Process all entity remove requests. This will remove /// entities and their components. This function is protected to /// facilitate testing. diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 67fd71b596..e84863900c 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -48,6 +48,18 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \return True if created successfully. public: bool CreateComponentStorage(const ComponentTypeId _typeId); + /// \brief Create a message for the removed components + /// \param[in] _entity Entity with the removed components + /// \param[out] _msg Entity message + public: void SetRemovedComponentsMsgs(Entity &_entity, + msgs::SerializedEntity *_msg); + + /// \brief Create a message for the removed components + /// \param[in] _entity Entity with the removed components + /// \param[out] _msg State message + public: void SetRemovedComponentsMsgs(Entity &_entity, + msgs::SerializedStateMap &_msg); + /// \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::map> entityComponents; - /// \brief A mutex to protect newly created entityes. + /// \brief A mutex to protect newly created entities. public: std::mutex entityCreatedMutex; /// \brief A mutex to protect entity remove. @@ -84,6 +96,9 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \brief A mutex to protect from concurrent writes to views public: mutable std::mutex viewsMutex; + /// \brief A mutex to protect removed components + public: mutable std::mutex removedComponentsMutex; + /// \brief The set of all views. public: mutable std::map views; @@ -94,6 +109,11 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \brief Keep track of entities already used to ensure uniqueness. public: uint64_t entityCount{0}; + + /// \brief Unordered multimap of removed components. The key is the entity to + /// which belongs the component, and the value is the component being + /// removed. + std::unordered_multimap removedComponents; }; ////////////////////////////////////////////////// @@ -156,6 +176,13 @@ void EntityComponentManager::ClearNewlyCreatedEntities() } } +///////////////////////////////////////////////// +void EntityComponentManager::ClearRemovedComponents() +{ + std::lock_guard lock(this->dataPtr->removedComponentsMutex); + this->dataPtr->removedComponents.clear(); +} + ///////////////////////////////////////////////// void EntityComponentManagerPrivate::InsertEntityRecursive(Entity _entity, std::set &_set) @@ -297,6 +324,13 @@ bool EntityComponentManager::RemoveComponent( this->dataPtr->periodicChangedComponents.erase(_key); this->UpdateViews(_entity); + + // Add component to map of removed components + { + std::lock_guard lock(this->dataPtr->removedComponentsMutex); + this->dataPtr->removedComponents.insert(std::make_pair(_entity, _key)); + } + return true; } @@ -733,6 +767,62 @@ void EntityComponentManager::RebuildViews() } } +////////////////////////////////////////////////// +void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, + msgs::SerializedEntity *_entityMsg) +{ + std::lock_guard lock(this->removedComponentsMutex); + uint64_t nEntityKeys = this->removedComponents.count(_entity); + if (nEntityKeys == 0) + return; + + auto it = this->removedComponents.find(_entity); + for (uint64_t i = 0; i < nEntityKeys; ++i) + { + auto compMsg = _entityMsg->add_components(); + + auto removedComponent = it->second; + + // Empty data is needed for the component to be processed afterwards + compMsg->set_component(" "); + compMsg->set_type(removedComponent.first); + compMsg->set_remove(true); + + it++; + } +} + +////////////////////////////////////////////////// +void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, + msgs::SerializedStateMap &_msg) +{ + std::lock_guard lock(this->removedComponentsMutex); + uint64_t nEntityKeys = this->removedComponents.count(_entity); + if (nEntityKeys == 0) + return; + + // Find the entity in the message + auto entIter = _msg.mutable_entities()->find(_entity); + + auto it = this->removedComponents.find(_entity); + for (uint64_t i = 0; i < nEntityKeys; ++i) + { + auto removedComponent = it->second; + + msgs::SerializedComponent compMsg; + + // Empty data is needed for the component to be processed afterwards + compMsg.set_component(" "); + compMsg.set_type(removedComponent.first); + compMsg.set_remove(true); + + (*(entIter->second.mutable_components()))[ + static_cast(removedComponent.first)] = compMsg; + + it++; + } +} + ////////////////////////////////////////////////// void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, Entity _entity, const std::unordered_set &_types) const @@ -766,9 +856,11 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, compBase->Serialize(ostr); compMsg->set_component(ostr.str()); - - // TODO(anyone) Set component being removed once we have a way to queue it } + + // Add a component to the message and set it to be removed if the component + // exists in the removedComponents map. + this->dataPtr->SetRemovedComponentsMsgs(_entity, entityMsg); } ////////////////////////////////////////////////// @@ -851,10 +943,12 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, std::ostringstream ostr; compBase->Serialize(ostr); compIter->second.set_component(ostr.str()); - - // TODO(anyone) Set component being removed once we have a way to queue it } + // Add a component to the message and set it to be removed if the component + // exists in the removedComponents map. + this->dataPtr->SetRemovedComponentsMsgs(_entity, _msg); + // Remove the entity from the message if a component for the entity was // not modified or added. This will allow the state message to shrink. if (entIter == _msg.mutable_entities()->end() && diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index f3932a07b0..efa9853dc5 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -82,6 +82,10 @@ class EntityCompMgrTest : public EntityComponentManager { this->SetAllComponentsUnchanged(); } + public: void RunClearRemovedComponents() + { + this->ClearRemovedComponents(); + } }; class EntityComponentManagerFixture : public ::testing::TestWithParam @@ -2009,6 +2013,227 @@ TEST_P(EntityComponentManagerFixture, SetChanged) manager.ComponentState(e2, c2.first)); } +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, SerializedStateMapMsgAfterRemoveComponent) +{ + // Create entity + Entity e1 = manager.CreateEntity(); + auto e1c0 = + manager.CreateComponent(e1, IntComponent(123)); + auto e1c1 = + manager.CreateComponent(e1, DoubleComponent(0.0)); + auto e1c2 = + manager.CreateComponent(e1, StringComponent("int")); + + // We use this map because the order in which components are iterated + // through depends on the (undetermined) order of unordered multimaps + std::map expectations; + expectations.insert(std::make_pair(e1c0.first, false)); + expectations.insert(std::make_pair(e1c1.first, true)); + expectations.insert(std::make_pair(e1c2.first, true)); + + manager.RemoveComponent(e1, e1c1); + manager.RemoveComponent(e1, e1c2); + + // Serialize into a message + msgs::SerializedStateMap stateMsg; + manager.State(stateMsg); + + // Check message + { + auto iter = stateMsg.entities().find(e1); + const auto &e1Msg = iter->second; + auto compIter = e1Msg.components().begin(); + + // First component + const auto &c0 = compIter->second; + compIter++; + EXPECT_EQ(c0.remove(), expectations.find(c0.type())->second); + + // Second component + const auto &c1 = compIter->second; + compIter++; + EXPECT_EQ(c1.remove(), expectations.find(c1.type())->second); + + // Third component + const auto &c2 = compIter->second; + EXPECT_EQ(c2.remove(), expectations.find(c2.type())->second); + } + + // Check that removed components don't exist anymore after clearing them + manager.RunClearRemovedComponents(); + msgs::SerializedStateMap newStateMsg; + manager.State(newStateMsg); + + // Check message + { + auto iter = newStateMsg.entities().find(e1); + const auto &e1Msg = iter->second; + EXPECT_EQ(1, e1Msg.components_size()); + auto compIter = e1Msg.components().begin(); + + // First component + const auto &e1c0Msg = compIter->second; + EXPECT_FALSE(e1c0Msg.remove()); + } +} + +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, SerializedStateMsgAfterRemoveComponent) +{ + // Create entity + Entity e1 = manager.CreateEntity(); + auto e1c0 = + manager.CreateComponent(e1, IntComponent(123)); + auto e1c1 = + manager.CreateComponent(e1, DoubleComponent(0.0)); + auto e1c2 = + manager.CreateComponent(e1, StringComponent("int")); + + // We use this map because the order in which components are iterated + // through depends on the (undetermined) order of unordered multimaps + std::map expectations; + expectations.insert(std::make_pair(e1c0.first, false)); + expectations.insert(std::make_pair(e1c1.first, true)); + expectations.insert(std::make_pair(e1c2.first, true)); + + manager.RemoveComponent(e1, e1c1); + manager.RemoveComponent(e1, e1c2); + + // Serialize into a message + msgs::SerializedState stateMsg; + stateMsg = manager.State(); + + // Check message + { + auto const &entityMsg = stateMsg.entities(0); + + // First component + const auto &c0 = entityMsg.components(0); + EXPECT_EQ(c0.remove(), expectations.find(c0.type())->second); + + // Second component + const auto &c1 = entityMsg.components(1); + EXPECT_EQ(c1.remove(), expectations.find(c1.type())->second); + + // Third component + const auto &c2 = entityMsg.components(2); + EXPECT_EQ(c2.remove(), expectations.find(c2.type())->second); + } + + // Check that removed components don't exist anymore after clearing them + manager.RunClearRemovedComponents(); + msgs::SerializedState newStateMsg; + newStateMsg = manager.State(); + + // Check message + { + auto const &entityMsg = newStateMsg.entities(0); + EXPECT_EQ(1, entityMsg.components_size()); + + // First component + const auto &e1c0Msg = entityMsg.components(0); + EXPECT_FALSE(e1c0Msg.remove()); + } +} + +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) +{ + // Simulate the GUI's ECM + EntityCompMgrTest guiManager; + + // Create entity + Entity e1 = manager.CreateEntity(); + auto e1c0 = + manager.CreateComponent(e1, IntComponent(123)); + auto e1c1 = + manager.CreateComponent(e1, DoubleComponent(0.0)); + auto e1c2 = + manager.CreateComponent(e1, StringComponent("int")); + + // We use this map because the order in which components are iterated + // through depends on the (undetermined) order of unordered multimaps + std::map expectationsBeforeRemoving; + expectationsBeforeRemoving.insert(std::make_pair(e1c0.first, false)); + expectationsBeforeRemoving.insert(std::make_pair(e1c1.first, false)); + expectationsBeforeRemoving.insert(std::make_pair(e1c2.first, false)); + + // Serialize server ECM into a message + msgs::SerializedStateMap stateMsg; + manager.State(stateMsg); + + // Set GUI's ECM and serialize into a message + guiManager.SetState(stateMsg); + msgs::SerializedStateMap guiStateMsg; + guiManager.State(guiStateMsg); + + // Check sync message + { + auto iter = guiStateMsg.entities().find(e1); + const auto &e1Msg = iter->second; + auto compIter = e1Msg.components().begin(); + + // First component + const auto &c0 = compIter->second; + compIter++; + EXPECT_EQ(c0.remove(), expectationsBeforeRemoving.find(c0.type())->second); + + // Second component + const auto &c1 = compIter->second; + compIter++; + EXPECT_EQ(c1.remove(), expectationsBeforeRemoving.find(c1.type())->second); + + // Third component + const auto &c2 = compIter->second; + EXPECT_EQ(c2.remove(), expectationsBeforeRemoving.find(c2.type())->second); + } + + std::map expectationsAfterRemoving; + expectationsAfterRemoving.insert(std::make_pair(e1c0.first, false)); + expectationsAfterRemoving.insert(std::make_pair(e1c1.first, true)); + expectationsAfterRemoving.insert(std::make_pair(e1c2.first, true)); + + // Remove components and synchronize again + manager.RemoveComponent(e1, e1c1); + manager.RemoveComponent(e1, e1c2); + + msgs::SerializedStateMap newStateMsg; + manager.State(newStateMsg); + + EXPECT_TRUE(nullptr != guiManager.Component(e1)); + EXPECT_TRUE(nullptr != guiManager.Component(e1)); + EXPECT_TRUE(nullptr != guiManager.Component(e1)); + guiManager.SetState(newStateMsg); + EXPECT_TRUE(nullptr != guiManager.Component(e1)); + EXPECT_TRUE(nullptr == guiManager.Component(e1)); + EXPECT_TRUE(nullptr == guiManager.Component(e1)); + + msgs::SerializedStateMap newGuiStateMsg; + guiManager.State(newGuiStateMsg); + + // Check message + { + auto iter = newGuiStateMsg.entities().find(e1); + const auto &e1Msg = iter->second; + auto compIter = e1Msg.components().begin(); + + // First component + const auto &c0 = compIter->second; + compIter++; + EXPECT_EQ(c0.remove(), expectationsAfterRemoving.find(c0.type())->second); + + // Second component + const auto &c1 = compIter->second; + compIter++; + EXPECT_EQ(c1.remove(), expectationsAfterRemoving.find(c1.type())->second); + + // Third component + const auto &c2 = compIter->second; + EXPECT_EQ(c2.remove(), expectationsAfterRemoving.find(c2.type())->second); + } +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_CASE_P(EntityComponentManagerRepeat, diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 3594e5c222..f2c91b390d 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -716,6 +716,9 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Process entity removals. this->entityCompMgr.ProcessRemoveEntityRequests(); + // Process components removals + this->entityCompMgr.ClearRemovedComponents(); + // Each network manager takes care of marking its components as unchanged if (!this->networkMgr) this->entityCompMgr.SetAllComponentsUnchanged(); diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 6d19354829..b0c95760ce 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -102,5 +102,6 @@ void GuiRunner::OnState(const msgs::SerializedStepMap &_msg) } this->ecm.ClearNewlyCreatedEntities(); this->ecm.ProcessRemoveEntityRequests(); + this->ecm.ClearRemovedComponents(); }