Skip to content

Commit

Permalink
Fix included nested model expansion in SDF generation (#768)
Browse files Browse the repository at this point in the history 
* fixed included nested model expansion

Signed-off-by: Jenn Nguyen <[email protected]>

* added resource path to test

Signed-off-by: Jenn Nguyen <[email protected]>

* use orig URIs & support multi level nesting

Signed-off-by: Jenn Nguyen <[email protected]>

* save fuel version when enabled

Signed-off-by: Jenn Nguyen <[email protected]>

* retrieve uri from map

Signed-off-by: Jenn Nguyen <[email protected]>

* copy included element

Signed-off-by: Jenn Nguyen <[email protected]>

* clear attributes before copying include element

Signed-off-by: Jenn Nguyen <[email protected]>
  • Loading branch information
@jennuine
jennuine authored May 11, 2021
1 parent a9cfa0d commit dca877a
Show file tree
Hide file tree
Showing 4 changed files with 310 additions and 3 deletions.
68 changes: 68 additions & 0 deletions src/SdfGenerator.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include "SdfGenerator.hh"

#include <ctype.h>
#include <memory>
#include <vector>

Expand Down Expand Up @@ -305,6 +306,7 @@ namespace sdf_generator

auto modelDir =
common::parentPath(_modelSdf->Data().Element()->FilePath());

const std::string modelName =
scopedName(_modelEntity, _ecm, "::", false);

Expand All @@ -324,6 +326,13 @@ namespace sdf_generator
{
auto modelElem = _elem->AddElement("model");
updateModelElement(modelElem, _ecm, _modelEntity);

// Check & update possible //model/include(s)
if (!modelConfig.expand_include_tags().data())
{
updateModelElementWithNestedInclude(modelElem,
modelConfig.save_fuel_version().data(), _includeUriMap);
}
}
else if (uriMapIt != _includeUriMap.end())
{
Expand Down Expand Up @@ -422,6 +431,65 @@ namespace sdf_generator
return true;
}

/////////////////////////////////////////////////
/// \brief Checks if a string is a number
/// \param[in] _str The string to check
/// \return True if the string is a number
bool isNumber(const std::string &_str)
{
for (const char &c : _str)
if (!std::isdigit(c)) return false;

return true;
}

/////////////////////////////////////////////////
void updateModelElementWithNestedInclude(sdf::ElementPtr &_elem,
const bool _saveFuelVersion,
const IncludeUriMap &_includeUriMap)
{
sdf::ElementPtr e = _elem->GetFirstElement(), nextE = nullptr;
while (e != nullptr)
{
nextE = e->GetNextElement();

if (e->GetIncludeElement() != nullptr)
{
std::string modelDir = common::parentPath(e->FilePath());
auto uriMapIt = _includeUriMap.find(modelDir);

if (_saveFuelVersion && uriMapIt != _includeUriMap.end())
{
// find fuel model version from file path
std::string version = common::basename(modelDir);

if (isNumber(version))
{
std::string uri = e->GetIncludeElement()->Get<std::string>("uri");
uri = uri + "/" + version;
e->GetIncludeElement()->GetElement("uri")->Set(uri);
}
else
{
ignwarn << "Error retrieving Fuel model version,"
<< " saving model without version."
<< std::endl;
}
}

e->RemoveAllAttributes();
e->Copy(e->GetIncludeElement());
}
else if (e->GetName() == "model")
{
updateModelElementWithNestedInclude(e,
_saveFuelVersion, _includeUriMap);
}

e = nextE;
}
}

/////////////////////////////////////////////////
bool updateIncludeElement(const sdf::ElementPtr &_elem,
const EntityComponentManager &_ecm,
Expand Down
11 changes: 11 additions & 0 deletions src/SdfGenerator.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,17 @@ namespace sdf_generator
const EntityComponentManager &_ecm,
const Entity &_entity);

/// \brief Update a sdf::Element model to use //include instead of expanded
/// model (to be used when expand_include_tags is disabled)
/// \input[in, out] _elem sdf::Element to update
/// \input[in] _saveFuelVersion True if "Save Fuel model versions" is enabled
/// \input[in] _includeUriMap Map from file paths to URIs used to preserve
/// included Fuel models
IGNITION_GAZEBO_VISIBLE
void updateModelElementWithNestedInclude(sdf::ElementPtr &_elem,
const bool _saveFuelVersion,
const IncludeUriMap &_includeUriMap);

/// \brief Update a sdf::Element of an included resource.
/// Intended for internal use.
/// \input[in, out] _elem sdf::Element to update
Expand Down
136 changes: 133 additions & 3 deletions test/integration/save_world.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include <gtest/gtest.h>
#include <ignition/msgs/sdf_generator_config.pb.h>
#include <sstream>
#include <tinyxml2.h>

#include <sdf/Collision.hh>
Expand All @@ -29,6 +30,7 @@
#include <ignition/common/Console.hh>
#include <ignition/common/Filesystem.hh>
#include <ignition/common/Util.hh>
#include <ignition/math/Pose3.hh>
#include <ignition/transport/Node.hh>
#include <ignition/utilities/ExtraTestMacros.hh>

Expand Down Expand Up @@ -60,16 +62,16 @@ class SdfGeneratorFixture : public ::testing::Test
this->server = std::make_unique<Server>(serverConfig);
EXPECT_FALSE(server->Running());
}
public: std::string RequestGeneratedSdf(const std::string &_worldName)
public: std::string RequestGeneratedSdf(const std::string &_worldName,
const msgs::SdfGeneratorConfig &_req = msgs::SdfGeneratorConfig())
{
transport::Node node;
msgs::SdfGeneratorConfig req;

msgs::StringMsg worldGenSdfRes;
bool result;
unsigned int timeout = 5000;
std::string service{"/world/" + _worldName + "/generate_world_sdf"};
EXPECT_TRUE(node.Request(service, req, timeout, worldGenSdfRes, result));
EXPECT_TRUE(node.Request(service, _req, timeout, worldGenSdfRes, result));
EXPECT_TRUE(result);
return worldGenSdfRes.data();
}
Expand Down Expand Up @@ -302,6 +304,134 @@ TEST_F(SdfGeneratorFixture, WorldWithNestedModel)
ASSERT_NE(nullptr, genWorld);
}


