[tpe] Add empty nested model construction and nested model entity management

tpe/lib/src/Model.cc

@@ -16,6 +16,7 @@
 */
 
 #include <set>
+#include <algorithm>
 #include <string>
 
 #include <ignition/common/Profiler.hh>
@@ -34,6 +35,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 links
+  public: std::vector<std::size_t> nestedModelIds;
 };
 
 using namespace ignition;
@@ -64,29 +71,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/EntityManagementFeatures.cc

@@ -172,11 +172,54 @@ 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
+{
+  std::size_t modelId = this->indexInContainerToId(_modelID.id, _modelIndex);
+  auto it = this->models.find(modelId);
+  if (it != this->models.end() && it->second != nullptr)
+  {
+    return this->GenerateIdentity(modelId, it->second);
+  }
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetNestedModel(
+  const Identity &_modelID, const std::string &_modelName) const
+{
+  auto modelInfo = this->ReferenceInterface<WorldInfo>(_modelID);
+  if (modelInfo != nullptr)
+  {
+    tpelib::Entity &modelEnt = modelInfo->world->GetChildByName(_modelName);
+    for (const auto &[parentModelId, nestedModelInfo]: this->models)
+    {
+      if (nestedModelInfo != nullptr)
+      {
+        const std::string &name = nestedModelInfo->model->GetName();
+        if (parentModelId == modelEnt.GetId() && name == modelEnt.GetName())
+        {
+          return this->GenerateIdentity(parentModelId, nestedModelInfo);
+        }
+      }
+    }
+  }
+  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();
 }
 
 /////////////////////////////////////////////////
@@ -406,6 +449,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,17 @@ 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(0u, nestedModel->GetLinkCount());
+  ASSERT_NE(nullptr, nestedModel->ConstructEmptyLink("empty link"));
+  EXPECT_EQ(1u, nestedModel->GetLinkCount());
+
+  EXPECT_EQ(2u, model->GetLinkCount());
 }
 
 TEST(EntityManagement_TEST, RemoveEntities)