Fix seg fault due to invalid iterator to mutable_entities

gazebosim · Dec 16, 2020 · 17e79ac · 17e79ac
1 parent 2f7f33d
commit 17e79ac
Showing 1 changed file with 9 additions and 0 deletions.
diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc
@@ -772,6 +772,15 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity,
 
   // Find the entity in the message
   auto entIter = _msg.mutable_entities()->find(_entity);
+  // Add the entity to the message, if not already added
+  if (entIter == _msg.mutable_entities()->end())
+  {
+    msgs::SerializedEntityMap ent;
+    ent.set_id(_entity);
+    entIter =
+      (_msg.mutable_entities())->insert({static_cast<uint64_t>(_entity), ent})
+      .first;
+  }
 
   auto it = this->removedComponents.find(_entity);
   for (uint64_t i = 0; i < nEntityKeys; ++i)