Merge 8d35373 into dfb43a0

src/EntityComponentManager.cc

@@ -87,15 +87,15 @@ class ignition::gazebo::EntityComponentManagerPrivate
   /// NOTE: Any modification of this data structure must be followed
   /// by setting `entityComponentsDirty` to true.
   public: std::unordered_map<Entity,
-          std::unordered_map<ComponentTypeId, ComponentKey>> entityComponents;
+          std::unordered_map<ComponentTypeId, ComponentId>> entityComponents;
 
   /// \brief A vector of iterators to evenly distributed spots in the
   /// `entityComponents` map.  Threads in the `State` function use this
   /// vector for easy access of their pre-allocated work.  This vector
   /// is recalculated if `entityComponents` is changed (when
   /// `entityComponentsDirty` == true).
   public: std::vector<std::unordered_map<Entity,
-          std::unordered_map<ComponentTypeId, ComponentKey>>::iterator>
+          std::unordered_map<ComponentTypeId, ComponentId>>::iterator>
             entityComponentIterators;
 
   /// \brief A mutex to protect newly created entityes.
@@ -272,8 +272,7 @@ void EntityComponentManager::ProcessRemoveEntityRequests()
       {
         for (const auto &key : entityIter->second)
         {
-          this->dataPtr->components.at(key.second.first)->Remove(
-              key.second.second);
+          this->dataPtr->components.at(key.first)->Remove(key.second);
         }
 
         // Remove the entry in the entityComponent map
@@ -384,12 +383,14 @@ ComponentState EntityComponentManager::ComponentState(const Entity _entity,
   if (typeKey == ecIter->second.end())
     return result;
 
-  if (this->dataPtr->oneTimeChangedComponents.find(typeKey->second) !=
+  ComponentKey key{_typeId, typeKey->second};
+
+  if (this->dataPtr->oneTimeChangedComponents.find(key) !=
       this->dataPtr->oneTimeChangedComponents.end())
   {
     result = ComponentState::OneTimeChange;
   }
-  else if (this->dataPtr->periodicChangedComponents.find(typeKey->second) !=
+  else if (this->dataPtr->periodicChangedComponents.find(key) !=
       this->dataPtr->periodicChangedComponents.end())
   {
     result = ComponentState::PeriodicChange;
@@ -496,7 +497,7 @@ ComponentKey EntityComponentManager::CreateComponentImplementation(
   ComponentKey componentKey{_componentTypeId, componentIdPair.first};
 
   this->dataPtr->entityComponents[_entity].insert(
-      {_componentTypeId, componentKey});
+      {_componentTypeId, componentIdPair.first});
   this->dataPtr->oneTimeChangedComponents.insert(componentKey);
   this->dataPtr->entityComponentsDirty = true;
 
@@ -541,7 +542,7 @@ ComponentId EntityComponentManager::EntityComponentIdFromType(
 
   auto typeIter = ecIter->second.find(_type);
   if (typeIter != ecIter->second.end())
-    return typeIter->second.second;
+    return typeIter->second;
 
   return -1;
 }
@@ -559,8 +560,8 @@ const components::BaseComponent
 
   auto typeIter = ecIter->second.find(_type);
   if (typeIter != ecIter->second.end())
-    return this->dataPtr->components.at(typeIter->second.first)->Component(
-        typeIter->second.second);
+    return this->dataPtr->components.at(_type)->Component(
+        typeIter->second);
 
   return nullptr;
 }
@@ -576,8 +577,7 @@ components::BaseComponent *EntityComponentManager::ComponentImplementation(
 
   auto typeIter = ecIter->second.find(_type);
   if (typeIter != ecIter->second.end())
-    return this->dataPtr->components.at(typeIter->second.first)->Component(
-        typeIter->second.second);
+    return this->dataPtr->components.at(_type)->Component(typeIter->second);
 
   return nullptr;
 }
@@ -768,16 +768,14 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg,
   for (const ComponentTypeId type : types)
   {
     // If the entity does not have the component, continue
-    std::unordered_map<ComponentTypeId, ComponentKey>::iterator typeIter =
-      iter->second.find(type);
+    auto typeIter = iter->second.find(type);
     if (typeIter == iter->second.end())
     {
       continue;
     }
-    ComponentKey comp = (typeIter->second);
 
     auto compMsg = entityMsg->add_components();
-    auto compBase = this->ComponentImplementation(_entity, comp.first);
+    auto compBase = this->ComponentImplementation(_entity, type);
     compMsg->set_type(compBase->TypeId());
 
     std::ostringstream ostr;
@@ -833,16 +831,16 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg,
   // Empty means all types
   for (const ComponentTypeId type : types)
   {
-    std::unordered_map<ComponentTypeId, ComponentKey>::iterator typeIter =
-      iter->second.find(type);
+    auto typeIter = iter->second.find(type);
     if (typeIter == iter->second.end())
     {
       continue;
     }
 
-    ComponentKey comp = typeIter->second;
     const components::BaseComponent *compBase =
-      this->ComponentImplementation(_entity, comp.first);
+      this->ComponentImplementation(_entity, type);
+
+    ComponentKey comp = {type, typeIter->second};
 
     // If not sending full state, skip unchanged components
     if (!_full &&
@@ -1303,20 +1301,22 @@ void EntityComponentManager::SetChanged(
   if (typeIter == ecIter->second.end())
     return;
 
+  ComponentKey key{_type, typeIter->second};
+
   if (_c == ComponentState::PeriodicChange)
   {
-    this->dataPtr->periodicChangedComponents.insert(typeIter->second);
-    this->dataPtr->oneTimeChangedComponents.erase(typeIter->second);
+    this->dataPtr->periodicChangedComponents.insert(key);
+    this->dataPtr->oneTimeChangedComponents.erase(key);
   }
   else if (_c == ComponentState::OneTimeChange)
   {
-    this->dataPtr->periodicChangedComponents.erase(typeIter->second);
-    this->dataPtr->oneTimeChangedComponents.insert(typeIter->second);
+    this->dataPtr->periodicChangedComponents.erase(key);
+    this->dataPtr->oneTimeChangedComponents.insert(key);
   }
   else
   {
-    this->dataPtr->periodicChangedComponents.erase(typeIter->second);
-    this->dataPtr->oneTimeChangedComponents.erase(typeIter->second);
+    this->dataPtr->periodicChangedComponents.erase(key);
+    this->dataPtr->oneTimeChangedComponents.erase(key);
   }
 }
 

src/EntityComponentManager_TEST.cc

@@ -2158,6 +2158,11 @@ TEST_P(EntityComponentManagerFixture, SetEntityCreateOffset)
   manager.SetEntityCreateOffset(1000);
   Entity entity2 = manager.CreateEntity();
   EXPECT_EQ(1001u, entity2);
+
+  // Apply a lower offset, prints warning but goes through.
+  manager.SetEntityCreateOffset(500);
+  Entity entity3 = manager.CreateEntity();
+  EXPECT_EQ(501u, entity3);
 }
 
 // Run multiple times. We want to make sure that static globals don't cause