Sync components removal

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.

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<ComponentTypeId,
@@ -75,7 +87,7 @@ class ignition::gazebo::EntityComponentManagerPrivate
   /// \brief The set of components that each entity has
   public: std::map<Entity, std::vector<ComponentKey>> 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<detail::ComponentTypeKey, detail::View> 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<Entity, ComponentKey> removedComponents;
 };
 
 //////////////////////////////////////////////////
@@ -156,6 +176,13 @@ void EntityComponentManager::ClearNewlyCreatedEntities()
   }
 }
 
+/////////////////////////////////////////////////
+void EntityComponentManager::ClearRemovedComponents()
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->removedComponentsMutex);
+  this->dataPtr->removedComponents.clear();
+}
+
 /////////////////////////////////////////////////
 void EntityComponentManagerPrivate::InsertEntityRecursive(Entity _entity,
     std::set<Entity> &_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<std::mutex> 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<std::mutex> 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<std::mutex> 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<int64_t>(removedComponent.first)] = compMsg;
+
+    it++;
+  }
+}
+
 //////////////////////////////////////////////////
 void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg,
     Entity _entity, const std::unordered_set<ComponentTypeId> &_types) const
@@ -766,9 +856,12 @@ 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.
+  // TODO(anyone) Set component being removed once we have a way to queue it
+  this->dataPtr->SetRemovedComponentsMsgs(_entity, entityMsg);
 }
 
 //////////////////////////////////////////////////
@@ -851,10 +944,13 @@ 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.
+  // TODO(anyone) Set component being removed once we have a way to queue it
+  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() &&

src/EntityComponentManager_TEST.cc

@@ -82,6 +82,10 @@ class EntityCompMgrTest : public EntityComponentManager
   {
     this->SetAllComponentsUnchanged();
   }
+  public: void RunClearRemovedComponents()
+  {
+    this->ClearRemovedComponents();
+  }
 };
 
 class EntityComponentManagerFixture : public ::testing::TestWithParam<int>
@@ -2009,6 +2013,191 @@ TEST_P(EntityComponentManagerFixture, SetChanged)
       manager.ComponentState(e2, c2.first));
 }
 
+//////////////////////////////////////////////////
+TEST_P(EntityComponentManagerFixture, SerializedStateMapMsgAfterRemoveComponent)
+{
+  // Create entity
+  Entity e1 = manager.CreateEntity();
+  manager.CreateComponent<IntComponent>(e1, IntComponent(123));
+  auto e1c1 =
+    manager.CreateComponent<DoubleComponent>(e1, DoubleComponent(0.0));
+  auto e1c2 =
+    manager.CreateComponent<StringComponent>(e1, StringComponent("int"));
+
+  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 &e1c2Msg = compIter->second;
+    compIter++;
+    EXPECT_TRUE(e1c2Msg.remove());
+
+    // Second component
+    const auto &e1c1Msg = compIter->second;
+    compIter++;
+    EXPECT_TRUE(e1c1Msg.remove());
+
+    // Third component
+    const auto &e1c0Msg = compIter->second;
+    EXPECT_FALSE(e1c0Msg.remove());
+  }
+
+  // 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();
+  manager.CreateComponent<IntComponent>(e1, IntComponent(123));
+  auto e1c1 =
+    manager.CreateComponent<DoubleComponent>(e1, DoubleComponent(0.0));
+  auto e1c2 =
+    manager.CreateComponent<StringComponent>(e1, StringComponent("int"));
+
+  manager.RemoveComponent(e1, e1c1);
+  manager.RemoveComponent(e1, e1c2);
+
+  // Serialize into a message
+  msgs::SerializedState stateMsg;
+  stateMsg = manager.State();
+
+  // Check message
+  {
+    auto entityMsg = stateMsg.entities(0);
+
+    // First component
+    const auto &e1c0Msg = entityMsg.components(0);
+    EXPECT_FALSE(e1c0Msg.remove());
+
+    // Second component
+    const auto &e1c1Msg = entityMsg.components(1);
+    EXPECT_TRUE(e1c1Msg.remove());
+
+    // Third component
+    const auto &e1c2Msg = entityMsg.components(2);
+    EXPECT_TRUE(e1c2Msg.remove());
+  }
+
+  // Check that removed components don't exist anymore after clearing them
+  manager.RunClearRemovedComponents();
+  msgs::SerializedState newStateMsg;
+  newStateMsg = manager.State();
+
+  // Check message
+  {
+    auto 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();
+  manager.CreateComponent<IntComponent>(e1, IntComponent(123));
+  auto e1c1 =
+    manager.CreateComponent<DoubleComponent>(e1, DoubleComponent(0.0));
+  auto e1c2 =
+    manager.CreateComponent<StringComponent>(e1, StringComponent("int"));
+
+  // 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 &e1c2Msg = compIter->second;
+    compIter++;
+    EXPECT_FALSE(e1c2Msg.remove());
+
+    // Second component
+    const auto &e1c1Msg = compIter->second;
+    compIter++;
+    EXPECT_FALSE(e1c1Msg.remove());
+
+    // Third component
+    const auto &e1c0Msg = compIter->second;
+    EXPECT_FALSE(e1c0Msg.remove());
+  }
+
+  // Remove components and synchronize again
+  manager.RemoveComponent(e1, e1c1);
+  manager.RemoveComponent(e1, e1c2);
+
+  msgs::SerializedStateMap newStateMsg;
+  manager.State(newStateMsg);
+
+  guiManager.SetState(newStateMsg);
+  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 &e1c2Msg = compIter->second;
+    compIter++;
+    EXPECT_TRUE(e1c2Msg.remove());
+
+    // Second component
+    const auto &e1c1Msg = compIter->second;
+    compIter++;
+    EXPECT_TRUE(e1c1Msg.remove());
+
+    // Third component
+    const auto &e1c0Msg = compIter->second;
+    EXPECT_FALSE(e1c0Msg.remove());
+  }
+}
+
 // Run multiple times. We want to make sure that static globals don't cause
 // problems.
 INSTANTIATE_TEST_CASE_P(EntityComponentManagerRepeat,

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();