[tpe] Add empty nested model construction and nested model entity man…

…agement (#229)

Implements the features `ConstructEmptyNestedModelFeature` and `GetNestedModelFromModel` in TPE

Signed-off-by: Addisu Z. Taddese <[email protected]>

Co-authored-by: Ashton Larkin <[email protected]>

tpe/lib/src/Model.cc

@@ -34,6 +34,12 @@ class ignition::physics::tpelib::ModelPrivate
 
   /// \brief First inserted link id;
   public: std::size_t firstLinkId = kNullEntityId;
+
+  /// \brief Links in the model
+  public: std::vector<std::size_t> linkIds;
+
+  /// \brief Nested models
+  public: std::vector<std::size_t> nestedModelIds;
 };
 
 using namespace ignition;
@@ -64,29 +70,43 @@ Entity &Model::AddLink()
 {
   std::size_t linkId = Entity::GetNextId();
 
-  if (this->GetChildren().empty())
+  if (this->GetLinkCount() == 0u)
     this->dataPtr->firstLinkId = linkId;
 
   const auto[it, success]  = this->GetChildren().insert(
       {linkId, std::make_shared<Link>(linkId)});
+  this->dataPtr->linkIds.push_back(linkId);
 
   it->second->SetParent(this);
   this->ChildrenChanged();
   return *it->second.get();
 }
 
+//////////////////////////////////////////////////
+std::size_t Model::GetLinkCount() const
+{
+  return this->dataPtr->linkIds.size();
+}
+
 //////////////////////////////////////////////////
 Entity &Model::AddModel()
 {
   std::size_t modelId = Entity::GetNextId();
   const auto[it, success]  = this->GetChildren().insert(
       {modelId, std::make_shared<Model>(modelId)});
+  this->dataPtr->nestedModelIds.push_back(modelId);
 
   it->second->SetParent(this);
   this->ChildrenChanged();
   return *it->second.get();
 }
 
+//////////////////////////////////////////////////
+std::size_t Model::GetModelCount() const
+{
+  return this->dataPtr->nestedModelIds.size();
+}
+
 //////////////////////////////////////////////////
 void Model::SetCanonicalLink(std::size_t linkId)
 {

tpe/lib/src/Model.hh

@@ -48,10 +48,18 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
   /// \return Newly created Link
   public: Entity &AddLink();
 
+  /// \brief Get the number of links in the model
+  /// \return Number of links in the model
+  public: std::size_t GetLinkCount() const;
+
   /// \brief Add a nested model
   /// \return Newly created nested model
   public: Entity &AddModel();
 
+  /// \brief Get the number of nested models in the model
+  /// \return Number of nested mdoels in the model
+  public: std::size_t GetModelCount() const;
+
   /// \brief Set the canonical link of model
   public: void SetCanonicalLink(
     std::size_t linkId = kNullEntityId);

tpe/plugin/src/Base.hh

@@ -88,26 +88,38 @@ class Base : public Implements3d<FeatureList<Feature>>
     return -1;
   }
 
-  public: inline std::size_t indexInContainerToId(
-    const std::size_t _containerId, const std::size_t _index) const
+  public: template <typename EntityType>
+  inline std::pair<std::size_t, EntityType> indexInContainerToId(
+      const std::size_t _containerId,
+      const std::size_t _index,
+      const std::map<std::size_t, EntityType> &_idMap) const
   {
     std::size_t counter = 0;
     auto it = this->childIdToParentId.begin();
 
     while (counter <= _index && it != this->childIdToParentId.end())
     {
-      if (it->second == _containerId && counter == _index)
+      if (it->second == _containerId)
       {
-        return it->first;
-      }
-      else if (it->second == _containerId)
-      {
-        ++counter;
+        auto idMapIt = _idMap.find(it->first);
+        // only count if the entity is found in the idMap. This makes sure we're
+        // counting only the entities with the correct EntityType
+        if (idMapIt != _idMap.end())
+        {
+          if (counter == _index)
+          {
+            return *idMapIt;
+          }
+          else
+          {
+            ++counter;
+          }
+        }
       }
       ++it;
     }
     // return invalid id if entity not found
-    return -1;
+    return {INVALID_ENTITY_ID, nullptr};
   }
 
 

tpe/plugin/src/Base_TEST.cc

@@ -158,24 +158,28 @@ TEST(BaseClass, AddEntities)
   // check indices
   std::size_t modelInd1 = base.idToIndexInContainer(modelId1);
   EXPECT_EQ(0u, modelInd1);
-  EXPECT_EQ(modelId1, base.indexInContainerToId(worldId, 0u));
+  EXPECT_EQ(modelId1,
+            base.indexInContainerToId(worldId, 0u, base.models).first);
   std::size_t modelInd2 = base.idToIndexInContainer(modelId2);
   EXPECT_EQ(1u, modelInd2);