/////////////////////////////////////////////////
TEST_F(SdfGeneratorFixture, ModelWithNestedIncludes)
{
std::string path =
common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models");
common::setenv("IGN_GAZEBO_RESOURCE_PATH", path);

this->LoadWorld("test/worlds/model_nested_include.sdf");

EXPECT_NE(kNullEntity, this->server->EntityByName("ground_plane"));
EXPECT_NE(kNullEntity, this->server->EntityByName("L0"));
EXPECT_NE(kNullEntity, this->server->EntityByName("C0"));
EXPECT_NE(kNullEntity, this->server->EntityByName("V0"));
EXPECT_NE(kNullEntity, this->server->EntityByName("M1"));
EXPECT_NE(kNullEntity, this->server->EntityByName("M2"));
EXPECT_NE(kNullEntity, this->server->EntityByName("M3"));
EXPECT_NE(kNullEntity, this->server->EntityByName("coke"));
EXPECT_NE(kNullEntity, this->server->EntityByName("L1"));
EXPECT_NE(kNullEntity, this->server->EntityByName("C1"));
EXPECT_NE(kNullEntity, this->server->EntityByName("V1"));
EXPECT_NE(kNullEntity, this->server->EntityByName("include_nested_new_name"));
EXPECT_NE(kNullEntity, this->server->EntityByName("link_00"));
EXPECT_NE(kNullEntity, this->server->EntityByName("link_01"));
EXPECT_NE(kNullEntity, this->server->EntityByName("sphere"));
EXPECT_NE(kNullEntity, this->server->EntityByName("V"));
EXPECT_NE(kNullEntity, this->server->EntityByName("C"));

msgs::SdfGeneratorConfig req;
req.mutable_global_entity_gen_config()
->mutable_save_fuel_version()->set_data(true);

const std::string worldGenSdfRes =
this->RequestGeneratedSdf("model_nested_include_world", req);

// check that model w/ nested includes are not expanded
tinyxml2::XMLDocument genSdfDoc;
genSdfDoc.Parse(worldGenSdfRes.c_str());
ASSERT_NE(nullptr, genSdfDoc.RootElement());
auto genWorld = genSdfDoc.RootElement()->FirstChildElement("world");
ASSERT_NE(nullptr, genWorld);

auto model = genWorld->FirstChildElement("model"); // ground_plane
ASSERT_NE(nullptr, model);
model = model->NextSiblingElement("model"); // M1
ASSERT_NE(nullptr, model);

// M1's child include
auto include = model->FirstChildElement("include");
ASSERT_NE(nullptr, include);

auto uri = include->FirstChildElement("uri");
ASSERT_NE(nullptr, uri);
ASSERT_NE(nullptr, uri->GetText());
EXPECT_EQ("include_nested", std::string(uri->GetText()));

auto name = include->FirstChildElement("name");
ASSERT_NE(nullptr, name);
ASSERT_NE(nullptr, name->GetText());
EXPECT_EQ("include_nested_new_name", std::string(name->GetText()));

auto pose = include->FirstChildElement("pose");
ASSERT_NE(nullptr, pose);
ASSERT_NE(nullptr, pose->GetText());

std::stringstream ss(pose->GetText());
ignition::math::Pose3d p;
ss >> p;
EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), p);

