diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 8562cfc072..afb65bdd6e 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -478,6 +478,15 @@ ComponentKey EntityComponentManager::CreateComponentImplementation( const Entity _entity, const ComponentTypeId _componentTypeId, const components::BaseComponent *_data) { + // make sure the entity exists + if (!this->HasEntity(_entity)) + { + ignerr << "Trying to create a component of type [" << _componentTypeId + << "] attached to entity [" << _entity << "], but this entity does not " + << "exist. This create component request will be ignored." << std::endl; + return ComponentKey(); + } + // If type hasn't been instantiated yet, create a storage for it if (!this->HasComponentType(_componentTypeId)) { diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 75228f3803..59db43c671 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -433,6 +433,16 @@ TEST_P(EntityComponentManagerFixture, EntitiesAndComponents) EXPECT_FALSE(manager.EntityHasComponentType(entity, DoubleComponent::typeId)); EXPECT_FALSE(manager.EntityHasComponentType(entity2, IntComponent::typeId)); + // Try to add a component to an entity that does not exist + EXPECT_FALSE(manager.HasEntity(kNullEntity)); + EXPECT_FALSE(manager.EntityHasComponentType(kNullEntity, + IntComponent::typeId)); + EXPECT_EQ(ComponentKey(), manager.CreateComponent(kNullEntity, + IntComponent(123))); + EXPECT_FALSE(manager.HasEntity(kNullEntity)); + EXPECT_FALSE(manager.EntityHasComponentType(kNullEntity, + IntComponent::typeId)); + // Remove all entities manager.RequestRemoveEntities(); EXPECT_EQ(3u, manager.EntityCount()); diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index eb356bd491..71e0ab704d 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -238,7 +238,6 @@ void DiffDrive::Configure(const Entity &_entity, this->dataPtr->limiterAng->SetMaxJerk(maxJerk); } - double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; if (odomFreq > 0) { @@ -324,6 +323,10 @@ void DiffDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, for (Entity joint : this->dataPtr->leftJoints) { + // skip this entity if it has been removed + if (!_ecm.HasEntity(joint)) + continue; + // Update wheel velocity auto vel = _ecm.Component(joint); @@ -340,6 +343,10 @@ void DiffDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, for (Entity joint : this->dataPtr->rightJoints) { + // skip this entity if it has been removed + if (!_ecm.HasEntity(joint)) + continue; + // Update wheel velocity auto vel = _ecm.Component(joint); @@ -358,7 +365,7 @@ void DiffDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, // don't exist. auto leftPos = _ecm.Component( this->dataPtr->leftJoints[0]); - if (!leftPos) + if (!leftPos && _ecm.HasEntity(this->dataPtr->leftJoints[0])) { _ecm.CreateComponent(this->dataPtr->leftJoints[0], components::JointPosition()); @@ -366,7 +373,7 @@ void DiffDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, auto rightPos = _ecm.Component( this->dataPtr->rightJoints[0]); - if (!rightPos) + if (!rightPos && _ecm.HasEntity(this->dataPtr->rightJoints[0])) { _ecm.CreateComponent(this->dataPtr->rightJoints[0], components::JointPosition());