-  EXPECT_EQ(modelId2, base.indexInContainerToId(worldId, 1u));
+  EXPECT_EQ(modelId2,
+            base.indexInContainerToId(worldId, 1u, base.models).first);
 
   std::size_t linkInd1 = base.idToIndexInContainer(linkId1);
   EXPECT_EQ(0u, linkInd1);
-  EXPECT_EQ(linkId1, base.indexInContainerToId(modelId1, 0u));
+  EXPECT_EQ(linkId1, base.indexInContainerToId(modelId1, 0u, base.links).first);
   std::size_t linkInd2 = base.idToIndexInContainer(linkId2);
   EXPECT_EQ(0u, linkInd2);
-  EXPECT_EQ(linkId2, base.indexInContainerToId(modelId2, 0u));
+  EXPECT_EQ(linkId2, base.indexInContainerToId(modelId2, 0u, base.links).first);
 
   std::size_t boxInd = base.idToIndexInContainer(boxId);
   EXPECT_EQ(0u, boxInd);
-  EXPECT_EQ(boxId, base.indexInContainerToId(linkId1, 0u));
+  EXPECT_EQ(boxId,
+            base.indexInContainerToId(linkId1, 0u, base.collisions).first);
   std::size_t cylinderInd = base.idToIndexInContainer(cylinderId);
   EXPECT_EQ(0u, cylinderInd);
-  EXPECT_EQ(cylinderId, base.indexInContainerToId(linkId2, 0u));
+  EXPECT_EQ(cylinderId,
+            base.indexInContainerToId(linkId2, 0u, base.collisions).first);
 }
 
 int main(int argc, char *argv[])

tpe/plugin/src/EntityManagementFeatures.cc

