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 + + + + +