update remove logic, cleanup

Signed-off-by: Ian Chen <[email protected]>
gazebosim · Nov 14, 2023 · 1e2bcdd · 1e2bcdd
1 parent 00dba93
commit 1e2bcdd
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 14 deletions.
diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh
@@ -405,17 +405,22 @@ class Base : public Implements3d<FeatureList<Feature>>
     model->nestedModelEntityIds.clear();
 
     // remove references in parent model or world model
-    auto *parentModel = this->ReferenceInterface<ModelInfo>(_parentID);
-    auto nestedModelIt =
-        parentModel->nestedModelNameToEntityId.find(model->name);
-    if (nestedModelIt != parentModel->nestedModelNameToEntityId.end())
+    auto parentModelIt = this->models.find(_parentID);
+    if (parentModelIt != this->models.end())
     {
-      std::size_t nestedModelID = nestedModelIt->second;
-      parentModel->nestedModelNameToEntityId.erase(nestedModelIt);
-      parentModel->nestedModelEntityIds.erase(std::remove(
-          parentModel->nestedModelEntityIds.begin(),
-          parentModel->nestedModelEntityIds.end(), nestedModelID),
-          parentModel->nestedModelEntityIds.end());
+      auto parentModel = parentModelIt->second;
+      auto nestedModelIt =
+          parentModel->nestedModelNameToEntityId.find(model->name);
+      if (nestedModelIt !=
+          parentModel->nestedModelNameToEntityId.end())
+      {
+        std::size_t nestedModelID = nestedModelIt->second;
+        parentModel->nestedModelNameToEntityId.erase(nestedModelIt);
+        parentModel->nestedModelEntityIds.erase(std::remove(
+            parentModel->nestedModelEntityIds.begin(),
+            parentModel->nestedModelEntityIds.end(), nestedModelID),
+            parentModel->nestedModelEntityIds.end());
+      }
     }
 
     // If nested, we are done here. No need to remove multibody

diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc
@@ -215,10 +215,6 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID)
 bool EntityManagementFeatures::ModelRemoved(
     const Identity &_modelID) const
 {
-//  auto *model = this->ReferenceInterface<ModelInfo>(_modelID);
-//  auto *world = this->ReferenceInterface<WorldInfo>(model->world);
-//  return world->modelIndexToEntityId.count(model->indexInWorld) == 0;
-
   return this->models.find(_modelID) == this->models.end();
 }