diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h b/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h index 6c8958001..636446314 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h @@ -149,9 +149,7 @@ namespace scenario::gazebo::utils { sdf::World renameSDFWorld(const sdf::World& world, const std::string& newWorldName); - bool renameSDFModel(sdf::Root& sdfRoot, - const std::string& newModelName, - const size_t modelIndex = 0); + bool renameSDFModel(sdf::Root& sdfRoot, const std::string& newModelName); bool updateSDFPhysics(sdf::Root& sdfRoot, const double maxStepSize, diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/utils.h b/cpp/scenario/gazebo/include/scenario/gazebo/utils.h index 008e1c5d4..19864955d 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/utils.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/utils.h @@ -112,15 +112,15 @@ namespace scenario::gazebo::utils { /** * Get the name of a model from a SDF file. * + * @note sdformat supports only one model per SDF file. + * * @param fileName An SDF file. It could be either an absolute path * to the file or the file name if the parent folder is part * of the ``IGN_GAZEBO_RESOURCE_PATH`` environment variable. - * @param modelIndex The index of the model in the SDF file. By - * default it finds the first model. - * @return The name of the model. + * @return The name of the model if the file was found and is valid, + * an empty string otherwise. */ - std::string getModelNameFromSdf(const std::string& fileName, - const size_t modelIndex = 0); + std::string getModelNameFromSdf(const std::string& fileName); /** * Get the name of a world from a SDF file. @@ -130,7 +130,8 @@ namespace scenario::gazebo::utils { * of the ``IGN_GAZEBO_RESOURCE_PATH`` environment variable. * @param worldIndex The index of the world in the SDF file. By * default it finds the first world. - * @return The name of the world. + * @return The name of the world if the file was found and is valid, + * an empty string otherwise. */ std::string getWorldNameFromSdf(const std::string& fileName, const size_t worldIndex = 0); diff --git a/cpp/scenario/gazebo/src/World.cpp b/cpp/scenario/gazebo/src/World.cpp index db7ca5b86..1e866232f 100644 --- a/cpp/scenario/gazebo/src/World.cpp +++ b/cpp/scenario/gazebo/src/World.cpp @@ -72,23 +72,16 @@ class World::Impl const std::string& overrideModelName, World& world) { - if (modelSdfRoot->ModelCount() != 1) { - sError << "The SDF file contains more than one model" << std::endl; - return false; - } - - constexpr size_t ModelIndex = 0; + // NOTE: sdf::Root objects could only contain one sdf::Model starting + // from sdformat11. - // Every SDF model has a name. In order to insert multiple models from - // the same SDF file, the modelData struct allows providing a scoped - // name. + // Name of the model to insert (allowing renaming from SDF) std::string finalModelEntityName; // Get the final name of the model if (overrideModelName.empty()) { - assert(modelSdfRoot->ModelByIndex(ModelIndex)); - finalModelEntityName = - modelSdfRoot->ModelByIndex(ModelIndex)->Name(); + assert(modelSdfRoot->Model()); + finalModelEntityName = modelSdfRoot->Model()->Name(); } else { finalModelEntityName = overrideModelName; @@ -107,17 +100,15 @@ class World::Impl // Rename the model. // NOTE: The following is not enough because the name is not serialized - // to - // string. We need also to operate directly on the raw element. - const_cast(modelSdfRoot->ModelByIndex(ModelIndex)) + // to string. We need also to operate directly on the raw element. + const_cast(modelSdfRoot->Model()) ->SetName(finalModelEntityName); // Update the name in the sdf model. This is necessary because model // plugins are loaded right before the creation of the model entity and, // instead of receiving the model entity name, they receive the model // sdf name. - if (!utils::renameSDFModel( - *modelSdfRoot, finalModelEntityName, ModelIndex)) { + if (!utils::renameSDFModel(*modelSdfRoot, finalModelEntityName)) { sError << "Failed to rename SDF model" << std::endl; return false; } @@ -128,9 +119,8 @@ class World::Impl } // Create the model entity - ignition::gazebo::Entity modelEntity; - modelEntity = this->sdfEntityCreator->CreateEntities( - modelSdfRoot->ModelByIndex(ModelIndex)); + const ignition::gazebo::Entity modelEntity = + this->sdfEntityCreator->CreateEntities(modelSdfRoot->Model()); // Attach the model entity to the world entity this->sdfEntityCreator->SetParent(modelEntity, world.m_entity); @@ -138,8 +128,7 @@ class World::Impl { // Check that the model name is correct assert(modelSdfRoot->ModelCount() == 1); - std::string modelNameSDF = - modelSdfRoot->ModelByIndex(ModelIndex)->Name(); + std::string modelNameSDF = modelSdfRoot->Model()->Name(); std::string modelNameEntity = utils::getExistingComponentData< // ignition::gazebo::components::Name>(world.m_ecm, modelEntity); assert(modelNameSDF == modelNameEntity); @@ -170,7 +159,7 @@ class World::Impl // We directly override the Pose component instead of using // Model::resetBasePose because it would just store a pose command that // needs to be processed by the Physics system. Overriding the - // component, instead, has direct effect. + // component, instead, has instantaneous effect. if (pose != core::Pose::Identity()) { utils::setComponentData( world.m_ecm, modelEntity, utils::toIgnitionPose(pose)); @@ -409,8 +398,7 @@ bool World::insertModelFromFile(const std::string& path, return false; } - return pImpl.get()->insertModel( - modelSdfRoot, pose, overrideModelName, *this); + return pImpl->insertModel(modelSdfRoot, pose, overrideModelName, *this); } bool World::insertModelFromString(const std::string& sdfString, diff --git a/cpp/scenario/gazebo/src/helpers.cpp b/cpp/scenario/gazebo/src/helpers.cpp index 22ea23b27..843237151 100644 --- a/cpp/scenario/gazebo/src/helpers.cpp +++ b/cpp/scenario/gazebo/src/helpers.cpp @@ -320,20 +320,20 @@ sdf::World utils::renameSDFWorld(const sdf::World& world, return renamedWorld; } -bool utils::renameSDFModel(sdf::Root& sdfRoot, - const std::string& newModelName, - const size_t modelIndex) +bool utils::renameSDFModel(sdf::Root& sdfRoot, const std::string& newModelName) { - const size_t initialNrOfModels = sdfRoot.ModelCount(); - // Create a new model with the scoped name auto renamedModel = std::make_shared(); renamedModel->SetName("model"); renamedModel->AddAttribute("name", "string", newModelName, true); + if (!sdfRoot.Model()) { + sError << "The sdf Root does not contain any model" << std::endl; + return false; + } + // Get the first child of the original model element - sdf::ElementPtr child = - sdfRoot.ModelByIndex(modelIndex)->Element()->GetFirstElement(); + sdf::ElementPtr child = sdfRoot.Model()->Element()->GetFirstElement(); // Add all the children to the renamed model element while (child) { @@ -343,19 +343,14 @@ bool utils::renameSDFModel(sdf::Root& sdfRoot, } // Remove the old model - auto originalModelElement = sdfRoot.ModelByIndex(modelIndex)->Element(); + auto originalModelElement = sdfRoot.Model()->Element(); originalModelElement->RemoveFromParent(); // Insert the renamed model renamedModel->SetParent(sdfRoot.Element()); sdfRoot.Element()->InsertElement(renamedModel); - if (sdfRoot.ModelCount() != initialNrOfModels) { - sError << "Failed to rename SDF model" << std::endl; - return false; - } - - if (!sdfRoot.ModelNameExists(newModelName)) { + if (sdfRoot.Model()->Name() != newModelName) { sError << "Failed to insert renamed model in SDF root" << std::endl; return false; } diff --git a/cpp/scenario/gazebo/src/utils.cpp b/cpp/scenario/gazebo/src/utils.cpp index b14855539..3b54fbfb3 100644 --- a/cpp/scenario/gazebo/src/utils.cpp +++ b/cpp/scenario/gazebo/src/utils.cpp @@ -106,8 +106,7 @@ std::string utils::getSdfString(const std::string& fileName) return root->Element()->ToString(""); } -std::string utils::getModelNameFromSdf(const std::string& fileName, - const size_t modelIndex) +std::string utils::getModelNameFromSdf(const std::string& fileName) { std::string absFileName = findSdfFile(fileName); @@ -116,25 +115,18 @@ std::string utils::getModelNameFromSdf(const std::string& fileName, return {}; } - auto root = utils::getSdfRootFromFile(absFileName); + const auto root = utils::getSdfRootFromFile(absFileName); if (!root) { return {}; } - if (root->ModelCount() == 0) { - sError << "Didn't find any model in file " << fileName << std::endl; - return {}; - } - - if (modelIndex >= root->ModelCount()) { - sError << "Model with index " << modelIndex - << " not found. The model has only " << root->ModelCount() - << " model(s)" << std::endl; - return {}; + if (const auto model = root->Model()) { + return model->Name(); } - return root->ModelByIndex(modelIndex)->Name(); + sError << "No model found in file " << fileName << std::endl; + return {}; } std::string utils::getWorldNameFromSdf(const std::string& fileName, diff --git a/tests/test_scenario/test_world.py b/tests/test_scenario/test_world.py index 28e4d3800..236340bba 100644 --- a/tests/test_scenario/test_world.py +++ b/tests/test_scenario/test_world.py @@ -89,7 +89,7 @@ def test_world_api(gazebo: scenario.GazeboSimulator): assert world.insert_model(cube_urdf) assert len(world.model_names()) == 1 - default_model_name = scenario.get_model_name_from_sdf(cube_urdf, 0) + default_model_name = scenario.get_model_name_from_sdf(cube_urdf) assert default_model_name in world.model_names() cube1 = world.get_model(default_model_name) # TODO: assert cube1