Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Address sdformat11 deprecations #321

Merged
merged 2 commits into from
Apr 28, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 1 addition & 3 deletions cpp/scenario/gazebo/include/scenario/gazebo/helpers.h
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
13 changes: 7 additions & 6 deletions cpp/scenario/gazebo/include/scenario/gazebo/utils.h
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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);
Expand Down
38 changes: 13 additions & 25 deletions cpp/scenario/gazebo/src/World.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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<sdf::Model*>(modelSdfRoot->ModelByIndex(ModelIndex))
// to string. We need also to operate directly on the raw element.
const_cast<sdf::Model*>(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;
}
Expand All @@ -128,18 +119,16 @@ 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);

{
// 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);
Expand Down Expand Up @@ -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<ignition::gazebo::components::Pose>(
world.m_ecm, modelEntity, utils::toIgnitionPose(pose));
Expand Down Expand Up @@ -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,
Expand Down
23 changes: 9 additions & 14 deletions cpp/scenario/gazebo/src/helpers.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<sdf::Element>();
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) {
Expand All @@ -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;
}
Expand Down
20 changes: 6 additions & 14 deletions cpp/scenario/gazebo/src/utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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,
Expand Down
2 changes: 1 addition & 1 deletion tests/test_scenario/test_world.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down