From f9641a51ed5b377dca5acfa42a285e71a41702b8 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 24 Mar 2021 22:11:43 -0500 Subject: [PATCH] Remove nested models (#230) This adds a new feature, RemoveNestedModelFromModel, that allow users to remove a nested model via it's parent model entity. This PR also ensures that when a parent model is removed, all its nested models are removed as well. Signed-off-by: Addisu Z. Taddese Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- dartsim/src/Base.hh | 72 ++++++++++-- dartsim/src/EntityManagementFeatures.cc | 103 +++++++++++++----- dartsim/src/EntityManagementFeatures.hh | 6 + dartsim/src/EntityManagement_TEST.cc | 74 +++++++++++++ dartsim/src/JointFeatures.cc | 5 +- dartsim/src/SDFFeatures.cc | 10 +- dartsim/src/SDFFeatures_TEST.cc | 1 - include/ignition/physics/RemoveEntities.hh | 36 +++++- .../ignition/physics/detail/RemoveEntities.hh | 20 ++++ tpe/lib/src/Model.cc | 59 ++++++++++ tpe/lib/src/Model.hh | 25 ++++- tpe/plugin/src/Base.hh | 55 +++++++++- tpe/plugin/src/EntityManagementFeatures.cc | 52 ++++++--- tpe/plugin/src/EntityManagementFeatures.hh | 6 + tpe/plugin/src/EntityManagement_TEST.cc | 39 ++++++- 15 files changed, 495 insertions(+), 68 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index b8f2feb70..1ae7a4e92 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -278,18 +278,16 @@ class Base : public Implements3d> const dart::simulation::WorldPtr &world = worlds[_worldID]; - const std::size_t indexInWorld = world->getNumSkeletons(); - this->models.idToIndexInContainer[id] = indexInWorld; std::vector &indexInContainerToID = this->models.indexInContainerToID[_worldID]; + const std::size_t indexInWorld = indexInContainerToID.size(); + this->models.idToIndexInContainer[id] = indexInWorld; indexInContainerToID.push_back(id); world->addSkeleton(entry.model); this->models.idToContainerID[id] = _worldID; this->frames[id] = _info.frame.get(); - assert(indexInContainerToID.size() == world->getNumSkeletons()); - return std::forward_as_tuple(id, entry); } @@ -297,8 +295,25 @@ class Base : public Implements3d> const ModelInfo &_info, const std::size_t _parentID, const std::size_t _worldID) { - auto [id, entry] = this->AddModel(_info, _worldID); - this->models.at(_parentID)->nestedModels.push_back(id); + const std::size_t id = this->GetNextEntity(); + this->models.idToObject[id] = std::make_shared(_info); + ModelInfo &entry = *this->models.idToObject[id]; + this->models.objectToID[_info.model] = id; + + const dart::simulation::WorldPtr &world = worlds[_worldID]; + + auto parentModelInfo = this->models.at(_parentID); + const std::size_t indexInModel = + parentModelInfo->nestedModels.size(); + this->models.idToIndexInContainer[id] = indexInModel; + std::vector &indexInContainerToID = + this->models.indexInContainerToID[_parentID]; + indexInContainerToID.push_back(id); + world->addSkeleton(entry.model); + + this->models.idToContainerID[id] = _parentID; + this->frames[id] = _info.frame.get(); + parentModelInfo->nestedModels.push_back(id); return {id, entry}; } @@ -352,13 +367,19 @@ class Base : public Implements3d> return id; } - public: void RemoveModelImpl(const std::size_t _worldID, + public: bool RemoveModelImpl(const std::size_t _worldID, const std::size_t _modelID) { - // TODO(addisu) Handle removal of nested models const auto &world = this->worlds.at(_worldID); - auto skel = this->models.at(_modelID)->model; + auto modelInfo = this->models.at(_modelID); + auto skel = modelInfo->model; // Remove the contents of the skeleton from local entity storage containers + for (auto &nestedModel : modelInfo->nestedModels) + { + this->RemoveModelImpl(_worldID, nestedModel); + } + modelInfo->nestedModels.clear(); + for (auto &jt : skel->getJoints()) { this->joints.RemoveEntity(jt); @@ -373,8 +394,41 @@ class Base : public Implements3d> this->linksByName.erase(::sdf::JoinName( world->getName(), ::sdf::JoinName(skel->getName(), bn->getName()))); } + + // If this is a nested model, remove an entry from the parent models + // "nestedModels" vector + auto parentID = this->models.idToContainerID.at(_modelID); + if (parentID != _worldID) + { + auto parentModelInfo = this->models.at(parentID); + const std::size_t modelIndex = + this->models.idToIndexInContainer.at(_modelID); + if (modelIndex >= parentModelInfo->nestedModels.size()) + return false; + parentModelInfo->nestedModels.erase( + parentModelInfo->nestedModels.begin() + modelIndex); + } this->models.RemoveEntity(skel); world->removeSkeleton(skel); + return true; + } + + public: inline std::size_t GetWorldOfModelImpl( + const std::size_t &_modelID) const + { + if (this->models.HasEntity(_modelID)) + { + auto parentIt = this->models.idToContainerID.find(_modelID); + if (parentIt != this->models.idToContainerID.end()) + { + if (this->worlds.HasEntity(parentIt->second)) + { + return parentIt->second; + } + return this->GetWorldOfModelImpl(parentIt->second); + } + } + return this->GenerateInvalidId(); } public: EntityStorage worlds; diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index a85bfe6ab..57a8a5ac6 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -99,6 +99,10 @@ class BitmaskContactFilter : public dart::collision::BodyNodeCollisionFilter /// Utility functions ///////////////////////////////////////////////// +/// TODO (addisu): There's a lot of code duplication in model removal code, such +/// as RemoveModelByIndex, where we call GetFilterPtr followed by +/// RemoveSkeletonCollisions(model). To de-duplicate this, move this logic into +/// RemoveModelImpl. To do that, we need to move GetFilterPtr into Base.hh. static const std::shared_ptr GetFilterPtr( const EntityManagementFeatures* _emf, std::size_t _worldID) { @@ -121,7 +125,7 @@ static std::size_t GetWorldOfShapeNode(const EntityManagementFeatures *_emf, // Now find the skeleton's model const std::size_t modelID = _emf->models.objectToID.at(skelPtr); // And the world containing the model - return _emf->models.idToContainerID.at(modelID); + return _emf->GetWorldOfModelImpl(modelID); } ///////////////////////////////////////////////// @@ -198,14 +202,19 @@ std::size_t EntityManagementFeatures::GetModelCount( Identity EntityManagementFeatures::GetModel( const Identity &_worldID, const std::size_t _modelIndex) const { - const DartSkeletonPtr &model = - this->ReferenceInterface(_worldID)->getSkeleton(_modelIndex); + const auto &indexInContainerToID = + this->models.indexInContainerToID.at(_worldID); + + if (_modelIndex >= indexInContainerToID.size()) + { + return this->GenerateInvalidId(); + } + const std::size_t modelID = indexInContainerToID[_modelIndex]; // If the model doesn't exist in "models", it means the containing entity has // been removed. - if (this->models.HasEntity(model)) + if (this->models.HasEntity(modelID)) { - const std::size_t modelID = this->models.IdentityOf(model); return this->GenerateIdentity(modelID, this->models.at(modelID)); } else @@ -255,16 +264,12 @@ std::size_t EntityManagementFeatures::GetModelIndex( Identity EntityManagementFeatures::GetWorldOfModel( const Identity &_modelID) const { - // If the model doesn't exist in "models", it it has been removed. - if (this->models.HasEntity(_modelID)) + auto worldID = this->GetWorldOfModelImpl(_modelID); + if (worldID != INVALID_ENTITY_ID ) { - const std::size_t worldID = this->models.idToContainerID.at(_modelID); return this->GenerateIdentity(worldID, this->worlds.at(worldID)); } - else - { - return this->GenerateInvalidId(); - } + return this->GenerateInvalidId(); } ///////////////////////////////////////////////// @@ -310,7 +315,7 @@ Identity EntityManagementFeatures::GetNestedModel( if (this->models.HasEntity(_modelID)) { - auto worldID = this->models.idToContainerID.at(_modelID); + auto worldID = this->GetWorldOfModelImpl(_modelID); auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName); if (nullptr == nestedSkel) { @@ -620,8 +625,7 @@ bool EntityManagementFeatures::RemoveModelByIndex(const Identity &_worldID, { auto filterPtr = GetFilterPtr(this, _worldID); filterPtr->RemoveSkeletonCollisions(model); - this->RemoveModelImpl(_worldID, this->models.IdentityOf(model)); - return true; + return this->RemoveModelImpl(_worldID, this->models.IdentityOf(model)); } return false; } @@ -637,8 +641,7 @@ bool EntityManagementFeatures::RemoveModelByName(const Identity &_worldID, { auto filterPtr = GetFilterPtr(this, _worldID); filterPtr->RemoveSkeletonCollisions(model); - this->RemoveModelImpl(_worldID, this->models.IdentityOf(model)); - return true; + return this->RemoveModelImpl(_worldID, this->models.IdentityOf(model)); } return false; } @@ -648,13 +651,13 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) { if (this->models.HasEntity(_modelID)) { - auto worldID = this->models.idToContainerID.at(_modelID); + auto worldID = this->GetWorldOfModelImpl(_modelID); auto model = this->models.at(_modelID)->model; auto filterPtr = GetFilterPtr(this, worldID); filterPtr->RemoveSkeletonCollisions(model); - this->RemoveModelImpl(this->models.idToContainerID.at(_modelID), _modelID); - return true; + + return this->RemoveModelImpl(worldID, _modelID); } return false; } @@ -665,6 +668,50 @@ bool EntityManagementFeatures::ModelRemoved(const Identity &_modelID) const return !this->models.HasEntity(_modelID); } +///////////////////////////////////////////////// +bool EntityManagementFeatures::RemoveNestedModelByIndex( + const Identity &_modelID, std::size_t _nestedModelIndex) +{ + auto modelInfo = this->ReferenceInterface(_modelID); + if (_nestedModelIndex >= modelInfo->nestedModels.size()) + { + return this->GenerateInvalidId(); + } + const auto nestedModelID = modelInfo->nestedModels[_nestedModelIndex]; + if (this->models.HasEntity(nestedModelID)) + { + const auto worldID = this->GetWorldOfModelImpl(nestedModelID); + const auto model = this->models.at(nestedModelID)->model; + const auto filterPtr = GetFilterPtr(this, worldID); + filterPtr->RemoveSkeletonCollisions(model); + return this->RemoveModelImpl(worldID, nestedModelID); + } + return false; +} + +///////////////////////////////////////////////// +bool EntityManagementFeatures::RemoveNestedModelByName(const Identity &_modelID, + const std::string &_modelName) +{ + auto modelInfo = this->ReferenceInterface(_modelID); + const std::string fullName = + ::sdf::JoinName(modelInfo->model->getName(), _modelName); + + if (this->models.HasEntity(_modelID)) + { + auto worldID = this->GetWorldOfModelImpl(_modelID); + auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName); + if (nullptr == nestedSkel || !this->models.HasEntity(nestedSkel)) + { + return false; + } + const std::size_t nestedModelID = this->models.IdentityOf(nestedSkel); + const auto filterPtr = GetFilterPtr(this, worldID); + filterPtr->RemoveSkeletonCollisions(nestedSkel); + return this->RemoveModelImpl(worldID, nestedModelID); + } + return false; +} ///////////////////////////////////////////////// Identity EntityManagementFeatures::ConstructEmptyWorld( const Identity &/*_engineID*/, const std::string &_name) @@ -695,7 +742,7 @@ Identity EntityManagementFeatures::ConstructEmptyModel( dart::dynamics::Frame::World(), _name + "_frame"); - auto [modelID, modelInfo] = + const auto [modelID, modelInfo] = this->AddModel({model, _name, modelFrame, ""}, _worldID); // NOLINT return this->GenerateIdentity(modelID, this->models.at(modelID)); @@ -703,11 +750,11 @@ Identity EntityManagementFeatures::ConstructEmptyModel( ///////////////////////////////////////////////// Identity EntityManagementFeatures::ConstructEmptyNestedModel( - const Identity &_modelID, const std::string &_name) + const Identity &_parentModelID, const std::string &_name) { // find the world assocated with the model - auto worldID = this->models.idToContainerID.at(_modelID); - const auto &skel = this->models.at(_modelID)->model; + auto worldID = this->GetWorldOfModelImpl(_parentModelID); + const auto &skel = this->models.at(_parentModelID)->model; const std::string modelFullName = ::sdf::JoinName(skel->getName(), _name); dart::dynamics::SkeletonPtr model = @@ -719,7 +766,7 @@ Identity EntityManagementFeatures::ConstructEmptyNestedModel( modelFullName + "_frame"); auto [modelID, modelInfo] = this->AddNestedModel( - {model, _name, modelFrame, ""}, _modelID, worldID); // NOLINT + {model, _name, modelFrame, ""}, _parentModelID, worldID); // NOLINT return this->GenerateIdentity(modelID, this->models.at(modelID)); } @@ -740,8 +787,8 @@ Identity EntityManagementFeatures::ConstructEmptyLink( model->createJointAndBodyNodePair( nullptr, prop_fj, prop_bn).second; - auto worldIDIt = this->models.idToContainerID.find(_modelID); - if (worldIDIt == this->models.idToContainerID.end()) + auto worldID = this->GetWorldOfModelImpl(_modelID); + if (worldID == INVALID_ENTITY_ID) { ignerr << "World of model [" << model->getName() << "] could not be found when creating link [" << _name @@ -749,7 +796,7 @@ Identity EntityManagementFeatures::ConstructEmptyLink( return this->GenerateInvalidId(); } - auto world = this->worlds.at(worldIDIt->second); + auto world = this->worlds.at(worldID); const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(model->getName(), bn->getName())); diff --git a/dartsim/src/EntityManagementFeatures.hh b/dartsim/src/EntityManagementFeatures.hh index 370f334c7..db2f29bd1 100644 --- a/dartsim/src/EntityManagementFeatures.hh +++ b/dartsim/src/EntityManagementFeatures.hh @@ -147,6 +147,12 @@ class EntityManagementFeatures : public: bool ModelRemoved(const Identity &_modelID) const override; + public: bool RemoveNestedModelByIndex( + const Identity &_modelID, std::size_t _modelIndex) override; + + public: bool RemoveNestedModelByName( + const Identity &_modelID, const std::string &_modelName) override; + // ----- Construct empty entities ----- public: Identity ConstructEmptyWorld( const Identity &_engineID, const std::string &_name) override; diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index 9cd49f412..39864026e 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -237,8 +237,82 @@ TEST(EntityManagement_TEST, RemoveEntities) EXPECT_EQ(0ul, model2->GetIndex()); world->RemoveModel(0); EXPECT_EQ(0ul, world->GetModelCount()); + + auto parentModel = world->ConstructEmptyModel("parent model"); + ASSERT_NE(nullptr, parentModel); + EXPECT_EQ(0u, parentModel->GetNestedModelCount()); + auto nestedModel1 = + parentModel->ConstructEmptyNestedModel("empty nested model1"); + ASSERT_NE(nullptr, nestedModel1); + EXPECT_EQ(1u, parentModel->GetNestedModelCount()); + + EXPECT_TRUE(parentModel->RemoveNestedModel(0)); + EXPECT_EQ(0u, parentModel->GetNestedModelCount()); + EXPECT_TRUE(nestedModel1->Removed()); + + auto nestedModel2 = + parentModel->ConstructEmptyNestedModel("empty nested model2"); + ASSERT_NE(nullptr, nestedModel2); + EXPECT_EQ(nestedModel2, parentModel->GetNestedModel(0)); + EXPECT_TRUE(parentModel->RemoveNestedModel("empty nested model2")); + EXPECT_EQ(0u, parentModel->GetNestedModelCount()); + EXPECT_TRUE(nestedModel2->Removed()); + + auto nestedModel3 = + parentModel->ConstructEmptyNestedModel("empty nested model3"); + ASSERT_NE(nullptr, nestedModel3); + EXPECT_EQ(nestedModel3, parentModel->GetNestedModel(0)); + EXPECT_TRUE(nestedModel3->Remove()); + EXPECT_EQ(0u, parentModel->GetNestedModelCount()); + EXPECT_TRUE(nestedModel3->Removed()); + + auto nestedModel4 = + parentModel->ConstructEmptyNestedModel("empty nested model4"); + ASSERT_NE(nullptr, nestedModel4); + EXPECT_EQ(nestedModel4, parentModel->GetNestedModel(0)); + // Remove the parent model and check that the nested model is removed as well + EXPECT_TRUE(parentModel->Remove()); + EXPECT_TRUE(nestedModel4->Removed()); } +TEST(EntityManagement_TEST, ModelByIndexWithNestedModels) +{ + ignition::plugin::Loader loader; + loader.LoadLib(dartsim_plugin_LIB); + + ignition::plugin::PluginPtr dartsim = + loader.Instantiate("ignition::physics::dartsim::Plugin"); + + auto engine = + ignition::physics::RequestEngine3d::From(dartsim); + ASSERT_NE(nullptr, engine); + + auto world = engine->ConstructEmptyWorld("empty world"); + ASSERT_NE(nullptr, world); + auto model1 = world->ConstructEmptyModel("model1"); + ASSERT_NE(nullptr, model1); + EXPECT_EQ(0ul, model1->GetIndex()); + + auto parentModel = world->ConstructEmptyModel("parent model"); + ASSERT_NE(nullptr, parentModel); + EXPECT_EQ(1ul, parentModel->GetIndex()); + + auto nestedModel1 = + parentModel->ConstructEmptyNestedModel("empty nested model1"); + ASSERT_NE(nullptr, nestedModel1); + EXPECT_EQ(0ul, nestedModel1->GetIndex()); + + auto model2 = world->ConstructEmptyModel("model2"); + ASSERT_NE(nullptr, model2); + EXPECT_EQ(2ul, model2->GetIndex()); + EXPECT_TRUE(model2->Remove()); + + auto model2Again = world->ConstructEmptyModel("model2_again"); + ASSERT_NE(nullptr, model2Again); + EXPECT_EQ(2ul, model2Again->GetIndex()); +} + + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index f1ea1b2cd..d19884e5f 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -238,9 +238,8 @@ void JointFeatures::DetachJoint(const Identity &_jointId) { // Assume that the original and the current skeletons are in the same // world. - auto worldId = - this->models - .idToContainerID[this->models.IdentityOf(joint->getSkeleton())]; + auto worldId = this->GetWorldOfModelImpl( + this->models.IdentityOf(joint->getSkeleton())); auto dartWorld = this->worlds.at(worldId); std::string modelName = oldName.substr(0, originalNameIndex - 1); skeleton = dartWorld->getSkeleton(modelName); diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 89edcfef3..8baff95aa 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -441,7 +441,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( // If this is a nested model, find the world assocated with the model if (isNested) { - worldID = this->models.idToContainerID.at(_parentID); + worldID = this->GetWorldOfModelImpl(_parentID); const auto &skel = this->models.at(_parentID)->model; modelName = ::sdf::JoinName(skel->getName(), _sdfModel.Name()); } @@ -628,8 +628,8 @@ Identity SDFFeatures::ConstructSdfLink( dart::dynamics::BodyNode * const bn = result.second; - auto worldIDIt = this->models.idToContainerID.find(_modelID); - if (worldIDIt == this->models.idToContainerID.end()) + auto worldID = this->GetWorldOfModelImpl(_modelID); + if (worldID == INVALID_ENTITY_ID) { ignerr << "World of model [" << modelInfo.model->getName() << "] could not be found when creating link [" << _sdfLink.Name() @@ -637,7 +637,7 @@ Identity SDFFeatures::ConstructSdfLink( return this->GenerateInvalidId(); } - auto world = this->worlds.at(worldIDIt->second); + auto world = this->worlds.at(worldID); const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(modelInfo.model->getName(), bn->getName())); @@ -714,7 +714,7 @@ Identity SDFFeatures::ConstructSdfJoint( // name to identify the correct parent skeleton. If the corresponding body // node is found, an error will not be printed. // - const std::size_t worldID = this->models.idToContainerID.at(_modelID); + const std::size_t worldID = this->GetWorldOfModelImpl(_modelID); auto & world = this->worlds.at(worldID); std::string parentLinkName; diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index ce5f5438d..4d25c84fe 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -545,7 +545,6 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModel) // check top level model EXPECT_EQ("parent_model", world->GetModel(0)->GetName()); - EXPECT_EQ("nested_model", world->GetModel(1)->GetName()); auto parentModel = world->GetModel("parent_model"); ASSERT_NE(nullptr, parentModel); diff --git a/include/ignition/physics/RemoveEntities.hh b/include/ignition/physics/RemoveEntities.hh index 8474b5bba..71821d502 100644 --- a/include/ignition/physics/RemoveEntities.hh +++ b/include/ignition/physics/RemoveEntities.hh @@ -76,8 +76,42 @@ namespace ignition }; }; + ///////////////////////////////////////////////// + /// \brief This feature removes a nested Model entity from the + /// specified parent Model. + class IGNITION_PHYSICS_VISIBLE RemoveNestedModelFromModel + : public virtual Feature + { + public: template + class Model : public virtual Feature::Model + { + /// \brief Remove a Model that is nested within this Model. + /// \param[in] _index + /// Index of the model within this world. + /// \return True if the model was found and removed. + public: bool RemoveNestedModel(std::size_t _index); + + /// \brief Remove a Model that is nested within this Model. + /// \param[in] _name + /// Name of the model within this world. + /// \return True if the model was found and removed. + public: bool RemoveNestedModel(const std::string &_name); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual bool RemoveNestedModelByIndex( + const Identity &_modelID, std::size_t _modelIndex) = 0; + + public: virtual bool RemoveNestedModelByName( + const Identity &_modelID, const std::string &_modelName) = 0; + }; + }; + using RemoveEntities = FeatureList< - RemoveModelFromWorld + RemoveModelFromWorld, + RemoveNestedModelFromModel >; } } diff --git a/include/ignition/physics/detail/RemoveEntities.hh b/include/ignition/physics/detail/RemoveEntities.hh index 6dcc4d857..88fe4d45c 100644 --- a/include/ignition/physics/detail/RemoveEntities.hh +++ b/include/ignition/physics/detail/RemoveEntities.hh @@ -59,6 +59,26 @@ namespace ignition return this->template Interface() ->ModelRemoved(this->identity); } + + ///////////////////////////////////////////////// + template + bool + RemoveNestedModelFromModel::Model::RemoveNestedModel( + const std::size_t _index) + { + return this->template Interface() + ->RemoveNestedModelByIndex(this->identity, _index); + } + + ///////////////////////////////////////////////// + template + bool + RemoveNestedModelFromModel::Model::RemoveNestedModel( + const std::string &_name) + { + return this->template Interface() + ->RemoveNestedModelByName(this->identity, _name); + } } } diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc index 813db13c5..53d82287c 100644 --- a/tpe/lib/src/Model.cc +++ b/tpe/lib/src/Model.cc @@ -190,3 +190,62 @@ void Model::UpdatePose( currentPose.Rot().Integrate(_angularVelocity, _timeStep)); this->SetPose(nextPose); } + +////////////////////////////////////////////////// +bool Model::RemoveModelById(std::size_t _id) +{ + auto it = std::find(this->dataPtr->nestedModelIds.begin(), + this->dataPtr->nestedModelIds.end(), _id); + if (it != this->dataPtr->nestedModelIds.end()) + { + this->dataPtr->nestedModelIds.erase(it); + return true; + } + return false; +} + +////////////////////////////////////////////////// +bool Model::RemoveLinkById(std::size_t _id) +{ + auto it = std::find(this->dataPtr->linkIds.begin(), + this->dataPtr->linkIds.end(), _id); + if (it != this->dataPtr->linkIds.end()) + { + this->dataPtr->linkIds.erase(it); + return true; + } + return false; +} + +////////////////////////////////////////////////// +bool Model::RemoveChildById(std::size_t _id) +{ + Entity &ent = this->GetChildById(_id); + return this->RemoveChildEntityBasedOnType(&ent); +} + +////////////////////////////////////////////////// +bool Model::RemoveChildByName(const std::string &_name) +{ + Entity &ent = this->GetChildByName(_name); + return this->RemoveChildEntityBasedOnType(&ent); +} + +////////////////////////////////////////////////// +bool Model::RemoveChildEntityBasedOnType(const Entity *_ent) +{ + if (nullptr == _ent) + return false; + + bool result = true; + if (nullptr != dynamic_cast(_ent)) + { + result &= this->RemoveModelById(_ent->GetId()); + } + else + { + result &= this->RemoveLinkById(_ent->GetId()); + } + result &= Entity::RemoveChildById(_ent->GetId()); + return result; +} diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh index 3639b58dc..9b6cc6e8a 100644 --- a/tpe/lib/src/Model.hh +++ b/tpe/lib/src/Model.hh @@ -57,7 +57,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity public: Entity &AddModel(); /// \brief Get the number of nested models in the model - /// \return Number of nested mdoels in the model + /// \return Number of nested models in the model public: std::size_t GetModelCount() const; /// \brief Set the canonical link of model @@ -93,6 +93,21 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity const math::Vector3d _linearVelocity, const math::Vector3d _angularVelocity); + /// \brief Removes a child entity (either a link or model) from the + /// appropriate child entity containers + /// \param[in] _ent Pointer to entity + /// \return True if the entity was found and removed + public: bool RemoveChildEntityBasedOnType(const Entity *_ent); + /// \brief Remove a child entity by id + /// \param[in] _id Id of child entity to remove + /// \return True if the entity was found and removed + public: bool RemoveChildById(std::size_t _id) override; + + /// \brief Remove a child entity by name + /// \param[in] _name Name of child entity to remove + /// \return True if child entity was removed, false otherwise + public: bool RemoveChildByName(const std::string &_name) override; + IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief linear velocity of model protected: math::Vector3d linearVelocity; @@ -101,6 +116,14 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity protected: math::Vector3d angularVelocity; IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING + /// \brief Remove a model entity by id + /// \param[in] _id Id of model entity to remove + private: bool RemoveModelById(std::size_t _id); + + /// \brief Remove a link entity by id + /// \param[in] _id Id of link entity to remove + private: bool RemoveLinkById(std::size_t _id); + /// \brief Pointer to private data class private: ModelPrivate *dataPtr = nullptr; }; diff --git a/tpe/plugin/src/Base.hh b/tpe/plugin/src/Base.hh index 6b9e58eb6..c509a7218 100644 --- a/tpe/plugin/src/Base.hh +++ b/tpe/plugin/src/Base.hh @@ -122,7 +122,6 @@ class Base : public Implements3d> return {INVALID_ENTITY_ID, nullptr}; } - public: inline Identity AddWorld(std::shared_ptr _world) { size_t worldId = _world->GetId(); @@ -170,6 +169,60 @@ class Base : public Implements3d> return this->GenerateIdentity(collisionId, collisionPtr); } + public: bool RemoveModelImpl(std::size_t _modelID) + { + auto parentIt = this->childIdToParentId.find(_modelID); + if (parentIt == this->childIdToParentId.end()) + return false; + + auto modelInfoIt = this->models.find(_modelID); + if (modelInfoIt == this->models.end()) + return false; + + tpelib::Entity *parentEntity = nullptr; + auto worldIt = this->worlds.find(parentIt->second); + if (worldIt != this->worlds.end()) + parentEntity = worldIt->second->world.get(); + + if (nullptr == parentEntity) + { + // If the parent entity is not a world, try a parent model of a nested + // model + auto modelIt = this->models.find(parentIt->second); + if (modelIt != this->models.end()) + parentEntity = modelIt->second->model; + } + + if (nullptr == parentEntity) + return false; + + bool result = true; + for (std::size_t i = 0; i < modelInfoIt->second->model->GetChildCount(); + ++i) + { + // Get a reference so we can dynamic cast + auto &ent = modelInfoIt->second->model->GetChildByIndex(i); + auto modelEnt = dynamic_cast(&ent); + if (modelEnt) + { + result &= this->RemoveModelImpl(modelEnt->GetId()); + } + } + result &= this->RemoveModelFromParent(_modelID, parentEntity); + return result; + } + + public: bool RemoveModelFromParent(std::size_t _modelID, + tpelib::Entity *_parentEntity) + { + if (nullptr == _parentEntity) + return false; + bool result = this->models.erase(_modelID) == 1; + result &= this->childIdToParentId.erase(_modelID) == 1; + result &= _parentEntity->RemoveChildById(_modelID); + return result; + } + public: std::map> worlds; public: std::map> models; public: std::map> links; diff --git a/tpe/plugin/src/EntityManagementFeatures.cc b/tpe/plugin/src/EntityManagementFeatures.cc index a69a6a5f2..e845d1ea4 100644 --- a/tpe/plugin/src/EntityManagementFeatures.cc +++ b/tpe/plugin/src/EntityManagementFeatures.cc @@ -369,9 +369,7 @@ bool EntityManagementFeatures::RemoveModelByIndex( this->indexInContainerToId(_worldID.id, _modelIndex, this->models); if (modelInfo != nullptr) { - this->models.erase(modelId); - this->childIdToParentId.erase(modelId); - return worldInfo->world->RemoveChildById(modelId); + this->RemoveModelImpl(modelId); } } return false; @@ -386,9 +384,7 @@ bool EntityManagementFeatures::RemoveModelByName( { std::size_t modelId = worldInfo->world->GetChildByName(_modelName).GetId(); - this->models.erase(modelId); - this->childIdToParentId.erase(modelId); - return worldInfo->world->RemoveChildById(modelId); + this->RemoveModelImpl(modelId); } return false; } @@ -396,18 +392,7 @@ bool EntityManagementFeatures::RemoveModelByName( ///////////////////////////////////////////////// bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) { - auto it = this->childIdToParentId.find(_modelID.id); - if (it != this->childIdToParentId.end()) - { - auto worldIt = this->worlds.find(it->second); - if (worldIt != this->worlds.end() && worldIt->second != nullptr) - { - this->models.erase(_modelID.id); - this->childIdToParentId.erase(_modelID.id); - return worldIt->second->world->RemoveChildById(_modelID.id); - } - } - return false; + return this->RemoveModelImpl(_modelID.id); } ///////////////////////////////////////////////// @@ -420,6 +405,37 @@ bool EntityManagementFeatures::ModelRemoved(const Identity &_modelID) const return false; } +///////////////////////////////////////////////// +bool EntityManagementFeatures::RemoveNestedModelByIndex( + const Identity &_modelID, std::size_t _modelIndex) +{ + auto modelInfo = this->ReferenceInterface(_modelID); + if (modelInfo != nullptr) + { + const auto &[nestedModelId, nestedModelInfo] = + this->indexInContainerToId(_modelID.id, _modelIndex, this->models); + if (nestedModelInfo != nullptr) + { + return this->RemoveModelFromParent(nestedModelId, modelInfo->model); + } + } + return false; +} + +///////////////////////////////////////////////// +bool EntityManagementFeatures::RemoveNestedModelByName( + const Identity &_modelID, const std::string &_modelName) +{ + auto modelInfo = this->ReferenceInterface(_modelID); + if (modelInfo != nullptr) + { + std::size_t nestedModelId = + modelInfo->model->GetChildByName(_modelName).GetId(); + return this->RemoveModelFromParent(nestedModelId, modelInfo->model); + } + return false; +} + ///////////////////////////////////////////////// Identity EntityManagementFeatures::ConstructEmptyWorld( const Identity &, const std::string &_name) diff --git a/tpe/plugin/src/EntityManagementFeatures.hh b/tpe/plugin/src/EntityManagementFeatures.hh index 3e5075349..34e3ae3d2 100644 --- a/tpe/plugin/src/EntityManagementFeatures.hh +++ b/tpe/plugin/src/EntityManagementFeatures.hh @@ -139,6 +139,12 @@ class EntityManagementFeatures : public: bool ModelRemoved(const Identity &_modelID) const override; + public: bool RemoveNestedModelByIndex( + const Identity &_modelId, std::size_t _modelIndex) override; + + public: bool RemoveNestedModelByName( + const Identity &_modelId, const std::string &_modelName) override; + // ----- Construct empty entities ----- public: Identity ConstructEmptyWorld( const Identity &_engineID, const std::string &_name) override; diff --git a/tpe/plugin/src/EntityManagement_TEST.cc b/tpe/plugin/src/EntityManagement_TEST.cc index 8dccad7d9..3f1289416 100644 --- a/tpe/plugin/src/EntityManagement_TEST.cc +++ b/tpe/plugin/src/EntityManagement_TEST.cc @@ -84,6 +84,8 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto nestedModel = model->ConstructEmptyNestedModel("empty nested model"); EXPECT_EQ(1u, model->GetNestedModelCount()); ASSERT_NE(nullptr, nestedModel); + EXPECT_EQ(nestedModel, model->GetNestedModel(0)); + EXPECT_EQ(nestedModel, model->GetNestedModel("empty nested model")); EXPECT_EQ("empty nested model", nestedModel->GetName()); EXPECT_EQ(nestedModel, model->GetNestedModel("empty nested model")); EXPECT_EQ(nestedModel, model->GetNestedModel(0)); @@ -110,7 +112,6 @@ TEST(EntityManagement_TEST, RemoveEntities) ASSERT_NE(nullptr, world); auto model = world->ConstructEmptyModel("empty model"); ASSERT_NE(nullptr, model); - auto modelAlias = world->GetModel(0); model->Remove(); @@ -132,6 +133,42 @@ TEST(EntityManagement_TEST, RemoveEntities) world->RemoveModel("model 3"); EXPECT_EQ(0ul, world->GetModelCount()); EXPECT_EQ(nullptr, world->GetModel("model 3")); + + auto parentModel = world->ConstructEmptyModel("parent model"); + ASSERT_NE(nullptr, parentModel); + EXPECT_EQ(0u, parentModel->GetNestedModelCount()); + auto nestedModel1 = + parentModel->ConstructEmptyNestedModel("empty nested model1"); + ASSERT_NE(nullptr, nestedModel1); + EXPECT_EQ(1u, parentModel->GetNestedModelCount()); + + EXPECT_TRUE(parentModel->RemoveNestedModel(0)); + EXPECT_EQ(0u, parentModel->GetNestedModelCount()); + EXPECT_TRUE(nestedModel1->Removed()); + + auto nestedModel2 = + parentModel->ConstructEmptyNestedModel("empty nested model2"); + ASSERT_NE(nullptr, nestedModel2); + EXPECT_EQ(nestedModel2, parentModel->GetNestedModel(0)); + EXPECT_TRUE(parentModel->RemoveNestedModel("empty nested model2")); + EXPECT_EQ(0u, parentModel->GetNestedModelCount()); + EXPECT_TRUE(nestedModel2->Removed()); + + auto nestedModel3 = + parentModel->ConstructEmptyNestedModel("empty nested model3"); + ASSERT_NE(nullptr, nestedModel3); + EXPECT_EQ(nestedModel3, parentModel->GetNestedModel(0)); + EXPECT_TRUE(nestedModel3->Remove()); + EXPECT_EQ(0u, parentModel->GetNestedModelCount()); + EXPECT_TRUE(nestedModel3->Removed()); + + auto nestedModel4 = + parentModel->ConstructEmptyNestedModel("empty nested model4"); + ASSERT_NE(nullptr, nestedModel4); + EXPECT_EQ(nestedModel4, parentModel->GetNestedModel(0)); + // Remove the parent model and check that the nested model is removed as well + EXPECT_TRUE(parentModel->Remove()); + EXPECT_TRUE(nestedModel4->Removed()); } int main(int argc, char *argv[])