Don't create components for entities that don't exist (#927)

Signed-off-by: Ashton Larkin <[email protected]>

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))
  {

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

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<double>("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<components::JointVelocityCmd>(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<components::JointVelocityCmd>(joint);
 
@@ -358,15 +365,15 @@ void DiffDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
  // don't exist.
  auto leftPos = _ecm.Component<components::JointPosition>(
  this->dataPtr->leftJoints[0]);
- if (!leftPos)
+ if (!leftPos && _ecm.HasEntity(this->dataPtr->leftJoints[0]))
  {
  _ecm.CreateComponent(this->dataPtr->leftJoints[0],
  components::JointPosition());
  }
 
  auto rightPos = _ecm.Component<components::JointPosition>(
  this->dataPtr->rightJoints[0]);
- if (!rightPos)
+ if (!rightPos && _ecm.HasEntity(this->dataPtr->rightJoints[0]))
  {
  _ecm.CreateComponent(this->dataPtr->rightJoints[0],
  components::JointPosition());