@@ -110,11 +110,11 @@ std::size_t EntityManagementFeatures::GetModelCount(
 Identity EntityManagementFeatures::GetModel(
   const Identity &_worldID, const std::size_t _modelIndex) const
 {
-  std::size_t modelId = this->indexInContainerToId(_worldID.id, _modelIndex);
-  auto it = this->models.find(modelId);
-  if (it != this->models.end() && it->second != nullptr)
+  const auto &[modelId, modelInfo] =
+      this->indexInContainerToId(_worldID.id, _modelIndex, this->models);
+  if (modelInfo != nullptr)
   {
-    return this->GenerateIdentity(modelId, it->second);
+    return this->GenerateIdentity(modelId, modelInfo);
   }
   return this->GenerateInvalidId();
 }
@@ -172,22 +172,59 @@ Identity EntityManagementFeatures::GetWorldOfModel(
   return this->GenerateInvalidId();
 }
 
+/////////////////////////////////////////////////
+std::size_t EntityManagementFeatures::GetNestedModelCount(
+  const Identity &_modelID) const
+{
+  return this->ReferenceInterface<ModelInfo>(_modelID)->model->GetModelCount();
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetNestedModel(
+  const Identity &_modelID, const std::size_t _modelIndex) const
+{
+  const auto &[nestedModelId, nestedModelInfo] =
+      this->indexInContainerToId(_modelID.id, _modelIndex, this->models);
+  if (nestedModelInfo != nullptr)
+  {
+    return this->GenerateIdentity(nestedModelId, nestedModelInfo);
+  }
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetNestedModel(
+  const Identity &_modelID, const std::string &_modelName) const
+{
+  auto modelInfo = this->ReferenceInterface<ModelInfo>(_modelID);
+  if (modelInfo != nullptr)
+  {
+    tpelib::Entity &modelEnt = modelInfo->model->GetChildByName(_modelName);
+    auto it = this->models.find(modelEnt.GetId());
+    if (it != this->models.end() && it->second != nullptr)
+    {
+      return this->GenerateIdentity(it->first, it->second);
+    }
+  }
+  return this->GenerateInvalidId();
+}
+
 /////////////////////////////////////////////////
 std::size_t EntityManagementFeatures::GetLinkCount(
   const Identity &_modelID) const
 {
-  return this->ReferenceInterface<ModelInfo>(_modelID)->model->GetChildCount();
+  return this->ReferenceInterface<ModelInfo>(_modelID)->model->GetLinkCount();
 }
 
 /////////////////////////////////////////////////
 Identity EntityManagementFeatures::GetLink(
   const Identity &_modelID, const std::size_t _linkIndex) const
 {
-  std::size_t linkId = this->indexInContainerToId(_modelID.id, _linkIndex);
-  auto it = this->links.find(linkId);
-  if (it != this->links.end() && it->second != nullptr)
+  const auto &[linkId, linkInfo] =
+      this->indexInContainerToId(_modelID.id, _linkIndex, this->links);
+  if (linkInfo != nullptr)
   {
-    return this->GenerateIdentity(it->first, it->second);
+    return this->GenerateIdentity(linkId, linkInfo);
   }
   return this->GenerateInvalidId();
 }
@@ -256,11 +293,11 @@ std::size_t EntityManagementFeatures::GetShapeCount(
 Identity EntityManagementFeatures::GetShape(
   const Identity &_linkID, const std::size_t _shapeIndex) const
 {
-  std::size_t shapeId = this->indexInContainerToId(_linkID.id, _shapeIndex);
-  auto it = this->collisions.find(shapeId);
-  if (it != this->collisions.end() && it->second != nullptr)
+  const auto &[shapeId, shapeInfo] =
+      this->indexInContainerToId(_linkID.id, _shapeIndex, this->collisions);
+  if (shapeInfo != nullptr)
   {
-    return this->GenerateIdentity(it->first, it->second);
+    return this->GenerateIdentity(shapeId, shapeInfo);
   }
   return this->GenerateInvalidId();
 }
@@ -328,8 +365,9 @@ bool EntityManagementFeatures::RemoveModelByIndex(
   auto worldInfo = this->ReferenceInterface<WorldInfo>(_worldID);
   if (worldInfo != nullptr)
   {
-    auto modelId = this->indexInContainerToId(_worldID.id, _modelIndex);
-    if (this->models.find(modelId) != this->models.end())
+    const auto [modelId, modelInfo] =
+        this->indexInContainerToId(_worldID.id, _modelIndex, this->models);
+    if (modelInfo != nullptr)
     {
       this->models.erase(modelId);
       this->childIdToParentId.erase(modelId);
@@ -406,6 +444,21 @@ Identity EntityManagementFeatures::ConstructEmptyModel(
   return this->GenerateInvalidId();
 }
 
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::ConstructEmptyNestedModel(
+  const Identity &_modelID, const std::string &_name)
+{
+  auto modelInfo = this->ReferenceInterface<ModelInfo>(_modelID);
+  if (modelInfo != nullptr)
+  {
+    auto &modelEnt = modelInfo->model->AddModel();
+    modelEnt.SetName(_name);
+    tpelib::Model *model = static_cast<tpelib::Model *>(&modelEnt);
+    return this->AddModel(_modelID.id, *model);
+  }
+  return this->GenerateInvalidId();
+}
+
 /////////////////////////////////////////////////
 Identity EntityManagementFeatures::ConstructEmptyLink(
   const Identity &_modelID, const std::string &_name)

tpe/plugin/src/EntityManagementFeatures.hh

@@ -36,11 +36,13 @@ struct EntityManagementFeatureList : FeatureList<
   GetEngineInfo,
   GetWorldFromEngine,
   GetModelFromWorld,
+  GetNestedModelFromModel,
   GetLinkFromModel,
   GetShapeFromLink,
   RemoveEntities,
   ConstructEmptyWorldFeature,
   ConstructEmptyModelFeature,
+  ConstructEmptyNestedModelFeature,
   ConstructEmptyLinkFeature,
   CollisionFilterMaskFeature
 > { };
@@ -87,6 +89,15 @@ class EntityManagementFeatures :
 
   public: Identity GetWorldOfModel(const Identity &_modelID) const override;
 
+  public: std::size_t GetNestedModelCount(
+    const Identity &_modelID) const override;
+
+  public: Identity GetNestedModel(
+    const Identity &_modelID, std::size_t _modelIndex) const override;
+
+  public: Identity GetNestedModel(
+    const Identity &_modelID, const std::string &_modelName) const override;
+
   public: std::size_t GetLinkCount(const Identity &_modelID) const override;
 
   public: Identity GetLink(
@@ -135,6 +146,9 @@ class EntityManagementFeatures :
   public: Identity ConstructEmptyModel(
     const Identity &_worldID, const std::string &_name) override;
 
+  public: Identity ConstructEmptyNestedModel(
+    const Identity &_modelID, const std::string &_name) override;
+
   public: Identity ConstructEmptyLink(
     const Identity &_modelID, const std::string &_name) override;
 

tpe/plugin/src/EntityManagement_TEST.cc

@@ -79,6 +79,19 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld)
   EXPECT_EQ(model, link->GetModel());
   EXPECT_EQ(link, model->GetLink(0));
   EXPECT_EQ(link, model->GetLink("empty link"));
+
+  EXPECT_EQ(0u, model->GetNestedModelCount());
+  auto nestedModel = model->ConstructEmptyNestedModel("empty nested model");
+  EXPECT_EQ(1u, model->GetNestedModelCount());
+  ASSERT_NE(nullptr, nestedModel);
+  EXPECT_EQ("empty nested model", nestedModel->GetName());
+  EXPECT_EQ(nestedModel, model->GetNestedModel("empty nested model"));
+  EXPECT_EQ(nestedModel, model->GetNestedModel(0));
+  EXPECT_EQ(0u, nestedModel->GetLinkCount());
+  ASSERT_NE(nullptr, nestedModel->ConstructEmptyLink("empty link"));
+  EXPECT_EQ(1u, nestedModel->GetLinkCount());
+
+  EXPECT_EQ(2u, model->GetLinkCount());
 }
 
 TEST(EntityManagement_TEST, RemoveEntities)