diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index b7aa6e0a6a5..8562cfc0721 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -87,7 +87,7 @@ class ignition::gazebo::EntityComponentManagerPrivate /// NOTE: Any modification of this data structure must be followed /// by setting `entityComponentsDirty` to true. public: std::unordered_map> entityComponents; + std::unordered_map> entityComponents; /// \brief A vector of iterators to evenly distributed spots in the /// `entityComponents` map. Threads in the `State` function use this @@ -95,7 +95,7 @@ class ignition::gazebo::EntityComponentManagerPrivate /// is recalculated if `entityComponents` is changed (when /// `entityComponentsDirty` == true). public: std::vector>::iterator> + std::unordered_map>::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::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::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); } } diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 0ef35872477..75228f38039 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/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