Fix topLevelModel method (#600)

Signed-off-by: Ashton Larkin <[email protected]>
gazebosim · Feb 1, 2021 · d8b9d0c · d8b9d0c
1 parent 4c87e1d
commit d8b9d0c
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 16 deletions.
diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh
@@ -134,7 +134,8 @@ namespace ignition
     /// \brief Get the top level model of an entity
     /// \param[in] _entity Input entity
     /// \param[in] _ecm Constant reference to ECM.
-    /// \return Entity of top level model
+    /// \return Entity of top level model. If _entity has no top level model,
+    /// kNullEntity is returned.
     ignition::gazebo::Entity IGNITION_GAZEBO_VISIBLE topLevelModel(
         const Entity &_entity,
         const EntityComponentManager &_ecm);

diff --git a/src/Util.cc b/src/Util.cc
@@ -400,22 +400,23 @@ ignition::gazebo::Entity topLevelModel(const Entity &_entity,
 {
   auto entity = _entity;
 
-  // check if parent is a model
-  auto parentComp = _ecm.Component<components::ParentEntity>(entity);
-  while (parentComp)
-  {
-    // check if parent is a model
-    auto parentEntity = parentComp->Data();
-    auto modelComp = _ecm.Component<components::Model>(
-        parentEntity);
-    if (!modelComp)
+  // search up the entity tree and find the model with no parent models
+  // (there is the possibility of nested models)
+  Entity modelEntity = kNullEntity;
+  while (entity)
+  {
+    if (_ecm.Component<components::Model>(entity))
+      modelEntity = entity;
+
+    // stop searching if we are at the root of the tree
+    auto parentComp = _ecm.Component<components::ParentEntity>(entity);
+    if (!parentComp)
       break;
 
-    // set current model entity
-    entity = parentEntity;
-    parentComp = _ecm.Component<components::ParentEntity>(entity);
+    entity = parentComp->Data();
   }
-  return entity;
+
+  return modelEntity;
 }
 
 //////////////////////////////////////////////////

diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc
@@ -428,6 +428,7 @@ TEST_F(UtilTest, TopLevelModel)
   //    - linkA
   //    - modelB
   //      - linkB
+  //        - visualB
   //  - modelC
 
   // World
@@ -459,20 +460,31 @@ TEST_F(UtilTest, TopLevelModel)
   ecm.CreateComponent(linkBEntity, components::Name("linkB_name"));
   ecm.CreateComponent(linkBEntity, components::ParentEntity(modelBEntity));
 
+  // Visual B - child of Link B
+  auto visualBEntity = ecm.CreateEntity();
+  ecm.CreateComponent(visualBEntity, components::Visual());
+  ecm.CreateComponent(visualBEntity, components::Name("visualB_name"));
+  ecm.CreateComponent(visualBEntity, components::ParentEntity(linkBEntity));
+
   // Model C
   auto modelCEntity = ecm.CreateEntity();
   ecm.CreateComponent(modelCEntity, components::Model());
   ecm.CreateComponent(modelCEntity, components::Name("modelC_name"));
   ecm.CreateComponent(modelCEntity, components::ParentEntity(worldEntity));
 
-  // model A, link A, model B and link B should have model A as top level entity
+  // model A, link A, model B, link B and visual B should have
+  // model A as the top level model
   EXPECT_EQ(modelAEntity, topLevelModel(modelAEntity, ecm));
   EXPECT_EQ(modelAEntity, topLevelModel(linkAEntity, ecm));
   EXPECT_EQ(modelAEntity, topLevelModel(modelBEntity, ecm));
   EXPECT_EQ(modelAEntity, topLevelModel(linkBEntity, ecm));
+  EXPECT_EQ(modelAEntity, topLevelModel(visualBEntity, ecm));
 
-  // model C should have itself as the top level entity
+  // model C should have itself as the top level model
   EXPECT_EQ(modelCEntity, topLevelModel(modelCEntity, ecm));
+
+  // the world should have no top level model
+  EXPECT_EQ(kNullEntity, topLevelModel(worldEntity, ecm));
 }
 
 /////////////////////////////////////////////////