diff --git a/sdf/1.8/1_7.convert b/sdf/1.8/1_7.convert
index f184573b4..86b85c3dd 100644
--- a/sdf/1.8/1_7.convert
+++ b/sdf/1.8/1_7.convert
@@ -1,3 +1,9 @@
+
+
+
+
+
+
diff --git a/src/Converter.cc b/src/Converter.cc
index dff989ad2..818501a50 100644
--- a/src/Converter.cc
+++ b/src/Converter.cc
@@ -40,6 +40,36 @@ bool EndsWith(const std::string& _a, const std::string& _b)
return (_a.size() >= _b.size()) &&
(_a.compare(_a.size() - _b.size(), _b.size(), _b) == 0);
}
+
+/////////////////////////////////////////////////
+// returns true if the element is not one of the listed for Unflatten conversion
+bool IsNotFlattenedElement(const std::string &_elemName)
+{
+ return (_elemName != "frame" && _elemName != "joint" && _elemName != "link"
+ && _elemName != "model" && _elemName != "gripper");
+}
+
+/////////////////////////////////////////////////
+// used to update //pose/@relative_to in FindNewModelElements()
+void UpdatePose(tinyxml2::XMLElement *_elem,
+ const size_t &_childNameIdx,
+ const std::string &_modelName)
+{
+ tinyxml2::XMLElement *pose = _elem->FirstChildElement("pose");
+ if (pose && pose->Attribute("relative_to"))
+ {
+ std::string poseRelTo = pose->Attribute("relative_to");
+
+ SDF_ASSERT(poseRelTo.compare(0, _modelName.size(), _modelName) == 0,
+ "Error: Pose attribute 'relative_to' does not start with " + _modelName);
+
+ poseRelTo = poseRelTo.substr(_childNameIdx);
+ pose->SetAttribute("relative_to", poseRelTo.c_str());
+ }
+
+ if (_elem->FirstChildElement("camera"))
+ UpdatePose(_elem->FirstChildElement("camera"), _childNameIdx, _modelName);
+}
}
/////////////////////////////////////////////////
@@ -229,6 +259,10 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem,
{
Remove(_elem, childElem);
}
+ else if (name == "unflatten")
+ {
+ Unflatten(_elem);
+ }
else if (name != "convert")
{
sdferr << "Unknown convert element[" << name << "]\n";
@@ -236,6 +270,279 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem,
}
}
+/////////////////////////////////////////////////
+void Converter::Unflatten(tinyxml2::XMLElement *_elem)
+{
+ SDF_ASSERT(_elem != nullptr, "SDF element is nullptr");
+
+ tinyxml2::XMLDocument *doc = _elem->GetDocument();
+
+ tinyxml2::XMLElement *nextElem = nullptr, *firstUnflatModel = nullptr;
+ for (tinyxml2::XMLElement *elem = _elem->FirstChildElement();
+ elem;
+ elem = nextElem)
+ {
+ // break loop if reached the first unflattened model
+ if (firstUnflatModel == elem) break;
+
+ nextElem = elem->NextSiblingElement();
+ std::string elemName = elem->Name();
+
+ // skip element if not one of the following or if missing name attribute
+ if (IsNotFlattenedElement(elemName) || !elem->Attribute("name"))
+ continue;
+
+ std::string attrName = elem->Attribute("name");
+
+ size_t found = attrName.find("::");
+ if (found == std::string::npos)
+ {
+ // recursive unflatten
+ if (elemName == "model")
+ {
+ Unflatten(elem);
+ break;
+ }
+
+ continue;
+ }
+
+ std::string newModelName = attrName.substr(0, found);
+ tinyxml2::XMLElement *newModel = doc->NewElement("model");
+ newModel->SetAttribute("name", newModelName.c_str());
+
+ if (FindNewModelElements(_elem, newModel, found + 2))
+ {
+ Unflatten(newModel);
+ _elem->InsertEndChild(newModel);
+
+ // since newModel is inserted at the end, point back to the top element
+ nextElem = _elem->FirstChildElement();
+
+ if (!firstUnflatModel)
+ firstUnflatModel = newModel;
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem,
+ tinyxml2::XMLElement *_newModel,
+ const size_t &_childNameIdx)
+{
+ bool unflattenedNewModel = false;
+ std::string newModelName = _newModel->Attribute("name");
+ size_t newModelNameSize = newModelName.size();
+
+ // loop through looking for new model elements
+ tinyxml2::XMLElement *elem = _elem->FirstChildElement(), *nextElem = nullptr;
+ while (elem)
+ {
+ nextElem = elem->NextSiblingElement();
+
+ std::string elemName = elem->Name();
+ std::string elemAttrName;
+
+ if (elem->Attribute("name"))
+ elemAttrName = elem->Attribute("name");
+
+ if (elemAttrName.empty() ||
+ elemAttrName.compare(0, newModelNameSize, newModelName) != 0 ||
+ IsNotFlattenedElement(elemName))
+ {
+ // since //gripper/@name may not be flattened but the children are
+ // & elemAttrName.compare will evaluate to true, don't skip this element
+ if (elemName != "gripper")
+ {
+ elem = nextElem;
+ continue;
+ }
+ }
+
+ // Child attribute name w/ newModelName prefix stripped except for
+ // possibly //gripper, which may or may not have a prefix
+ std::string childAttrName;
+ if (elemAttrName.compare(0, newModelNameSize, newModelName) == 0)
+ {
+ childAttrName = elemAttrName.substr(_childNameIdx);
+ elem->SetAttribute("name", childAttrName.c_str());
+ }
+
+ // strip new model prefix from //pose/@relative_to
+ tinyxml2::XMLElement *poseElem = elem->FirstChildElement("pose");
+ if (poseElem != nullptr && poseElem->Attribute("relative_to"))
+ {
+ std::string poseRelTo = poseElem->Attribute("relative_to");
+
+ if (poseRelTo.compare(0, newModelNameSize, newModelName) == 0)
+ {
+ poseRelTo = poseRelTo.substr(_childNameIdx);
+ poseElem->SetAttribute("relative_to", poseRelTo.c_str());
+ }
+ }
+
+ if (elemName == "frame")
+ {
+ std::string attachedTo;
+
+ if (elem->Attribute("attached_to"))
+ {
+ attachedTo = elem->Attribute("attached_to");
+
+ SDF_ASSERT(attachedTo.compare(0, newModelNameSize, newModelName) == 0,
+ "Error: Frame attribute 'attached_to' does not start with " +
+ newModelName);
+
+ // strip new model prefix from attached_to
+ attachedTo = attachedTo.substr(_childNameIdx);
+ elem->SetAttribute("attached_to", attachedTo.c_str());
+
+ // remove frame if childAttrName == __model__
+ if (childAttrName == "__model__")
+ {
+ _newModel->SetAttribute("canonical_link", attachedTo.c_str());
+ _newModel->InsertFirstChild(poseElem);
+
+ _elem->DeleteChild(elem);
+ elem = poseElem;
+ }
+ }
+ } // frame
+
+ else if (elemName == "link")
+ {
+ // find & strip new model prefix of all //link//pose/@relative_to
+ for (tinyxml2::XMLElement *e = elem->FirstChildElement();
+ e;
+ e = e->NextSiblingElement())
+ {
+ UpdatePose(e, _childNameIdx, newModelName);
+ }
+ } // link
+
+ else if (elemName == "joint")
+ {
+ std::string eText;
+
+ // strip new model prefix from //joint/parent
+ tinyxml2::XMLElement *e = elem->FirstChildElement("parent");
+ if (e != nullptr && e->GetText() != nullptr)
+ {
+ eText = e->GetText();
+
+ SDF_ASSERT(eText.compare(0, newModelNameSize, newModelName) == 0,
+ "Error: Joint's value does not start with " + newModelName);
+
+ e->SetText(eText.substr(_childNameIdx).c_str());
+ }
+
+ // strip new model prefix from //joint/child
+ e = elem->FirstChildElement("child");
+ if (e != nullptr && e->GetText() != nullptr)
+ {
+ eText = e->GetText();
+
+ SDF_ASSERT(eText.compare(0, newModelNameSize, newModelName) == 0,
+ "Error: Joint's value does not start with " + newModelName);
+
+ e->SetText(eText.substr(_childNameIdx).c_str());
+ }
+
+ // strip new model prefix from //xyz/@expressed_in
+ std::string axisStr = "axis";
+ while (true)
+ {
+ tinyxml2::XMLElement *axisElem =
+ elem->FirstChildElement(axisStr.c_str());
+ if (axisElem != nullptr)
+ {
+ if (axisElem->FirstChildElement("xyz")->Attribute("expressed_in"))
+ {
+ std::string expressIn =
+ axisElem->FirstChildElement("xyz")->Attribute("expressed_in");
+
+ SDF_ASSERT(
+ expressIn.compare(0, newModelNameSize, newModelName) == 0,
+ "Error: 's attribute 'expressed_in' does not start with " +
+ newModelName);
+
+ expressIn = expressIn.substr(_childNameIdx);
+
+ axisElem->FirstChildElement("xyz")
+ ->SetAttribute("expressed_in", expressIn.c_str());
+ }
+ }
+
+ if (axisStr == "axis2") break;
+
+ axisStr += "2";
+ }
+
+ // strip new model prefix from all //joint/sensor/pose/@relative_to
+ for (e = elem->FirstChildElement("sensor");
+ e;
+ e = e->NextSiblingElement("sensor"))
+ {
+ UpdatePose(e, _childNameIdx, newModelName);
+ }
+ } // joint
+
+ else if (elemName == "gripper")
+ {
+ bool hasPrefix = true;
+
+ // strip prefix from all /model/gripper/gripper_link
+ tinyxml2::XMLElement *e = elem->FirstChildElement("gripper_link");
+ std::string eText;
+ while (e)
+ {
+ if (e->GetText() != nullptr)
+ {
+ eText = e->GetText();
+
+ if (eText.compare(0, newModelNameSize, newModelName) != 0)
+ {
+ hasPrefix = false;
+ break;
+ }
+
+ e->SetText(eText.substr(_childNameIdx).c_str());
+ }
+
+ e = e->NextSiblingElement("gripper_link");
+ }
+
+ // if //model/gripper/gripper_link does not have new model prefix
+ // don't add to new model
+ if (!hasPrefix)
+ {
+ elem = nextElem;
+ continue;
+ }
+
+ // strip prefix from //model/gripper/palm_link
+ e = elem->FirstChildElement("palm_link");
+ if (e != nullptr && e->GetText() != nullptr)
+ {
+ eText = e->GetText();
+
+ SDF_ASSERT(eText.compare(0, newModelNameSize, newModelName) == 0,
+ "Error: Gripper's value does not start with "
+ + newModelName);
+
+ e->SetText(eText.substr(_childNameIdx).c_str());
+ }
+ } // gripper
+
+ unflattenedNewModel = true;
+ _newModel->InsertEndChild(elem);
+
+ elem = nextElem;
+ }
+
+ return unflattenedNewModel;
+}
+
/////////////////////////////////////////////////
void Converter::Rename(tinyxml2::XMLElement *_elem,
tinyxml2::XMLElement *_renameElem)
diff --git a/src/Converter.hh b/src/Converter.hh
index c5f7c6e7c..ce24c65dd 100644
--- a/src/Converter.hh
+++ b/src/Converter.hh
@@ -20,6 +20,7 @@
#include
#include
+#include
#include
#include "sdf/system_util.hh"
@@ -103,6 +104,20 @@ namespace sdf
private: static void Remove(tinyxml2::XMLElement *_elem,
tinyxml2::XMLElement *_removeElem);
+ /// \brief Unflatten an element (conversion from SDFormat <= 1.7 to 1.8)
+ /// \param[in] _elem The element to unflatten
+ private: static void Unflatten(tinyxml2::XMLElement *_elem);
+
+ /// \brief Finds all elements related to the unflattened model
+ /// \param[in] _elem The element to unflatten
+ /// \param[in] _newModel The new unflattened model element
+ /// \param[in] _childNameIdx The beginning index of child element names
+ /// (e.g., in newModelName::childName then _childNameIdx = 14)
+ /// \return True if unflattened new model elements
+ private: static bool FindNewModelElements(tinyxml2::XMLElement *_elem,
+ tinyxml2::XMLElement *_newModel,
+ const size_t &_childNameIdx);
+
private: static const char *GetValue(const char *_valueElem,
const char *_valueAttr,
tinyxml2::XMLElement *_elem);
diff --git a/src/Converter_TEST.cc b/src/Converter_TEST.cc
index 2649e5dd5..2e33b70b1 100644
--- a/src/Converter_TEST.cc
+++ b/src/Converter_TEST.cc
@@ -2141,6 +2141,395 @@ TEST(Converter, Pose_16_to_17)
EXPECT_STREQ("parent", jointLinkPoseElem->Attribute("relative_to"));
}
+const std::string CONVERT_DOC_17_18 =
+ sdf::testing::SourceFile("sdf", "1.8", "1_7.convert");
+
+/////////////////////////////////////////////////
+/// Test conversion unflattened world in 1.7 to 1.8
+TEST(Converter, World_17_to_18)
+{
+ // for ElementToString
+ using namespace sdf;
+
+ // ------- The flattened world in 1.7 format
+ std::string xmlString = R"(
+
+
+
+
+
+ 1 0 0 0 0 0
+
+
+ 0 1 0 0 0 0
+
+
+ 0 0 1 0 0 0
+
+
+
+ )";
+
+
+ tinyxml2::XMLDocument xmlDoc;
+ xmlDoc.Parse(xmlString.c_str());
+
+ // Convert
+ tinyxml2::XMLDocument convertXmlDoc;
+ convertXmlDoc.LoadFile(CONVERT_DOC_17_18.c_str());
+ sdf::Converter::Convert(&xmlDoc, &convertXmlDoc);
+
+ // Compare converted xml with expected
+ std::string convertedXmlStr = ElementToString(xmlDoc.RootElement());
+ ASSERT_FALSE(convertedXmlStr.empty());
+
+ std::string expectedXmlStr = R"(
+
+
+
+
+ 1 0 0 0 0 0
+
+ 0 1 0 0 0 0
+
+ 0 0 1 0 0 0
+
+
+
+
+
+ )";
+
+ tinyxml2::XMLDocument expectedXmlDoc;
+ expectedXmlDoc.Parse(expectedXmlStr.c_str());
+
+ EXPECT_EQ(ElementToString(expectedXmlDoc.RootElement()), convertedXmlStr);
+
+ // Check some basic elements
+ tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "sdf");
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "world");
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "model");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "include_links");
+
+ // Check unnested elements
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "model");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "A");
+ EXPECT_STREQ(convertedElem->Attribute("canonical_link"), "B::C");
+ EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "pose");
+ EXPECT_STREQ(convertedElem->Attribute("relative_to"), "__model__");
+
+ convertedElem = convertedElem->NextSiblingElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "model");
+ EXPECT_STREQ(convertedElem->Attribute("canonical_link"), "C");
+ EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "pose");
+ EXPECT_STREQ(convertedElem->Attribute("relative_to"), "__model__");
+ convertedElem = convertedElem->NextSiblingElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "link");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "C");
+ EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "pose");
+ EXPECT_STREQ(convertedElem->Attribute("relative_to"), "__model__");
+
+
+ // ------- Another flattened world in 1.7 format
+ xmlString = R"(
+
+
+
+
+
+ 1 0 1 0 0 0
+
+
+ 1 0 1 0 0 0
+
+
+ 0 1 0 0 0 0
+
+
+
+ 0.1
+
+
+
+
+
+ 0 0 1 0 0 0
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ ChildModel::L1
+ ChildModel::L2
+
+
+ 0 0 0 0 0 0
+ ChildModel::L1
+ ChildModel::L2
+
+ 0 0 1
+
+
+ 0 0 1
+
+
+ 1 0 0 0 0 0
+
+ 0 0 1 0 0 0
+
+
+
+
+ ChildModel::L1
+ ChildModel::L2
+
+
+ ChildModel::L1
+ ChildModel::L2
+
+
+
+ )";
+
+ xmlDoc.Clear();
+ xmlDoc.Parse(xmlString.c_str());
+
+ sdf::Converter::Convert(&xmlDoc, &convertXmlDoc);
+
+ // Compare converted xml with expected
+ convertedXmlStr = ElementToString(xmlDoc.RootElement());
+ ASSERT_FALSE(convertedXmlStr.empty());
+
+ expectedXmlStr = R"(
+
+
+
+
+
+ 1 0 1 0 0 0
+
+ 1 0 1 0 0 0
+
+
+ 0 1 0 0 0 0
+
+
+
+ 0.1
+
+
+
+
+
+ 0 0 1 0 0 0
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ L1
+ L2
+
+
+ 0 0 0 0 0 0
+ L1
+ L2
+
+ 0 0 1
+
+
+ 0 0 1
+
+
+ 1 0 0 0 0 0
+
+ 0 0 1 0 0 0
+
+
+
+
+ L1
+ L2
+
+
+ L1
+ L2
+
+
+
+
+ )";
+
+ expectedXmlDoc.Clear();
+ expectedXmlDoc.Parse(expectedXmlStr.c_str());
+
+ EXPECT_EQ(ElementToString(expectedXmlDoc.RootElement()), convertedXmlStr);
+
+
+ // ------- Another flattened world in 1.7 format
+ xmlString = R"(
+
+
+
+
+
+
+
+
+
+
+ 1 0 0 0 0 0
+
+
+ 0.1
+
+
+
+
+
+
+
+)";
+
+ xmlDoc.Clear();
+ xmlDoc.Parse(xmlString.c_str());
+
+ sdf::Converter::Convert(&xmlDoc, &convertXmlDoc);
+
+ // Compare converted xml with expected
+ convertedXmlStr = ElementToString(xmlDoc.RootElement());
+ ASSERT_FALSE(convertedXmlStr.empty());
+
+ expectedXmlStr = R"(
+
+
+
+
+
+
+
+
+
+
+
+ 1 0 0 0 0 0
+
+
+ 0.1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+)";
+
+ expectedXmlDoc.Clear();
+ expectedXmlDoc.Parse(expectedXmlStr.c_str());
+
+ EXPECT_EQ(ElementToString(expectedXmlDoc.RootElement()), convertedXmlStr);
+
+ // Check some basic elements
+ convertedElem = xmlDoc.FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "sdf");
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "world");
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "model");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "ParentModel");
+ EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);
+
+ // Check unnested elements
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "model");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "C");
+ convertedElem = convertedElem->NextSiblingElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "model");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "ChildModel");
+ EXPECT_STREQ(convertedElem->FirstChildElement()->Name(), "link");
+ EXPECT_STREQ(convertedElem->FirstChildElement()->Attribute("name"), "L1");
+ convertedElem = convertedElem->NextSiblingElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "model");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "A");
+ EXPECT_STREQ(convertedElem->FirstChildElement()->Name(), "model");
+ EXPECT_STREQ(convertedElem->FirstChildElement()->Attribute("name"), "B");
+ EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);
+ convertedElem = convertedElem->PreviousSiblingElement();
+ ASSERT_NE(convertedElem, nullptr);
+ convertedElem = convertedElem->PreviousSiblingElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "model");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "C");
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "model");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "D");
+ EXPECT_STREQ(convertedElem->FirstChildElement()->Name(), "link");
+ EXPECT_STREQ(convertedElem->FirstChildElement()->Attribute("name"), "E");
+ convertedElem = convertedElem->NextSiblingElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "model");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "F");
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "model");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "G");
+ EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "link");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "H");
+ EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "visual");
+ EXPECT_STREQ(convertedElem->Attribute("name"), "v1");
+ EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);
+ convertedElem = convertedElem->FirstChildElement();
+ ASSERT_NE(convertedElem, nullptr);
+ EXPECT_STREQ(convertedElem->Name(), "pose");
+ EXPECT_STREQ(convertedElem->Attribute("relative_to"), "__model__");
+ EXPECT_STREQ(convertedElem->NextSiblingElement()->Name(), "geometry");
+}
+
/////////////////////////////////////////////////
/// Main
int main(int argc, char **argv)
diff --git a/src/XmlUtils.cc b/src/XmlUtils.cc
index ff2d86b93..4dbceab71 100644
--- a/src/XmlUtils.cc
+++ b/src/XmlUtils.cc
@@ -55,6 +55,20 @@ tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc,
return copy;
}
+
+/////////////////////////////////////////////////
+std::string ElementToString(const tinyxml2::XMLElement *_elem)
+{
+ if (_elem == nullptr)
+ {
+ sdferr << "Pointer to XML Element _elem is nullptr\n";
+ return "";
+ }
+
+ tinyxml2::XMLPrinter printer;
+ _elem->Accept(&printer);
+
+ return std::string(printer.CStr());
+}
}
} // namespace sdf
-
diff --git a/src/XmlUtils.hh b/src/XmlUtils.hh
index 82b08b2b9..7ad0c30d4 100644
--- a/src/XmlUtils.hh
+++ b/src/XmlUtils.hh
@@ -40,6 +40,11 @@ namespace sdf
/// nullptr if an error occurs.
tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc,
const tinyxml2::XMLNode *_src);
+
+ /// \brief Converts the XML Element to a string
+ /// \param[in] _elem Element to be converted
+ /// \return The string representation
+ std::string ElementToString(const tinyxml2::XMLElement *_elem);
}
}
#endif
diff --git a/test/integration/converter.cc b/test/integration/converter.cc
index 73c13ffbe..2f52019e8 100644
--- a/test/integration/converter.cc
+++ b/test/integration/converter.cc
@@ -151,3 +151,127 @@ void ParserStringConverter(const std::string &_version)
EXPECT_EQ(_version, magElem->OriginalVersion());
}
+/////////////////////////////////////////////////
+/// Test conversion unflattened world in 1.7 to 1.8
+TEST(ConverterIntegration, UnflattenConversion)
+{
+ const std::string filename =
+ sdf::testing::SourceFile("test", "sdf",
+ "flattened_test_nested_model_with_frames.sdf");
+
+ sdf::SDFPtr sdf(new sdf::SDF());
+ sdf::init(sdf);
+
+ ASSERT_TRUE(sdf::convertFile(filename, "1.8", sdf));
+
+ ASSERT_NE(nullptr, sdf->Root());
+ EXPECT_EQ(sdf->Root()->GetName(), "sdf");
+ EXPECT_EQ("1.8", sdf->Root()->Get("version"));
+ EXPECT_EQ("1.7", sdf->OriginalVersion());
+ EXPECT_EQ("1.7", sdf->Root()->OriginalVersion());
+
+ sdf::ElementPtr worldElem = sdf->Root()->GetElement("world");
+ ASSERT_NE(nullptr, worldElem);
+ EXPECT_EQ(worldElem->Get("name"), "default");
+
+ sdf::ElementPtr modelElem = worldElem->GetElement("model");
+ ASSERT_NE(nullptr, modelElem);
+ EXPECT_EQ(modelElem->Get("name"),
+ "test_nested_model_with_frames");
+
+ sdf::ElementPtr nestModelElem = modelElem->GetElement("model");
+ ASSERT_NE(nullptr, nestModelElem);
+ EXPECT_EQ(nestModelElem->Get("name"), "test_model_with_frames");
+ EXPECT_EQ(nestModelElem->Get("canonical_link"), "L1");
+
+ sdf::ElementPtr poseElem = nestModelElem->GetElement("pose");
+ ASSERT_NE(nullptr, poseElem);
+ EXPECT_EQ(poseElem->Get("relative_to"), "__model__");
+
+ sdf::ElementPtr frameElem = nestModelElem->GetElement("frame");
+ ASSERT_NE(nullptr, frameElem);
+ EXPECT_EQ(frameElem->Get("name"), "F1");
+ EXPECT_EQ(frameElem->Get("attached_to"), "__model__");
+
+ frameElem = frameElem->GetNextElement("frame");
+ ASSERT_NE(nullptr, frameElem);
+ EXPECT_EQ(frameElem->Get("name"), "F2");
+ EXPECT_EQ(frameElem->Get("attached_to"), "__model__");
+ poseElem = frameElem->GetElement("pose");
+ ASSERT_NE(nullptr, poseElem);
+ EXPECT_EQ(poseElem->Get("relative_to"), "F1");
+
+ sdf::ElementPtr linkElem = frameElem->GetNextElement("link");
+ ASSERT_NE(nullptr, linkElem);
+ EXPECT_EQ(linkElem->Get("name"), "L1");
+ poseElem = linkElem->GetElement("pose");
+ ASSERT_NE(nullptr, poseElem);
+ EXPECT_EQ(poseElem->Get("relative_to"), "F1");
+ poseElem = linkElem->GetElement("visual")->GetElement("pose");
+ ASSERT_NE(nullptr, poseElem);
+ EXPECT_EQ(poseElem->Get("relative_to"), "F2");
+ poseElem = linkElem->GetElement("collision")->GetElement("pose");
+ ASSERT_NE(nullptr, poseElem);
+ EXPECT_EQ(poseElem->Get("relative_to"), "__model__");
+
+ linkElem = linkElem->GetNextElement("link");
+ ASSERT_NE(nullptr, linkElem);
+ EXPECT_EQ(linkElem->Get("name"), "L2");
+ poseElem = linkElem->GetElement("pose");
+ ASSERT_NE(nullptr, poseElem);
+ EXPECT_EQ(poseElem->Get("relative_to"), "F1");
+
+ linkElem = linkElem->GetNextElement("link");
+ ASSERT_NE(nullptr, linkElem);
+ EXPECT_EQ(linkElem->Get("name"), "L3");
+ poseElem = linkElem->GetElement("pose");
+ ASSERT_NE(nullptr, poseElem);
+ EXPECT_EQ(poseElem->Get("relative_to"), "L2");
+
+ linkElem = linkElem->GetNextElement("link");
+ ASSERT_NE(nullptr, linkElem);
+ EXPECT_EQ(linkElem->Get("name"), "L4");
+ poseElem = linkElem->GetElement("pose");
+ ASSERT_NE(nullptr, poseElem);
+ EXPECT_EQ(poseElem->Get("relative_to"), "__model__");
+
+ sdf::ElementPtr jointElem = linkElem->GetNextElement("joint");
+ ASSERT_NE(nullptr, jointElem);
+ EXPECT_EQ(jointElem->Get("name"), "J1");
+ poseElem = jointElem->GetElement("pose");
+ ASSERT_NE(nullptr, poseElem);
+ EXPECT_EQ(poseElem->Get("relative_to"), "L1");
+ EXPECT_EQ(jointElem->Get("parent"), "L1");
+ EXPECT_EQ(jointElem->Get("child"), "L2");
+ sdf::ElementPtr xyzElem = jointElem->GetElement("axis")->GetElement("xyz");
+ ASSERT_NE(nullptr, xyzElem);
+ EXPECT_EQ(xyzElem->Get("expressed_in"), "F2");
+ xyzElem = jointElem->GetElement("axis2")->GetElement("xyz");
+ ASSERT_NE(nullptr, xyzElem);
+ EXPECT_EQ(xyzElem->Get("expressed_in"), "F2");
+
+ jointElem = jointElem->GetNextElement("joint");
+ ASSERT_NE(nullptr, jointElem);
+ EXPECT_EQ(jointElem->Get("name"), "J2");
+ EXPECT_EQ(jointElem->Get("parent"), "L2");
+ EXPECT_EQ(jointElem->Get("child"), "L3");
+ xyzElem = jointElem->GetElement("axis")->GetElement("xyz");
+ ASSERT_NE(nullptr, xyzElem);
+ EXPECT_EQ(xyzElem->Get("expressed_in"), "__model__");
+
+ jointElem = jointElem->GetNextElement("joint");
+ ASSERT_NE(nullptr, jointElem);
+ EXPECT_EQ(jointElem->Get("name"), "J3");
+ poseElem = jointElem->GetElement("pose");
+ ASSERT_NE(nullptr, poseElem);
+ EXPECT_EQ(poseElem->Get("relative_to"), "__model__");
+ EXPECT_EQ(jointElem->Get("parent"), "L3");
+ EXPECT_EQ(jointElem->Get("child"), "L4");
+
+ nestModelElem = jointElem->GetNextElement("model");
+ ASSERT_NE(nullptr, nestModelElem);
+ EXPECT_EQ(nestModelElem->Get("name"), "M2");
+ poseElem = nestModelElem->GetElement("pose");
+ ASSERT_NE(nullptr, poseElem);
+ EXPECT_EQ(poseElem->Get("relative_to"), "F1");
+}
diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc
index 50cf7526c..754ec61fc 100644
--- a/test/integration/nested_model.cc
+++ b/test/integration/nested_model.cc
@@ -641,59 +641,84 @@ TEST(NestedModel, PartiallyFlattened)
const sdf::Model *outerModel = world->ModelByIndex(0);
ASSERT_NE(nullptr, outerModel);
EXPECT_EQ("ParentModel", outerModel->Name());
- EXPECT_EQ(4u, outerModel->LinkCount());
- EXPECT_NE(nullptr, outerModel->LinkByIndex(0));
- EXPECT_NE(nullptr, outerModel->LinkByIndex(1));
- EXPECT_NE(nullptr, outerModel->LinkByIndex(2));
- EXPECT_NE(nullptr, outerModel->LinkByIndex(3));
- EXPECT_EQ(nullptr, outerModel->LinkByIndex(4));
EXPECT_TRUE(outerModel->LinkNameExists("M1::L1"));
EXPECT_TRUE(outerModel->LinkNameExists("M1::L2"));
EXPECT_TRUE(outerModel->LinkNameExists("M1::L3"));
EXPECT_TRUE(outerModel->LinkNameExists("M1::L4"));
- EXPECT_EQ(3u, outerModel->JointCount());
EXPECT_TRUE(outerModel->JointNameExists("M1::J1"));
EXPECT_TRUE(outerModel->JointNameExists("M1::J2"));
EXPECT_TRUE(outerModel->JointNameExists("M1::J3"));
- EXPECT_EQ(3u, outerModel->FrameCount());
- EXPECT_TRUE(outerModel->FrameNameExists("M1::__model__"));
EXPECT_TRUE(outerModel->FrameNameExists("M1::F1"));
EXPECT_TRUE(outerModel->FrameNameExists("M1::F2"));
+ EXPECT_TRUE(outerModel->ModelNameExists("M1::M2"));
+
EXPECT_EQ(1u, outerModel->ModelCount());
- EXPECT_NE(nullptr, outerModel->ModelByIndex(0));
- EXPECT_EQ(nullptr, outerModel->ModelByIndex(1));
// Get the middle model
const sdf::Model *midModel = outerModel->ModelByIndex(0);
ASSERT_NE(nullptr, midModel);
- EXPECT_EQ("M1::M2", midModel->Name());
- // TODO: the following expectations should be true, but ModelNameExists
- // and ModelByName do not support names containing "::".
- // EXPECT_TRUE(outerModel->ModelNameExists("M1::M2"));
- // EXPECT_EQ(midModel, outerModel->ModelByName("M1::M2"));
-
- EXPECT_EQ(1u, midModel->LinkCount());
+ EXPECT_EQ(1u, midModel->ModelCount());
+ EXPECT_EQ("M1", midModel->Name());
+ EXPECT_EQ("L1", midModel->CanonicalLinkName());
+ EXPECT_EQ(ignition::math::Pose3d(10, 0, 0, 0, -0, 1.5708),
+ midModel->SemanticPose().RawPose());
+ EXPECT_EQ("__model__", midModel->SemanticPose().RelativeTo());
+
+ EXPECT_EQ(4u, midModel->LinkCount());
EXPECT_NE(nullptr, midModel->LinkByIndex(0));
- EXPECT_EQ(nullptr, midModel->LinkByIndex(1));
+ EXPECT_NE(nullptr, midModel->LinkByIndex(1));
+ EXPECT_NE(nullptr, midModel->LinkByIndex(2));
+ EXPECT_NE(nullptr, midModel->LinkByIndex(3));
+ EXPECT_EQ(nullptr, midModel->LinkByIndex(4));
+
+ EXPECT_TRUE(midModel->LinkNameExists("L1"));
+ EXPECT_TRUE(midModel->LinkNameExists("L2"));
+ EXPECT_TRUE(midModel->LinkNameExists("L3"));
+ EXPECT_TRUE(midModel->LinkNameExists("L4"));
+
+ EXPECT_EQ(3u, midModel->JointCount());
+ EXPECT_TRUE(midModel->JointNameExists("J1"));
+ EXPECT_TRUE(midModel->JointNameExists("J2"));
+ EXPECT_TRUE(midModel->JointNameExists("J3"));
+
+ EXPECT_EQ(2u, midModel->FrameCount());
+ EXPECT_TRUE(midModel->FrameNameExists("F1"));
+ EXPECT_TRUE(midModel->FrameNameExists("F2"));
+
+ EXPECT_EQ(1u, midModel->ModelCount());
+ EXPECT_NE(nullptr, midModel->ModelByIndex(0));
+ EXPECT_EQ(nullptr, midModel->ModelByIndex(1));
+
+ EXPECT_TRUE(midModel->ModelNameExists("M2"));
+
+ // Get the inner model of midModel
+ const sdf::Model *innerModel = midModel->ModelByIndex(0);
+ ASSERT_NE(nullptr, innerModel);
+ EXPECT_EQ("M2", innerModel->Name());
+ EXPECT_EQ(innerModel, midModel->ModelByName("M2"));
+ EXPECT_EQ(innerModel, outerModel->ModelByName("M1::M2"));
+
+ EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, -0, 0),
+ innerModel->SemanticPose().RawPose());
+ EXPECT_EQ("F1", innerModel->SemanticPose().RelativeTo());
- EXPECT_TRUE(midModel->LinkNameExists("L5"));
+ EXPECT_EQ(1u, innerModel->LinkCount());
+ EXPECT_NE(nullptr, innerModel->LinkByIndex(0));
+ EXPECT_EQ(nullptr, innerModel->LinkByIndex(1));
- EXPECT_EQ(0u, midModel->JointCount());
+ EXPECT_TRUE(innerModel->LinkNameExists("L5"));
+ EXPECT_NE(nullptr, innerModel->LinkByName("L5"));
+ EXPECT_TRUE(outerModel->LinkNameExists("M1::M2::L5"));
- EXPECT_EQ(0u, midModel->FrameCount());
+ EXPECT_EQ(0u, innerModel->JointCount());
- EXPECT_EQ(0u, midModel->ModelCount());
+ EXPECT_EQ(0u, innerModel->FrameCount());
- // test nested names from outer model
- // TODO: the following expectations also fail due to the limitations of
- // ModelNameExists and ModelByName not supporting names containing "::".
- // const std::string midLinkNestedName = "M1::M2::L5";
- // EXPECT_TRUE(outerModel->LinkNameExists(midLinkNestedName));
- // EXPECT_NE(nullptr, outerModel->LinkByName(midLinkNestedName));
+ EXPECT_EQ(0u, innerModel->ModelCount());
}
//////////////////////////////////////////////////
diff --git a/test/sdf/flattened_test_nested_model_with_frames.sdf b/test/sdf/flattened_test_nested_model_with_frames.sdf
new file mode 100644
index 000000000..0716caefd
--- /dev/null
+++ b/test/sdf/flattened_test_nested_model_with_frames.sdf
@@ -0,0 +1,82 @@
+
+
+
+
+ 0 10 0 1.5708 -0 0
+
+
+ 0 0 0 1.5708 -0 0
+
+
+ 0 0 0 0 0.785398 0
+
+
+ 0 0 0 0 -0 0
+
+ 0 0 0 0 -0 0
+
+
+ 1
+
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1
+
+
+
+
+
+ 1 0 0 0 -0 0
+
+
+ 0 1 0 0 -0 0
+
+
+ 0 0 1 0 -0 0
+
+
+ 0 0 0 0 -0 0
+ test_model_with_frames::L1
+ test_model_with_frames::L2
+
+ 0 0 1
+
+ -1e+16
+ 1e+16
+
+
+
+ 1 0 0
+
+
+
+
+ 0 0 1 0 -0 0
+ test_model_with_frames::L2
+ test_model_with_frames::L3
+
+ 0 0 1
+
+ -1e+16
+ 1e+16
+
+
+
+
+ 1 0 1 0 -0 0
+ test_model_with_frames::L3
+ test_model_with_frames::L4
+
+
+ 1 0 0 0 -0 0
+
+ 1 0 0 0 -0 0
+
+
+
+
+