// M2
model = model->FirstChildElement("model");
ASSERT_NE(nullptr, model);

// M2's child include
include = model->FirstChildElement("include");
ASSERT_NE(nullptr, include);

uri = include->FirstChildElement("uri");
ASSERT_NE(nullptr, uri);
ASSERT_NE(nullptr, uri->GetText());
EXPECT_EQ("sphere", std::string(uri->GetText()));

name = include->FirstChildElement("name");
EXPECT_EQ(nullptr, name);

pose = include->FirstChildElement("pose");
ASSERT_NE(nullptr, pose);
ASSERT_NE(nullptr, pose->GetText());

ss = std::stringstream(pose->GetText());
ss >> p;
EXPECT_EQ(ignition::math::Pose3d(0, 2, 2, 0, 0, 0), p);

// M3
model = model->FirstChildElement("model");
ASSERT_NE(nullptr, model);

// M3's child include
include = model->FirstChildElement("include");
ASSERT_NE(nullptr, include);

uri = include->FirstChildElement("uri");
ASSERT_NE(nullptr, uri);
ASSERT_NE(nullptr, uri->GetText());
EXPECT_EQ(
"https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Coke Can/2",
std::string(uri->GetText()));

name = include->FirstChildElement("name");
ASSERT_NE(nullptr, name);
ASSERT_NE(nullptr, name->GetText());
EXPECT_EQ("coke", std::string(name->GetText()));

pose = include->FirstChildElement("pose");
ASSERT_NE(nullptr, pose);
ASSERT_NE(nullptr, pose->GetText());

ss = std::stringstream(pose->GetText());
ss >> p;
EXPECT_EQ(ignition::math::Pose3d(2, 2, 2, 0, 0, 0), p);

// check reloading generated sdf
sdf::Root root;
sdf::Errors err = root.LoadSdfString(worldGenSdfRes);
EXPECT_TRUE(err.empty());
}

/////////////////////////////////////////////////
/// Main
int main(int _argc, char **_argv)
Expand Down
98 changes: 98 additions & 0 deletions test/worlds/model_nested_include.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
<?xml version="1.0" ?>
<sdf version="1.8">
<world name="model_nested_include_world">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="libignition-gazebo-physics-system.so"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="libignition-gazebo-scene-broadcaster-system.so"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>

<scene>
<ambient>1.0 1.0 1.0</ambient>
<background>0.8 0.8 0.8</background>
</scene>

<model name="ground_plane">
<static>true</static>
<link name="L0">
<collision name="C0">
<geometry>
<box>
<size>20 20 0.1</size>
</box>
</geometry>
</collision>
<visual name="V0">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>


<model name="M1">
<include>
<uri>include_nested</uri>
<name>include_nested_new_name</name>
<pose>1 2 3 0 0 0</pose>
</include>

<model name="M2">
<include>
<uri>sphere</uri>
<pose>0 2 2 0 0 0</pose>
</include>

<model name="M3">
<include>
<name>coke</name>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Coke Can</uri>
<pose>2 2 2 0 0 0</pose>
</include>
</model>
</model>

<pose>0 -2 0 0 0 0</pose>
<link name="L1">
<pose>0 0 2 0 0 0</pose>
<collision name="C1">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>

<visual name="V1">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>
</model>

</world>
</sdf>

0 comments on commit dca877a

Please sign in to comment.