diff --git a/src/SdfGenerator.cc b/src/SdfGenerator.cc index 08b281b0a4..85fa6918fb 100644 --- a/src/SdfGenerator.cc +++ b/src/SdfGenerator.cc @@ -17,6 +17,7 @@ #include "SdfGenerator.hh" +#include #include #include @@ -305,6 +306,7 @@ namespace sdf_generator auto modelDir = common::parentPath(_modelSdf->Data().Element()->FilePath()); + const std::string modelName = scopedName(_modelEntity, _ecm, "::", false); @@ -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()) { @@ -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("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, diff --git a/src/SdfGenerator.hh b/src/SdfGenerator.hh index d0c2744c72..e580fe53e8 100644 --- a/src/SdfGenerator.hh +++ b/src/SdfGenerator.hh @@ -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 diff --git a/test/integration/save_world.cc b/test/integration/save_world.cc index 149ce5c3f3..0c8fc6107b 100644 --- a/test/integration/save_world.cc +++ b/test/integration/save_world.cc @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -29,6 +30,7 @@ #include #include #include +#include #include #include @@ -60,16 +62,16 @@ class SdfGeneratorFixture : public ::testing::Test this->server = std::make_unique(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(); } @@ -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) diff --git a/test/worlds/model_nested_include.sdf b/test/worlds/model_nested_include.sdf new file mode 100644 index 0000000000..28f785cd48 --- /dev/null +++ b/test/worlds/model_nested_include.sdf @@ -0,0 +1,98 @@ + + + + + 0.001 + 1.0 + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + true + + + + + 20 20 0.1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + include_nested + include_nested_new_name + 1 2 3 0 0 0 + + + + + sphere + 0 2 2 0 0 0 + + + + + coke + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Coke Can + 2 2 2 0 0 0 + + + + + 0 -2 0 0 0 0 + + 0 0 2 0 0 0 + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + +