From 1d09b0148a0fcd6caa68a9a1de6b3181fba9c2c6 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 29 Jul 2021 12:15:14 -0500 Subject: [PATCH 1/9] Prototype implementation of merge-include Signed-off-by: Addisu Z. Taddese --- sdf/1.9/model.sdf | 4 + src/parser.cc | 157 ++++++++- test/integration/merge_include_model.sdf | 11 + .../model/merge_robot/model.config | 5 + test/integration/model/merge_robot/model.sdf | 306 ++++++++++++++++++ 5 files changed, 482 insertions(+), 1 deletion(-) create mode 100644 test/integration/merge_include_model.sdf create mode 100644 test/integration/model/merge_robot/model.config create mode 100644 test/integration/model/merge_robot/model.sdf diff --git a/sdf/1.9/model.sdf b/sdf/1.9/model.sdf index a6d30f392..b9b92cedf 100644 --- a/sdf/1.9/model.sdf +++ b/sdf/1.9/model.sdf @@ -41,6 +41,10 @@ Include resources from a URI. This can be used to nest models. Included resources can only contain one 'model', 'light' or 'actor' element. The URI can point to a directory or a file. If the URI is a directory, it must conform to the model database structure (see /tutorials?tut=composition&cat=specification&#defining-models-in-separate-files). + + Merge the included nested model into the top model + + URI to a resource, such as a model diff --git a/src/parser.cc b/src/parser.cc index 09401c00a..93347a284 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -130,6 +130,134 @@ static inline bool _initFile(const std::string &_filename, TPtr _sdf) return initDoc(&xmlDoc, _sdf); } +////////////////////////////////////////////////// +/// Helper function to insert included elements into a parent element. +/// \param[in,out] _parent The parent element that contains the tag +/// \param[in] _includeSDF The included element (eg. model) +/// \param[in] _merge Whether the included element should be merged into the +/// parent element. If true, children elements of _includeSDF will be copied to +/// _parent without introducing a new model scope. N.B, this only works for +/// included nested models. +static void insertIncludedElement(sdf::ElementPtr _parent, + sdf::ElementPtr _includeSDF, + bool _merge = false) +{ + if (_merge && _includeSDF->GetName() == "model") + { + std::string modelName = _includeSDF->Get("name"); + + ElementPtr proxyModelFrame = _parent->AddElement("frame"); + const std::string proxyModelFrameName = "__" + modelName + "__model__"; + + proxyModelFrame->GetAttribute("name")->Set(proxyModelFrameName); + + // Determine the canonical link so the proxy frame can be attached to it + std::string canonicalLinkName = ""; + if (_includeSDF->GetAttribute("canonical_link")->GetSet()) + { + canonicalLinkName = + _includeSDF->GetAttribute("canonical_link")->GetAsString(); + } + else if (_includeSDF->HasElement("link")) + { + canonicalLinkName = + _includeSDF->GetElement("link")->GetAttribute("name")->GetAsString(); + } + + proxyModelFrame->GetAttribute("attached_to")->Set(canonicalLinkName); + + auto modelPose = _includeSDF->Get("pose"); + + ElementPtr proxyModelFramePose = proxyModelFrame->AddElement("pose"); + proxyModelFramePose->Set(modelPose); + + // Set the proxyModelFrame's //pose/@relative_to to the frame used in + // //include/pose/@relative_to. + std::string modelPoseRelativeTo = ""; + if (_includeSDF->HasElement("pose")) + { + modelPoseRelativeTo = + _includeSDF->GetElement("pose")->Get("relative_to"); + } + + // If empty, use "__model__", since leaving it empty would make it + // relative_to the canonical link frame specified in //frame/@attached_to. + if (modelPoseRelativeTo.empty()) + { + modelPoseRelativeTo = "__model__"; + } + + proxyModelFramePose->GetAttribute("relative_to")->Set(modelPoseRelativeTo); + + auto setAttributeToProxyFrame = + [&proxyModelFrameName](const std::string &_attr, sdf::ElementPtr _elem, + bool updateIfEmpty) + { + if (nullptr == _elem) + return; + + auto attribute = _elem->GetAttribute(_attr); + if (attribute->GetAsString() == "__model__" || + (updateIfEmpty && attribute->GetAsString().empty())) + { + attribute->Set(proxyModelFrameName); + } + }; + + for (auto elem = _includeSDF->GetFirstElement(); elem; + elem = elem->GetNextElement()) + { + if ((elem->GetName() == "link") || (elem->GetName() == "model")) + { + // Add a pose element even if the element doesn't originally have one + setAttributeToProxyFrame("relative_to", elem->GetElement("pose"), true); + } + else if (elem->GetName() == "frame") + { + // If //frame/@attached_to is empty, explicitly set it to the name + // of the nested model frame. + setAttributeToProxyFrame("attached_to", elem, true); + setAttributeToProxyFrame("relative_to", elem->GetElementImpl("pose"), + false); + } + else if (elem->GetName() == "joint") + { + setAttributeToProxyFrame("relative_to", elem->GetElementImpl("pose"), + false); + if (auto axis = elem->GetElementImpl("axis"); axis) + { + setAttributeToProxyFrame("expressed_in", axis->GetElementImpl("xyz"), + false); + } + + if (auto axis2 = elem->GetElementImpl("axis2"); axis2) + { + setAttributeToProxyFrame("expressed_in", axis2->GetElementImpl("xyz"), + false); + } + } + + // Only named and custom elements are copied. Other elements, such as + // , , and are ignored. + if ((elem->GetName() == "link") || (elem->GetName() == "model") || + (elem->GetName() == "joint") || (elem->GetName() == "frame") || + (elem->GetName() == "plugin") || + (elem->GetName().find(':') != std::string::npos)) + { + _parent->InsertElement(elem); + } + } + } + else if (_merge) + { + sdferr << "Merge is only supported for included models\n"; + } + else + { + _parent->InsertElement(_includeSDF); + } +} + ////////////////////////////////////////////////// bool init(SDFPtr _sdf) { @@ -1137,6 +1265,30 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, { if (std::string("include") == elemXml->Value()) { + // Validate the //include tag by calling readXml on it. This is only + // here for error checking. We won't use the resulting sdf::ElementPtr + // because the contents of the //include are accessed directly via + // tinyxml in the subsequent code. + for (unsigned int descCounter = 0; + descCounter != _sdf->GetElementDescriptionCount(); ++descCounter) + { + ElementPtr elemDesc = _sdf->GetElementDescription(descCounter); + if (elemDesc->GetName() == elemXml->Value()) + { + ElementPtr element = elemDesc->Clone(); + if (!readXml(elemXml, element, _config, _source, _errors)) + { + Error err( + ErrorCode::ELEMENT_INVALID, + std::string("Error reading element <") + + elemXml->Value() + ">", + _source, + elemXml->GetLineNum()); + _errors.push_back(err); + } + } + } + std::string modelPath; std::string uri; tinyxml2::XMLElement *uriElement = elemXml->FirstChildElement("uri"); @@ -1429,7 +1581,10 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, copyChildren(includeInfo, elemXml, false); includeSDFFirstElem->SetIncludeElement(includeInfo); } - _sdf->InsertElement(includeSDFFirstElem); + bool toMerge = _sdf->GetName() == "model" && + elemXml->BoolAttribute("merge", false); + + insertIncludedElement(_sdf, includeSDFFirstElem, toMerge); continue; } diff --git a/test/integration/merge_include_model.sdf b/test/integration/merge_include_model.sdf new file mode 100644 index 000000000..82046d1ca --- /dev/null +++ b/test/integration/merge_include_model.sdf @@ -0,0 +1,11 @@ + + + + + + merge_robot + 100 0 0 0 0 0 + + + + diff --git a/test/integration/model/merge_robot/model.config b/test/integration/model/merge_robot/model.config new file mode 100644 index 000000000..04a17df92 --- /dev/null +++ b/test/integration/model/merge_robot/model.config @@ -0,0 +1,5 @@ + + + robot + model.sdf + diff --git a/test/integration/model/merge_robot/model.sdf b/test/integration/model/merge_robot/model.sdf new file mode 100644 index 000000000..8aa0e9d65 --- /dev/null +++ b/test/integration/model/merge_robot/model.sdf @@ -0,0 +1,306 @@ + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + -0.8 0 0.41 0 0 0 + + + 0.1 + 0.25 + + + + + + -0.8 0 0.41 0 0 0 + lidar + 10 + + + + 640 + 1 + -1.396263 + 1.396263 + + + 16 + 1 + -0.261799 + 0.261799 + + + + 0.08 + 10.0 + 0.01 + + + 1 + true + + + + + + + 0.6 0 0.7 0 0 0 + + + + 0.5 1 0.5 + + + + + + + 0.5 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + 0.5 + 1 + + + + + + + -0.2 0 0.3 0 0 3.14 + + + 0.1 0.1 0.1 + + + + + + -0.2 0 0.3 0 0 3.14 + + + 0.1 0.1 0.1 + + + + 0 1 0 1 + 0 1 0 1 + 0.5 0.5 0.5 1 + 1 + + + + + + -0.2 0 0.3 0 0.0 0 + + 2 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + true + top/camera + + + + + + 1 2 3 0 0 0 + chassis + top + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + 1 0 0 0 0 0 + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + 0 0 0 0 0 0 + + + + + + val + val2 + + + + true + + From dd3c940a0c61fb7df62d14b21346384458b7ad1e Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 31 Aug 2021 09:55:46 -0500 Subject: [PATCH 2/9] Add more tests Signed-off-by: Addisu Z. Taddese --- include/sdf/Error.hh | 13 ++ src/Error.cc | 11 ++ src/Error_TEST.cc | 13 ++ src/parser.cc | 198 +++++++++++++------ test/integration/includes.cc | 197 +++++++++++++++++- test/integration/merge_include_model.sdf | 13 +- test/integration/model/merge_robot/model.sdf | 3 +- test/sdf/model_invalid_frame_only.sdf | 7 + 8 files changed, 391 insertions(+), 64 deletions(-) create mode 100644 test/sdf/model_invalid_frame_only.sdf diff --git a/include/sdf/Error.hh b/include/sdf/Error.hh index 420626109..fb920ec72 100644 --- a/include/sdf/Error.hh +++ b/include/sdf/Error.hh @@ -143,6 +143,10 @@ namespace sdf /// \brief The provided version has been deprecated or it is pre-versioning VERSION_DEPRECATED, + + /// \brief Merge include is unspported for the type of entity being + /// included. + MERGE_INCLUDE_UNSUPPORTED, }; class SDFORMAT_VISIBLE Error @@ -188,10 +192,19 @@ namespace sdf /// nullopt otherwise. public: std::optional FilePath() const; + /// \brief Sets the file path that is associated with this error. + /// \param[in] _filePath The file path that is related to this error. (e.g. + /// /tmp/test_file.sdf) + public: void SetFilePath(const std::string &_filePath); + /// \brief Get the line number associated with this error. /// \return Returns the line number. nullopt otherwise. public: std::optional LineNumber() const; + /// \brief Sets the line number that is associated with this error. + /// \param[in] _lineNumber The line number that is related to this error. + public: void SetLineNumber(int _lineNumber); + /// \brief Get the XPath-like trace that is associated with this error. /// \return Returns the XPath-like trace that this error is related to, /// nullopt otherwise. diff --git a/src/Error.cc b/src/Error.cc index de8ea66de..b80ba9f21 100644 --- a/src/Error.cc +++ b/src/Error.cc @@ -88,6 +88,11 @@ std::optional Error::FilePath() const { return this->dataPtr->filePath; } +///////////////////////////////////////////////// +void Error::SetFilePath(const std::string &_filePath) +{ + this->dataPtr->filePath = _filePath; +} ///////////////////////////////////////////////// std::optional Error::LineNumber() const @@ -95,6 +100,12 @@ std::optional Error::LineNumber() const return this->dataPtr->lineNumber; } +///////////////////////////////////////////////// +void Error::SetLineNumber(int _lineNumber) +{ + this->dataPtr->lineNumber = _lineNumber; +} + ///////////////////////////////////////////////// std::optional Error::XmlPath() const { diff --git a/src/Error_TEST.cc b/src/Error_TEST.cc index 0044e621f..5696039ec 100644 --- a/src/Error_TEST.cc +++ b/src/Error_TEST.cc @@ -32,7 +32,20 @@ TEST(Error, DefaultConstruction) EXPECT_FALSE(error.LineNumber().has_value()); if (error) + { FAIL(); + } + error.SetXmlPath("/sdf/world"); + ASSERT_TRUE(error.XmlPath().has_value()); + EXPECT_EQ("/sdf/world", error.XmlPath()); + + error.SetFilePath("/tmp/test_file.sdf"); + ASSERT_TRUE(error.FilePath().has_value()); + EXPECT_EQ("/tmp/test_file.sdf", error.FilePath()); + + error.SetLineNumber(5); + ASSERT_TRUE(error.LineNumber().has_value()); + EXPECT_EQ(5, error.LineNumber()); } ///////////////////////////////////////////////// diff --git a/src/parser.cc b/src/parser.cc index 41ccaf0a6..dc629fd3f 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -49,6 +49,43 @@ namespace sdf { inline namespace SDF_VERSION_NAMESPACE { + +namespace +{ +////////////////////////////////////////////////// +/// Holds information about the location of a particular point in an SDFormat +/// file +struct SourceLocation +{ + /// \brief Xml path where the error was raised. + public: std::optional xmlPath = std::nullopt; + + /// \brief File path where the error was raised. + public: std::optional filePath = std::nullopt; + + /// \brief Line number in the file path where the error was raised. + public: std::optional lineNumber = std::nullopt; + + /// \brief Sets the source location on an sdf::Error object + /// \param[in,out] _error sdf::Error object on which the source location is to + /// be set. + public: void SetSourceLocationOnError(sdf::Error &_error) const + { + if (this->xmlPath.has_value()) + { + _error.SetXmlPath(*this->xmlPath); + } + if (this->filePath.has_value()) + { + _error.SetFilePath(*this->filePath); + } + if (this->lineNumber.has_value()) + { + _error.SetLineNumber(*this->lineNumber); + } + } +}; +} ////////////////////////////////////////////////// /// \brief Internal helper for readFile, which populates the SDF values /// from a file @@ -134,52 +171,60 @@ static inline bool _initFile(const std::string &_filename, TPtr _sdf) ////////////////////////////////////////////////// /// Helper function to insert included elements into a parent element. /// \param[in,out] _parent The parent element that contains the tag -/// \param[in] _includeSDF The included element (eg. model) -/// \param[in] _merge Whether the included element should be merged into the -/// parent element. If true, children elements of _includeSDF will be copied to -/// _parent without introducing a new model scope. N.B, this only works for +/// \param[in] _includeRoot The DOM object corresponding to the included element +/// \param[in] _merge Whether the included element should be merged into the +/// parent element. If true, children elements of _includeSDF will be copied to +/// _parent without introducing a new model scope. N.B, this only works for /// included nested models. -static void insertIncludedElement(sdf::ElementPtr _parent, - sdf::ElementPtr _includeSDF, +static void insertIncludedElement(const sdf::Root &_includeRoot, + const SourceLocation &_sourceLoc, + sdf::ElementPtr _parent, + sdf::Errors &_errors, bool _merge = false) { - if (_merge && _includeSDF->GetName() == "model") + auto _includeSDF = _includeRoot.Element()->GetFirstElement(); + const sdf::Model *model = _includeRoot.Model(); + if (_merge && model) { - std::string modelName = _includeSDF->Get("name"); - + if (_parent->GetName() != "model") + { + Error unsupportedError( + ErrorCode::MERGE_INCLUDE_UNSUPPORTED, + "Merge-include can not supported when parent element is " + + _parent->GetName()); + _sourceLoc.SetSourceLocationOnError(unsupportedError); + _errors.push_back(unsupportedError); + } ElementPtr proxyModelFrame = _parent->AddElement("frame"); - const std::string proxyModelFrameName = "__" + modelName + "__model__"; + const std::string proxyModelFrameName = model->Name() + "__model__"; proxyModelFrame->GetAttribute("name")->Set(proxyModelFrameName); // Determine the canonical link so the proxy frame can be attached to it - std::string canonicalLinkName = ""; - if (_includeSDF->GetAttribute("canonical_link")->GetSet()) - { - canonicalLinkName = - _includeSDF->GetAttribute("canonical_link")->GetAsString(); - } - else if (_includeSDF->HasElement("link")) - { - canonicalLinkName = - _includeSDF->GetElement("link")->GetAttribute("name")->GetAsString(); - } + const std::string canonicalLinkName = + model->CanonicalLinkAndRelativeName().second; proxyModelFrame->GetAttribute("attached_to")->Set(canonicalLinkName); - auto modelPose = _includeSDF->Get("pose"); + auto modelPose = model->RawPose(); + if (!model->PlacementFrameName().empty()) + { + // M - model frame (__model__) + // R - The `relative_to` frame of the placement frame's //pose element. + // See resolveModelPoseWithPlacementFrame in FrameSemantics.cc for + // notation and documentation + ignition::math::Pose3d X_RM = model->RawPose(); + sdf::Errors resolveErrors = model->SemanticPose().Resolve(X_RM); + _errors.insert(_errors.end(), resolveErrors.begin(), resolveErrors.end()); + modelPose = X_RM; + } ElementPtr proxyModelFramePose = proxyModelFrame->AddElement("pose"); proxyModelFramePose->Set(modelPose); // Set the proxyModelFrame's //pose/@relative_to to the frame used in // //include/pose/@relative_to. - std::string modelPoseRelativeTo = ""; - if (_includeSDF->HasElement("pose")) - { - modelPoseRelativeTo = - _includeSDF->GetElement("pose")->Get("relative_to"); - } + std::string modelPoseRelativeTo = model->PoseRelativeTo(); // If empty, use "__model__", since leaving it empty would make it // relative_to the canonical link frame specified in //frame/@attached_to. @@ -205,9 +250,13 @@ static void insertIncludedElement(sdf::ElementPtr _parent, } }; - for (auto elem = _includeSDF->GetFirstElement(); elem; - elem = elem->GetNextElement()) + sdf::ElementPtr nextElem = nullptr; + for (auto elem = _includeSDF->GetFirstElement(); elem; elem = nextElem) { + // We need to fetch the next element here before we call elem->SetParent + // later in this block. + nextElem = elem->GetNextElement(); + if ((elem->GetName() == "link") || (elem->GetName() == "model")) { // Add a pose element even if the element doesn't originally have one @@ -225,6 +274,7 @@ static void insertIncludedElement(sdf::ElementPtr _parent, { setAttributeToProxyFrame("relative_to", elem->GetElementImpl("pose"), false); + // cppcheck-suppress syntaxError if (auto axis = elem->GetElementImpl("axis"); axis) { setAttributeToProxyFrame("expressed_in", axis->GetElementImpl("xyz"), @@ -245,16 +295,22 @@ static void insertIncludedElement(sdf::ElementPtr _parent, (elem->GetName() == "plugin") || (elem->GetName().find(':') != std::string::npos)) { + elem->SetParent(_parent); _parent->InsertElement(elem); } } } else if (_merge) { - sdferr << "Merge is only supported for included models\n"; + Error unsupportedError( + ErrorCode::MERGE_INCLUDE_UNSUPPORTED, + "Merge-include is only supported for included models"); + _sourceLoc.SetSourceLocationOnError(unsupportedError); + _errors.push_back(unsupportedError); } else { + _includeSDF->SetParent(_parent); _parent->InsertElement(_includeSDF); } } @@ -1144,6 +1200,42 @@ std::string getModelFilePath(const std::string &_modelDirPath) return sdf::filesystem::append(_modelDirPath, modelFileName); } +////////////////////////////////////////////////// +// Helper function called from readXml to validate the //include tag by calling +// readXml on it. This is only here for error checking. We won't use the +// resulting sdf::ElementPtr because the contents of the //include are accessed +// directly via tinyxml in the subsequent code. +/// \param[in] _xml Pointer to the TinyXML element that corresponds to the +/// element +/// \param[in,out] _sdf SDF pointer to the parent of the element +/// \param[in] _config Custom parser configuration +/// \param[in] _source Source of the XML document +/// \param[out] _errors Captures errors found during parsing. +static void validateIncludeElement(tinyxml2::XMLElement *_xml, + ElementPtr _sdf, const ParserConfig &_config, + const std::string &_source, Errors &_errors) +{ + for (unsigned int descCounter = 0; + descCounter != _sdf->GetElementDescriptionCount(); ++descCounter) + { + ElementPtr elemDesc = _sdf->GetElementDescription(descCounter); + if (elemDesc->GetName() == _xml->Value()) + { + ElementPtr element = elemDesc->Clone(); + if (!readXml(_xml, element, _config, _source, _errors)) + { + Error err( + ErrorCode::ELEMENT_INVALID, + std::string("Error reading element <") + + _xml->Value() + ">", + _source, + _xml->GetLineNum()); + _errors.push_back(err); + } + } + } +} + ////////////////////////////////////////////////// bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, const ParserConfig &_config, const std::string &_source, Errors &_errors) @@ -1332,29 +1424,7 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, { if (std::string("include") == elemXml->Value()) { - // Validate the //include tag by calling readXml on it. This is only - // here for error checking. We won't use the resulting sdf::ElementPtr - // because the contents of the //include are accessed directly via - // tinyxml in the subsequent code. - for (unsigned int descCounter = 0; - descCounter != _sdf->GetElementDescriptionCount(); ++descCounter) - { - ElementPtr elemDesc = _sdf->GetElementDescription(descCounter); - if (elemDesc->GetName() == elemXml->Value()) - { - ElementPtr element = elemDesc->Clone(); - if (!readXml(elemXml, element, _config, _source, _errors)) - { - Error err( - ErrorCode::ELEMENT_INVALID, - std::string("Error reading element <") + - elemXml->Value() + ">", - _source, - elemXml->GetLineNum()); - _errors.push_back(err); - } - } - } + validateIncludeElement(elemXml, _sdf, _config, _source, _errors); std::string modelPath; std::string uri; @@ -1638,7 +1708,6 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, } auto includeSDFFirstElem = includeSDF->Root()->GetFirstElement(); - includeSDFFirstElem->SetParent(_sdf); auto includeDesc = _sdf->GetElementDescription("include"); if (includeDesc) { @@ -1648,11 +1717,20 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, copyChildren(includeInfo, elemXml, false); includeSDFFirstElem->SetIncludeElement(includeInfo); } - bool toMerge = _sdf->GetName() == "model" && - elemXml->BoolAttribute("merge", false); - - insertIncludedElement(_sdf, includeSDFFirstElem, toMerge); - + // Validate included model's frame semantics + // We create a throwaway sdf::Root object in order to validate the + // included entity. + sdf::Root includedRoot; + sdf::Errors includeDOMerrors = includedRoot.Load(includeSDF); + _errors.insert(_errors.end(), includeDOMerrors.begin(), + includeDOMerrors.end()); + + bool toMerge = elemXml->BoolAttribute("merge", false); + SourceLocation sourceLoc{includeXmlPath, _source, + elemXml->GetLineNum()}; + + insertIncludedElement(includedRoot, sourceLoc, _sdf, _errors, + toMerge); continue; } } diff --git a/test/integration/includes.cc b/test/integration/includes.cc index 81f16da98..cd8c1e138 100644 --- a/test/integration/includes.cc +++ b/test/integration/includes.cc @@ -15,9 +15,11 @@ * */ +#include + +#include #include #include -#include #include "sdf/Actor.hh" #include "sdf/Collision.hh" @@ -388,3 +390,196 @@ TEST(IncludesTest, IncludeUrdf) EXPECT_EQ(1u, model->JointCount()); } +////////////////////////////////////////////////// +TEST(IncludesTest, MergeInclude) +{ + using ignition::math::Pose3d; + sdf::ParserConfig config; + config.SetFindCallback(findFileCb); + + sdf::Root root; + sdf::Errors errors = root.Load( + sdf::testing::TestFile("integration", "merge_include_model.sdf"), config); + ASSERT_TRUE(errors.empty()) << errors; + + auto world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + auto model = world->ModelByIndex(0); + EXPECT_EQ("robot1", model->Name()); + EXPECT_EQ(5u, model->LinkCount()); + EXPECT_EQ(4u, model->JointCount()); + EXPECT_EQ(1u, model->ModelCount()); + ASSERT_NE(nullptr, model->CanonicalLink()); + EXPECT_EQ(model->LinkByIndex(0), model->CanonicalLink()); + + auto resolvePose = [](const sdf::SemanticPose &_semPose) + { + Pose3d result; + sdf::Errors poseErrors = _semPose.Resolve(result); + EXPECT_TRUE(poseErrors.empty()) << poseErrors; + return result; + }; + + // Check some poses + { + // Link "chassis" + auto testFrame = model->LinkByName("chassis"); + ASSERT_NE(nullptr, testFrame); + Pose3d testPose = resolvePose(testFrame->SemanticPose()); + // From SDFormat file + Pose3d expectedPose = + Pose3d(100, 0, 0, 0, 0, 0) * Pose3d(-0.151427, 0, 0.175, 0, 0, 0); + EXPECT_EQ(expectedPose, testPose); + } + { + // Link "top" + auto testFrame = model->LinkByName("top"); + ASSERT_NE(nullptr, testFrame); + Pose3d testPose = resolvePose(testFrame->SemanticPose()); + // From SDFormat file + Pose3d expectedPose = + Pose3d(100, 0, 0, 0, 0, 0) * Pose3d(0.6, 0, 0.7, 0, 0, 0); + EXPECT_EQ(expectedPose, testPose); + } + + // Verify that plugins get merged + auto modelElem = model->Element(); + ASSERT_NE(nullptr, modelElem); + auto pluginElem = modelElem->FindElement("plugin"); + ASSERT_NE(nullptr, pluginElem); + EXPECT_EQ("test", pluginElem->Get("name")); + + // Verify that custom elements get merged + auto customFoo = modelElem->FindElement("custom:foo"); + ASSERT_NE(nullptr, customFoo); + EXPECT_EQ("baz", customFoo->Get("name")); + + // Verify that other non-named elements, such as and do + // *NOT* get merged. This is also true for unknown elements + EXPECT_FALSE(modelElem->HasElement("unknown_element")); + EXPECT_FALSE(modelElem->HasElement("enable_wind")); + EXPECT_FALSE(modelElem->HasElement("static")); +} + +////////////////////////////////////////////////// +TEST(IncludesTest, MergeIncludePlacementFrame) +{ + using ignition::math::Pose3d; + sdf::ParserConfig config; + config.SetFindCallback(findFileCb); + + sdf::Root root; + sdf::Errors errors = root.Load( + sdf::testing::TestFile("integration", "merge_include_model.sdf"), config); + ASSERT_TRUE(errors.empty()) << errors; + + auto world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + auto model = world->ModelByIndex(1); + EXPECT_EQ("robot2", model->Name()); + EXPECT_EQ(5u, model->LinkCount()); + EXPECT_EQ(4u, model->JointCount()); + auto topLink = model->LinkByName("top"); + ASSERT_NE(nullptr, topLink); + Pose3d topLinkPose; + EXPECT_TRUE(topLink->SemanticPose().Resolve(topLinkPose).empty()); + // From SDFormat file + Pose3d expectedtopLinkPose = Pose3d(0, 0, 2, 0, 0, 0); + EXPECT_EQ(expectedtopLinkPose, topLinkPose); +} + +////////////////////////////////////////////////// +TEST(IncludesTest, InvalidMergeInclude) +{ + sdf::ParserConfig config; + // Useing the "file://" URI scheme to allow multiple search paths + config.AddURIPath("file://", sdf::testing::TestFile("sdf")); + config.AddURIPath("file://", sdf::testing::TestFile("integration", "model")); + + // Models that are not valid by themselves + { + const std::string sdfString = R"( + + + + + file://model_invalid_frame_only.sdf + + + )"; // NOLINT + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(sdfString, config); + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(sdf::ErrorCode::MODEL_WITHOUT_LINK, errors[0].Code()); + } + { + const std::string sdfString = R"( + + + + file://model_invalid_link_relative_to.sdf + + + )"; + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(sdfString, config); + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(sdf::ErrorCode::POSE_RELATIVE_TO_INVALID, errors[0].Code()); + } + + // Actors are not supported for merging + { + const std::string sdfString = R"( + + + + file://test_actor + + + )"; + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(sdfString, config); + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(sdf::ErrorCode::MERGE_INCLUDE_UNSUPPORTED, errors[0].Code()); + EXPECT_EQ(4u, errors[0].LineNumber()); + } + + // Lights are not supported for merging + { + const std::string sdfString = R"( + + + + file://test_light + + + )"; + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(sdfString, config); + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(sdf::ErrorCode::MERGE_INCLUDE_UNSUPPORTED, errors[0].Code()); + EXPECT_EQ("Merge-include is only supported for included models", + errors[0].Message()); + EXPECT_EQ(4u, errors[0].LineNumber()); + } + + // merge-include cannot be used directly under //world + // Lights are not supported for merging + { + const std::string sdfString = R"( + + + + file://merge_robot + + + )"; + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(sdfString, config); + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(sdf::ErrorCode::MERGE_INCLUDE_UNSUPPORTED, errors[0].Code()); + EXPECT_EQ("Merge-include can not supported when parent element is world", + errors[0].Message()); + EXPECT_EQ(4u, errors[0].LineNumber()); + } +} diff --git a/test/integration/merge_include_model.sdf b/test/integration/merge_include_model.sdf index 82046d1ca..e9853c036 100644 --- a/test/integration/merge_include_model.sdf +++ b/test/integration/merge_include_model.sdf @@ -1,11 +1,20 @@ - - + + merge_robot 100 0 0 0 0 0 + + + 0 100 0 0 0 0 + + merge_robot + top + 0 0 2 0 0 0 + + diff --git a/test/integration/model/merge_robot/model.sdf b/test/integration/model/merge_robot/model.sdf index 8aa0e9d65..b3dfa86e1 100644 --- a/test/integration/model/merge_robot/model.sdf +++ b/test/integration/model/merge_robot/model.sdf @@ -300,7 +300,8 @@ val2 - true + false + diff --git a/test/sdf/model_invalid_frame_only.sdf b/test/sdf/model_invalid_frame_only.sdf new file mode 100644 index 000000000..54e1def25 --- /dev/null +++ b/test/sdf/model_invalid_frame_only.sdf @@ -0,0 +1,7 @@ + + + + + + + From 64413f3ec1619b1af559f426b34a1c4c742ca073 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 31 Aug 2021 17:03:33 -0500 Subject: [PATCH 3/9] Create throwaway sdf::Model only if merge=true Signed-off-by: Addisu Z. Taddese --- src/parser.cc | 75 +++++++++++++++++++++++---------- test/integration/includes.cc | 35 ++++++++++++--- test/sdf/invalid_xml_syntax.sdf | 10 +++++ 3 files changed, 92 insertions(+), 28 deletions(-) create mode 100644 test/sdf/invalid_xml_syntax.sdf diff --git a/src/parser.cc b/src/parser.cc index dc629fd3f..7c62d5249 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -170,21 +170,38 @@ static inline bool _initFile(const std::string &_filename, TPtr _sdf) ////////////////////////////////////////////////// /// Helper function to insert included elements into a parent element. -/// \param[in,out] _parent The parent element that contains the tag -/// \param[in] _includeRoot The DOM object corresponding to the included element +/// \param[in] _includeSDF The SDFPtr corresponding to the included element +/// \param[in] _sourceLoc The location of the include element in the file /// \param[in] _merge Whether the included element should be merged into the /// parent element. If true, children elements of _includeSDF will be copied to /// _parent without introducing a new model scope. N.B, this only works for /// included nested models. -static void insertIncludedElement(const sdf::Root &_includeRoot, - const SourceLocation &_sourceLoc, - sdf::ElementPtr _parent, - sdf::Errors &_errors, - bool _merge = false) +/// \param[in,out] _parent The parent element that contains the tag. +/// The contents of _includeSDF will be added to this. +/// \param[out] _errors Captures errors encountered during parsing. +static void insertIncludedElement(sdf::SDFPtr _includeSDF, + const SourceLocation &_sourceLoc, bool _merge, + sdf::ElementPtr _parent, sdf::Errors &_errors) { - auto _includeSDF = _includeRoot.Element()->GetFirstElement(); - const sdf::Model *model = _includeRoot.Model(); - if (_merge && model) + Error invalidFileError(ErrorCode::FILE_READ, + "Included model is invalid. Skipping model."); + _sourceLoc.SetSourceLocationOnError(invalidFileError); + + sdf::ElementPtr rootElem = _includeSDF->Root(); + if (nullptr == rootElem) + { + _errors.push_back(invalidFileError); + return; + } + + sdf::ElementPtr firstElem = rootElem->GetFirstElement(); + if (nullptr == firstElem) + { + _errors.push_back(invalidFileError); + return; + } + + if (_merge && firstElem->GetName() == "model") { if (_parent->GetName() != "model") { @@ -194,7 +211,28 @@ static void insertIncludedElement(const sdf::Root &_includeRoot, _parent->GetName()); _sourceLoc.SetSourceLocationOnError(unsupportedError); _errors.push_back(unsupportedError); + return; } + + // Validate included model's frame semantics + // We create a throwaway sdf::Root object in order to validate the + // included entity. + sdf::Root includedRoot; + sdf::Errors includeDOMerrors = includedRoot.Load(_includeSDF); + _errors.insert(_errors.end(), includeDOMerrors.begin(), + includeDOMerrors.end()); + + const sdf::Model *model = includedRoot.Model(); + if (nullptr == model) + { + Error unsupportedError( + ErrorCode::MERGE_INCLUDE_UNSUPPORTED, + "Included model is invalid. Skipping model."); + _sourceLoc.SetSourceLocationOnError(unsupportedError); + _errors.push_back(unsupportedError); + return; + } + ElementPtr proxyModelFrame = _parent->AddElement("frame"); const std::string proxyModelFrameName = model->Name() + "__model__"; @@ -251,7 +289,7 @@ static void insertIncludedElement(const sdf::Root &_includeRoot, }; sdf::ElementPtr nextElem = nullptr; - for (auto elem = _includeSDF->GetFirstElement(); elem; elem = nextElem) + for (auto elem = firstElem->GetFirstElement(); elem; elem = nextElem) { // We need to fetch the next element here before we call elem->SetParent // later in this block. @@ -310,8 +348,8 @@ static void insertIncludedElement(const sdf::Root &_includeRoot, } else { - _includeSDF->SetParent(_parent); - _parent->InsertElement(_includeSDF); + firstElem->SetParent(_parent); + _parent->InsertElement(firstElem); } } @@ -1717,20 +1755,11 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, copyChildren(includeInfo, elemXml, false); includeSDFFirstElem->SetIncludeElement(includeInfo); } - // Validate included model's frame semantics - // We create a throwaway sdf::Root object in order to validate the - // included entity. - sdf::Root includedRoot; - sdf::Errors includeDOMerrors = includedRoot.Load(includeSDF); - _errors.insert(_errors.end(), includeDOMerrors.begin(), - includeDOMerrors.end()); - bool toMerge = elemXml->BoolAttribute("merge", false); SourceLocation sourceLoc{includeXmlPath, _source, elemXml->GetLineNum()}; - insertIncludedElement(includedRoot, sourceLoc, _sdf, _errors, - toMerge); + insertIncludedElement(includeSDF, sourceLoc, toMerge, _sdf, _errors); continue; } } diff --git a/test/integration/includes.cc b/test/integration/includes.cc index cd8c1e138..837e6e201 100644 --- a/test/integration/includes.cc +++ b/test/integration/includes.cc @@ -35,6 +35,7 @@ #include "sdf/Visual.hh" #include "sdf/World.hh" #include "test_config.h" +#include "test_utils.hh" ///////////////////////////////////////////////// std::string findFileCb(const std::string &_input) @@ -400,7 +401,7 @@ TEST(IncludesTest, MergeInclude) sdf::Root root; sdf::Errors errors = root.Load( sdf::testing::TestFile("integration", "merge_include_model.sdf"), config); - ASSERT_TRUE(errors.empty()) << errors; + EXPECT_TRUE(errors.empty()) << errors; auto world = root.WorldByIndex(0); ASSERT_NE(nullptr, world); @@ -541,7 +542,7 @@ TEST(IncludesTest, InvalidMergeInclude) sdf::Errors errors = root.LoadSdfString(sdfString, config); ASSERT_FALSE(errors.empty()); EXPECT_EQ(sdf::ErrorCode::MERGE_INCLUDE_UNSUPPORTED, errors[0].Code()); - EXPECT_EQ(4u, errors[0].LineNumber()); + EXPECT_EQ(4, *errors[0].LineNumber()); } // Lights are not supported for merging @@ -560,11 +561,10 @@ TEST(IncludesTest, InvalidMergeInclude) EXPECT_EQ(sdf::ErrorCode::MERGE_INCLUDE_UNSUPPORTED, errors[0].Code()); EXPECT_EQ("Merge-include is only supported for included models", errors[0].Message()); - EXPECT_EQ(4u, errors[0].LineNumber()); + EXPECT_EQ(4, *errors[0].LineNumber()); } // merge-include cannot be used directly under //world - // Lights are not supported for merging { const std::string sdfString = R"( @@ -580,6 +580,31 @@ TEST(IncludesTest, InvalidMergeInclude) EXPECT_EQ(sdf::ErrorCode::MERGE_INCLUDE_UNSUPPORTED, errors[0].Code()); EXPECT_EQ("Merge-include can not supported when parent element is world", errors[0].Message()); - EXPECT_EQ(4u, errors[0].LineNumber()); + EXPECT_EQ(4, errors[0].LineNumber()); + } + + // Syntax error in included file + { + // Redirect sdferr output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + const std::string sdfString = R"( + + + + file://invalid_xml_syntax.sdf + + + )"; + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(sdfString, config); + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(sdf::ErrorCode::FILE_READ, errors[0].Code()); + EXPECT_EQ(0u, errors[0].Message().find("Unable to read file")); + EXPECT_EQ(5, *errors[0].LineNumber()); + EXPECT_TRUE(buffer.str().find("Error parsing XML in file") != + std::string::npos); } } diff --git a/test/sdf/invalid_xml_syntax.sdf b/test/sdf/invalid_xml_syntax.sdf new file mode 100644 index 000000000..706777d01 --- /dev/null +++ b/test/sdf/invalid_xml_syntax.sdf @@ -0,0 +1,10 @@ + + + + + + 1 0 0 0 0 0 + + + + From 923cc729451f4aa98b801ac7fec54323d726d5b6 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 7 Sep 2021 20:33:17 -0500 Subject: [PATCH 4/9] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- src/parser.cc | 251 +++++++++---------- test/integration/includes.cc | 81 +++++- test/integration/merge_include_model.sdf | 6 +- test/integration/model/merge_robot/model.sdf | 7 +- test/sdf/model_invalid_frame_only.sdf | 2 +- 5 files changed, 206 insertions(+), 141 deletions(-) diff --git a/src/parser.cc b/src/parser.cc index db486f92c..4d81f59cc 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -201,155 +201,154 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF, return; } - if (_merge && firstElem->GetName() == "model") + if (!_merge) { - if (_parent->GetName() != "model") - { - Error unsupportedError( - ErrorCode::MERGE_INCLUDE_UNSUPPORTED, - "Merge-include can not supported when parent element is " + - _parent->GetName()); - _sourceLoc.SetSourceLocationOnError(unsupportedError); - _errors.push_back(unsupportedError); - return; - } + firstElem->SetParent(_parent); + _parent->InsertElement(firstElem); + return; + } + else if (firstElem->GetName() != "model") + { + Error unsupportedError( + ErrorCode::MERGE_INCLUDE_UNSUPPORTED, + "Merge-include is only supported for included models"); + _sourceLoc.SetSourceLocationOnError(unsupportedError); + _errors.push_back(unsupportedError); + return; + } + else if (_parent->GetName() != "model") + { + Error unsupportedError( + ErrorCode::MERGE_INCLUDE_UNSUPPORTED, + "Merge-include does not support parent element of type " + + _parent->GetName()); + _sourceLoc.SetSourceLocationOnError(unsupportedError); + _errors.push_back(unsupportedError); + return; + } - // Validate included model's frame semantics - // We create a throwaway sdf::Root object in order to validate the - // included entity. - sdf::Root includedRoot; - sdf::Errors includeDOMerrors = includedRoot.Load(_includeSDF); - _errors.insert(_errors.end(), includeDOMerrors.begin(), - includeDOMerrors.end()); + // Validate included model's frame semantics + // We create a throwaway sdf::Root object in order to validate the + // included entity. + sdf::Root includedRoot; + sdf::Errors includeDOMerrors = includedRoot.Load(_includeSDF); + _errors.insert(_errors.end(), includeDOMerrors.begin(), + includeDOMerrors.end()); - const sdf::Model *model = includedRoot.Model(); - if (nullptr == model) - { - Error unsupportedError( - ErrorCode::MERGE_INCLUDE_UNSUPPORTED, - "Included model is invalid. Skipping model."); - _sourceLoc.SetSourceLocationOnError(unsupportedError); - _errors.push_back(unsupportedError); - return; - } + const sdf::Model *model = includedRoot.Model(); + if (nullptr == model) + { + Error unsupportedError(ErrorCode::MERGE_INCLUDE_UNSUPPORTED, + "Included model is invalid. Skipping model."); + _sourceLoc.SetSourceLocationOnError(unsupportedError); + _errors.push_back(unsupportedError); + return; + } - ElementPtr proxyModelFrame = _parent->AddElement("frame"); - const std::string proxyModelFrameName = model->Name() + "__model__"; + ElementPtr proxyModelFrame = _parent->AddElement("frame"); + const std::string proxyModelFrameName = model->Name() + "__model__"; - proxyModelFrame->GetAttribute("name")->Set(proxyModelFrameName); + proxyModelFrame->GetAttribute("name")->Set(proxyModelFrameName); - // Determine the canonical link so the proxy frame can be attached to it - const std::string canonicalLinkName = - model->CanonicalLinkAndRelativeName().second; + // Determine the canonical link so the proxy frame can be attached to it + const std::string canonicalLinkName = + model->CanonicalLinkAndRelativeName().second; - proxyModelFrame->GetAttribute("attached_to")->Set(canonicalLinkName); + proxyModelFrame->GetAttribute("attached_to")->Set(canonicalLinkName); - auto modelPose = model->RawPose(); - if (!model->PlacementFrameName().empty()) - { - // M - model frame (__model__) - // R - The `relative_to` frame of the placement frame's //pose element. - // See resolveModelPoseWithPlacementFrame in FrameSemantics.cc for - // notation and documentation - ignition::math::Pose3d X_RM = model->RawPose(); - sdf::Errors resolveErrors = model->SemanticPose().Resolve(X_RM); - _errors.insert(_errors.end(), resolveErrors.begin(), resolveErrors.end()); - modelPose = X_RM; - } + auto modelPose = model->RawPose(); + if (!model->PlacementFrameName().empty()) + { + // M - model frame (__model__) + // R - The `relative_to` frame of the placement frame's //pose element. + // See resolveModelPoseWithPlacementFrame in FrameSemantics.cc for + // notation and documentation + ignition::math::Pose3d X_RM = model->RawPose(); + sdf::Errors resolveErrors = model->SemanticPose().Resolve(X_RM); + _errors.insert(_errors.end(), resolveErrors.begin(), resolveErrors.end()); + modelPose = X_RM; + } - ElementPtr proxyModelFramePose = proxyModelFrame->AddElement("pose"); - proxyModelFramePose->Set(modelPose); + ElementPtr proxyModelFramePose = proxyModelFrame->AddElement("pose"); + proxyModelFramePose->Set(modelPose); - // Set the proxyModelFrame's //pose/@relative_to to the frame used in - // //include/pose/@relative_to. - std::string modelPoseRelativeTo = model->PoseRelativeTo(); + // Set the proxyModelFrame's //pose/@relative_to to the frame used in + // //include/pose/@relative_to. + std::string modelPoseRelativeTo = model->PoseRelativeTo(); - // If empty, use "__model__", since leaving it empty would make it - // relative_to the canonical link frame specified in //frame/@attached_to. - if (modelPoseRelativeTo.empty()) - { - modelPoseRelativeTo = "__model__"; - } + // If empty, use "__model__", since leaving it empty would make it + // relative_to the canonical link frame specified in //frame/@attached_to. + if (modelPoseRelativeTo.empty()) + { + modelPoseRelativeTo = "__model__"; + } - proxyModelFramePose->GetAttribute("relative_to")->Set(modelPoseRelativeTo); + proxyModelFramePose->GetAttribute("relative_to")->Set(modelPoseRelativeTo); - auto setAttributeToProxyFrame = + auto setAttributeToProxyFrame = [&proxyModelFrameName](const std::string &_attr, sdf::ElementPtr _elem, - bool updateIfEmpty) - { - if (nullptr == _elem) - return; - - auto attribute = _elem->GetAttribute(_attr); - if (attribute->GetAsString() == "__model__" || - (updateIfEmpty && attribute->GetAsString().empty())) - { - attribute->Set(proxyModelFrameName); - } - }; + bool updateIfEmpty) + { + if (nullptr == _elem) + return; - sdf::ElementPtr nextElem = nullptr; - for (auto elem = firstElem->GetFirstElement(); elem; elem = nextElem) + auto attribute = _elem->GetAttribute(_attr); + if (attribute->GetAsString() == "__model__" || + (updateIfEmpty && attribute->GetAsString().empty())) { - // We need to fetch the next element here before we call elem->SetParent - // later in this block. - nextElem = elem->GetNextElement(); + attribute->Set(proxyModelFrameName); + } + }; - if ((elem->GetName() == "link") || (elem->GetName() == "model")) - { - // Add a pose element even if the element doesn't originally have one - setAttributeToProxyFrame("relative_to", elem->GetElement("pose"), true); - } - else if (elem->GetName() == "frame") - { - // If //frame/@attached_to is empty, explicitly set it to the name - // of the nested model frame. - setAttributeToProxyFrame("attached_to", elem, true); - setAttributeToProxyFrame("relative_to", elem->GetElementImpl("pose"), - false); - } - else if (elem->GetName() == "joint") - { - setAttributeToProxyFrame("relative_to", elem->GetElementImpl("pose"), - false); - // cppcheck-suppress syntaxError - if (auto axis = elem->GetElementImpl("axis"); axis) - { - setAttributeToProxyFrame("expressed_in", axis->GetElementImpl("xyz"), - false); - } + sdf::ElementPtr nextElem = nullptr; + for (auto elem = firstElem->GetFirstElement(); elem; elem = nextElem) + { + // We need to fetch the next element here before we call elem->SetParent + // later in this block. + nextElem = elem->GetNextElement(); - if (auto axis2 = elem->GetElementImpl("axis2"); axis2) - { - setAttributeToProxyFrame("expressed_in", axis2->GetElementImpl("xyz"), - false); - } + if ((elem->GetName() == "link") || (elem->GetName() == "model")) + { + // Add a pose element even if the element doesn't originally have one + setAttributeToProxyFrame("relative_to", elem->GetElement("pose"), true); + } + else if (elem->GetName() == "frame") + { + // If //frame/@attached_to is empty, explicitly set it to the name + // of the nested model frame. + setAttributeToProxyFrame("attached_to", elem, true); + setAttributeToProxyFrame("relative_to", elem->GetElementImpl("pose"), + false); + } + else if (elem->GetName() == "joint") + { + setAttributeToProxyFrame("relative_to", elem->GetElementImpl("pose"), + false); + // cppcheck-suppress syntaxError + // cppcheck-suppress unmatchedSuppression + if (auto axis = elem->GetElementImpl("axis"); axis) + { + setAttributeToProxyFrame("expressed_in", axis->GetElementImpl("xyz"), + false); } - // Only named and custom elements are copied. Other elements, such as - // , , and are ignored. - if ((elem->GetName() == "link") || (elem->GetName() == "model") || - (elem->GetName() == "joint") || (elem->GetName() == "frame") || - (elem->GetName() == "plugin") || - (elem->GetName().find(':') != std::string::npos)) + if (auto axis2 = elem->GetElementImpl("axis2"); axis2) { - elem->SetParent(_parent); - _parent->InsertElement(elem); + setAttributeToProxyFrame("expressed_in", axis2->GetElementImpl("xyz"), + false); } } - } - else if (_merge) - { - Error unsupportedError( - ErrorCode::MERGE_INCLUDE_UNSUPPORTED, - "Merge-include is only supported for included models"); - _sourceLoc.SetSourceLocationOnError(unsupportedError); - _errors.push_back(unsupportedError); - } - else - { - firstElem->SetParent(_parent); - _parent->InsertElement(firstElem); + + // Only named and custom elements are copied. Other elements, such as + // , , and are ignored. + if ((elem->GetName() == "link") || (elem->GetName() == "model") || + (elem->GetName() == "joint") || (elem->GetName() == "frame") || + (elem->GetName() == "gripper") || (elem->GetName() == "plugin") || + (elem->GetName().find(':') != std::string::npos)) + { + elem->SetParent(_parent); + _parent->InsertElement(elem); + } } } diff --git a/test/integration/includes.cc b/test/integration/includes.cc index 837e6e201..3e93cd72a 100644 --- a/test/integration/includes.cc +++ b/test/integration/includes.cc @@ -24,7 +24,10 @@ #include "sdf/Actor.hh" #include "sdf/Collision.hh" #include "sdf/Filesystem.hh" +#include "sdf/Frame.hh" #include "sdf/Geometry.hh" +#include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" #include "sdf/Light.hh" #include "sdf/Link.hh" #include "sdf/Mesh.hh" @@ -395,6 +398,7 @@ TEST(IncludesTest, IncludeUrdf) TEST(IncludesTest, MergeInclude) { using ignition::math::Pose3d; + using ignition::math::Vector3d; sdf::ParserConfig config; config.SetFindCallback(findFileCb); @@ -421,28 +425,87 @@ TEST(IncludesTest, MergeInclude) return result; }; + // X_PM - Pose of original model (M) in parent model (P) frame. This is the + // pose override in the //include tag. + const Pose3d X_PM(100, 0, 0, IGN_PI_4, 0, 0); + // X_MRw - Pose of the right wheel in the original model (M) as specified in + // the SDFormat file. + const Pose3d X_MRw(0.554282, -0.625029, -0.025, -1.5707, 0, 0); + // X_MLw - Pose of the left wheel in the original model (M) as specified in + // the SDF file. + const Pose3d X_MLw(0.554282, 0.625029, -0.025, -1.5707, 0, 0); // Check some poses { // Link "chassis" auto testFrame = model->LinkByName("chassis"); ASSERT_NE(nullptr, testFrame); - Pose3d testPose = resolvePose(testFrame->SemanticPose()); - // From SDFormat file - Pose3d expectedPose = - Pose3d(100, 0, 0, 0, 0, 0) * Pose3d(-0.151427, 0, 0.175, 0, 0, 0); + const Pose3d testPose = resolvePose(testFrame->SemanticPose()); + // X_MC - Pose of chassis link(C) in the original model (M) as specified in + // the SDF file. + const Pose3d X_MC(-0.151427, 0, 0.175, 0, 0, 0); + const Pose3d expectedPose = X_PM * X_MC; EXPECT_EQ(expectedPose, testPose); } { // Link "top" auto testFrame = model->LinkByName("top"); ASSERT_NE(nullptr, testFrame); - Pose3d testPose = resolvePose(testFrame->SemanticPose()); + const Pose3d testPose = resolvePose(testFrame->SemanticPose()); + // From SDFormat file + // X_MT - Pose of top link(T) in the original model (M) as specified in + // the SDF file. + const Pose3d X_MT(0.6, 0, 0.7, 0, 0, 0); + const Pose3d expectedPose = X_PM * X_MT; + EXPECT_EQ(expectedPose, testPose); + } + { + // The pose of right_wheel_joint is specified relative to __model__. + auto testFrame = model->JointByName("right_wheel_joint"); + ASSERT_NE(nullptr, testFrame); + // Resolve the pose relative to it's child frame (right_wheel) + const Pose3d testPose = resolvePose(testFrame->SemanticPose()); + // From SDFormat file + // X_MJr - Pose of right_wheel_joint (Jr) in the original model (M) as + // specified in the SDF file. + const Pose3d X_MJr(1, 0, 0, 0, 0, 0); + const Pose3d expectedPose = X_MRw.Inverse() * X_MJr; + EXPECT_EQ(expectedPose, testPose); + } + { + // The pose of sensor_frame is specified relative to __model__. + auto testFrame = model->FrameByName("sensor_frame"); + ASSERT_NE(nullptr, testFrame); + // Resolve the pose relative to it's child frame (right_wheel) + const Pose3d testPose = resolvePose(testFrame->SemanticPose()); // From SDFormat file - Pose3d expectedPose = - Pose3d(100, 0, 0, 0, 0, 0) * Pose3d(0.6, 0, 0.7, 0, 0, 0); + // X_MS - Pose of sensor_frame (S) in the original model (M) as + // specified in the SDF file. + const Pose3d X_MS(0, 1, 0, 0, IGN_PI_4, 0); + const Pose3d expectedPose = X_PM * X_MS; EXPECT_EQ(expectedPose, testPose); } + // Check joint axes + { + // left_wheel_joint's axis is expressed in __model__. + auto joint = model->JointByName("left_wheel_joint"); + ASSERT_NE(nullptr, joint); + auto axis = joint->Axis(0); + ASSERT_NE(nullptr, axis); + Vector3d xyz; + sdf::Errors resolveErrors = axis->ResolveXyz(xyz); + EXPECT_TRUE(resolveErrors.empty()) << resolveErrors; + // From SDFormat file + // R_MJl - Rotation of left_wheel_joint (Jl) in the original model (M) as + // specified in the SDF file. This is the same as R_MLw since //joint/pose + // is identity. + const auto R_MJl = X_MLw.Rot(); + Vector3d xyzInOrigModel(0, 0, 1); + Vector3d expectedXyz = R_MJl.Inverse() * xyzInOrigModel; + EXPECT_EQ(expectedXyz, xyz); + } + + // Verify that plugins get merged auto modelElem = model->Element(); ASSERT_NE(nullptr, modelElem); @@ -493,7 +556,7 @@ TEST(IncludesTest, MergeIncludePlacementFrame) TEST(IncludesTest, InvalidMergeInclude) { sdf::ParserConfig config; - // Useing the "file://" URI scheme to allow multiple search paths + // Using the "file://" URI scheme to allow multiple search paths config.AddURIPath("file://", sdf::testing::TestFile("sdf")); config.AddURIPath("file://", sdf::testing::TestFile("integration", "model")); @@ -578,7 +641,7 @@ TEST(IncludesTest, InvalidMergeInclude) sdf::Errors errors = root.LoadSdfString(sdfString, config); ASSERT_FALSE(errors.empty()); EXPECT_EQ(sdf::ErrorCode::MERGE_INCLUDE_UNSUPPORTED, errors[0].Code()); - EXPECT_EQ("Merge-include can not supported when parent element is world", + EXPECT_EQ("Merge-include does not support parent element of type world", errors[0].Message()); EXPECT_EQ(4, errors[0].LineNumber()); } diff --git a/test/integration/merge_include_model.sdf b/test/integration/merge_include_model.sdf index e9853c036..06ab5584b 100644 --- a/test/integration/merge_include_model.sdf +++ b/test/integration/merge_include_model.sdf @@ -4,16 +4,16 @@ merge_robot - 100 0 0 0 0 0 + 100 0 0 0.7853981633974483 0 0 - 0 100 0 0 0 0 + 0 100 0 0 0 0 merge_robot top - 0 0 2 0 0 0 + 0 0 2 0 0 0 diff --git a/test/integration/model/merge_robot/model.sdf b/test/integration/model/merge_robot/model.sdf index b3dfa86e1..fee967a2f 100644 --- a/test/integration/model/merge_robot/model.sdf +++ b/test/integration/model/merge_robot/model.sdf @@ -1,5 +1,5 @@ - + 0 0 0.325 0 -0 0 @@ -263,7 +263,7 @@ chassis left_wheel - 0 0 1 + 0 0 1 -1.79769e+308 1.79769e+308 @@ -289,6 +289,9 @@ caster + + 0 1 0 0 45 0 + 0 0 0 0 0 0 diff --git a/test/sdf/model_invalid_frame_only.sdf b/test/sdf/model_invalid_frame_only.sdf index 54e1def25..9d4109326 100644 --- a/test/sdf/model_invalid_frame_only.sdf +++ b/test/sdf/model_invalid_frame_only.sdf @@ -2,6 +2,6 @@ - + From e7dc8be3e1ec2b89a7d6564deeccc1129c45d0e0 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 14 Sep 2021 17:04:39 -0500 Subject: [PATCH 5/9] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- test/integration/includes.cc | 2 +- test/integration/model/merge_robot/model.config | 2 +- test/integration/model/merge_robot/model.sdf | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/test/integration/includes.cc b/test/integration/includes.cc index 3e93cd72a..679e2125b 100644 --- a/test/integration/includes.cc +++ b/test/integration/includes.cc @@ -668,6 +668,6 @@ TEST(IncludesTest, InvalidMergeInclude) EXPECT_EQ(0u, errors[0].Message().find("Unable to read file")); EXPECT_EQ(5, *errors[0].LineNumber()); EXPECT_TRUE(buffer.str().find("Error parsing XML in file") != - std::string::npos); + std::string::npos) << buffer.str(); } } diff --git a/test/integration/model/merge_robot/model.config b/test/integration/model/merge_robot/model.config index 04a17df92..d49445a0e 100644 --- a/test/integration/model/merge_robot/model.config +++ b/test/integration/model/merge_robot/model.config @@ -1,5 +1,5 @@ robot - model.sdf + model.sdf diff --git a/test/integration/model/merge_robot/model.sdf b/test/integration/model/merge_robot/model.sdf index fee967a2f..4215df4a7 100644 --- a/test/integration/model/merge_robot/model.sdf +++ b/test/integration/model/merge_robot/model.sdf @@ -1,5 +1,5 @@ - + 0 0 0.325 0 -0 0 From 2a44f68fb56d3623dba999d11d67c4ca622dec75 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 14 Sep 2021 16:57:49 -0500 Subject: [PATCH 6/9] PIMPLize sdf::NestedInclude Signed-off-by: Addisu Z. Taddese --- doc/sdf.in | 2 +- include/sdf/InterfaceElements.hh | 145 +++++++++++++++++++++++++++-- src/CMakeLists.txt | 1 + src/FrameSemantics.cc | 16 ++-- src/InterfaceElements.cc | 146 ++++++++++++++++++++++++++++++ src/Utils.cc | 24 ++--- test/integration/interface_api.cc | 74 +++++++-------- 7 files changed, 342 insertions(+), 66 deletions(-) create mode 100644 src/InterfaceElements.cc diff --git a/doc/sdf.in b/doc/sdf.in index 39b19d004..7621f1844 100644 --- a/doc/sdf.in +++ b/doc/sdf.in @@ -1510,7 +1510,7 @@ SEARCH_INCLUDES = YES # contain include files that are not input files but should be processed by # the preprocessor. -INCLUDE_PATH = . +INCLUDE_PATH = @PROJECT_BINARY_DIR@ @PROJECT_SOURCE_DIR@/include # You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard # patterns (like *.h and *.hpp) to filter out the header-files in the diff --git a/include/sdf/InterfaceElements.hh b/include/sdf/InterfaceElements.hh index 922c77164..e7a881408 100644 --- a/include/sdf/InterfaceElements.hh +++ b/include/sdf/InterfaceElements.hh @@ -21,6 +21,7 @@ #include #include +#include #include "sdf/Element.hh" #include "sdf/InterfaceModel.hh" @@ -40,53 +41,177 @@ inline namespace SDF_VERSION_NAMESPACE #endif /// \brief Contains the necessary information about an included model file /// for custom model parsers to be able to find the file and parse it. -struct SDFORMAT_VISIBLE NestedInclude +class SDFORMAT_VISIBLE NestedInclude { + /// \brief Constructor + public: NestedInclude(); + // Defaulted copy, move constructors and destructors are needed to avoid + // deprecation warnings on memeber variables when simply instantiating this + // class. + // TODO(anyone) Remove the constructor and destructor once the depreceted + // members are removed. + SDF_SUPPRESS_DEPRECATED_BEGIN + public: NestedInclude(const NestedInclude&) = default; + public: NestedInclude(NestedInclude&&) = default; + public: NestedInclude& operator=(const NestedInclude&) = default; + public: NestedInclude& operator=(NestedInclude&&) = default; + public: ~NestedInclude() = default; + SDF_SUPPRESS_DEPRECATED_END + + /// \brief Provides the URI as specified in `//include/uri`. This may or may + /// not end with a file extension (it will not end with an extension if it + /// refers to a model package). + /// \return URI of the included model + public: const std::string &Uri() const; + + /// \brief Set the URI of the included model + /// \param[in] _uri URI of the included model + public: void SetUri(const std::string &_uri); + + /// \brief Provides the *resolved* absolute file path from the URI. + /// It is recommended to use this in `CustomModelParser` when checking + /// predicates on filenames -- however, the predicates should generally only + /// check the file extension. + /// \return The resolved absolute file path from the URI. + public: const std::string &ResolvedFileName() const; + + /// \brief Set the resolved absolute file path. + /// \param[in] _resolvedFileName The resolved absolute file path + public: void SetResolvedFileName(const std::string &_resolvedFileName); + + /// \brief Name of the parent entity in absolute hierarchy. + /// Example: if the interface model's name is + /// `top_model::middle_model::my_new_model`, the absoluteParentName would be + /// `top_model::middle_model`. If the parent entity is the world, this would + /// be an empty string. + /// \return Absolute name of parent entity + public: const std::string &AbsoluteParentName() const; + + /// \brief Set the absolute name of parent entity + /// \param[in] _absoluteparentname Absolute name of parent entity + public: void SetAbsoluteParentName(const std::string &_absoluteparentname); + + /// \brief Name relative to immediate parent as specified in + /// `//include/name`. This is nullopt if `//include/name` is not set. Then the + /// name of the model must be determined by the custom model parser from the + /// included model file. + /// Example: `my_new_model` + /// \return The local name. nullopt if `//include/name` is not set + public: const std::optional &LocalModelName() const; + + /// \brief Set the name relative to immediate parent as specified in + /// `//include/name` + /// \param[in] _localModelName The local name + public: void SetLocalModelName(const std::string &_localModelName); + + /// \brief Whether the model is static as defined by `//include/static`. This + /// is nullopt if `//include/static` is not set. + /// \return Whether the model is static. nullopt if `//include/static` is not + /// set. + public: const std::optional &IsStatic() const; + + /// \brief Set whether the model is static. + /// \param[in] _isStatic True if the model is static. + public: void SetIsStatic(bool _isStatic); + + /// \brief The raw pose as specified in `//include/pose`. This is nullopt if + /// `//include/pose` is not set. + /// \return The raw pose. nullopt if `//include/pose` is not set. + public: const std::optional &IncludeRawPose() const; + + /// \brief Set the raw pose as specified in `//include/pose`. + /// \param[in] _includeRawPose The raw pose + public: void SetIncludeRawPose(const ignition::math::Pose3d &_includeRawPose); + + /// \brief The relative-to frame of the pose as specified in + /// `//include/pose/@relative_to`. This is nullopt if + /// `//include/pose/@relative_to` is not set. + /// \return The relative-to frame of the pose. nullopt if + /// `//include/pose/@relative_to` is not set. + public: const std::optional &IncludePoseRelativeTo() const; + + /// \brief Set the relative-to frame of the pose. + /// \param[in] _includePoseRelativeTo The relative-to frame. + public: void SetIncludePoseRelativeTo( + const std::string &_includePoseRelativeTo); + + /// \brief The placement frame as specified in `//include/placement_frame`. + /// This is nullopt if `//include/placement_frame` is is not set. + /// \return The placement frame. nullopt if `//include/placement_frame` is is + /// not set. + public: const std::optional &PlacementFrame() const; + + /// \brief Set the placement frame. + /// \param[in] _placementFrame The placement frame. + public: void SetPlacementFrame(const std::string &_placementFrame); + + /// This is the `//include` element. This can be used to pass custom elements + /// and attributes to the custom model parser. + /// \return The `//include` element + public: sdf::ElementPtr IncludeElement() const; + + /// Set the `//include` element. + /// \param[in] _includeElement The include element + public: void SetIncludeElement(sdf::ElementPtr _includeElement); + /// \brief Provides the URI as specified in `//include/uri`. This may or may /// not end with a file extension (it will not end with an extension if it /// refers to a model package). - std::string uri; + /// \deprecated Use NestedInclude::Uri() instead + public: std::string uri SDF_DEPRECATED(12); /// \brief Provides the *resolved* absolute file path from the URI. /// It is recommended to use this in `CustomModelParser` when checking /// predicates on filenames -- however, the predicates should generally only /// check the file extension. - std::string resolvedFileName; + /// \deprecated Use NestedInclude::ResolvedFileName() instead + public: std::string resolvedFileName SDF_DEPRECATED(12); /// \brief Name of the parent entity in absolute hierarchy. /// Example: if the interface model's name is /// `top_model::middle_model::my_new_model`, the absoluteParentName would be /// `top_model::middle_model`. If the parent entity is the world, this would /// be an empty string. - std::string absoluteParentName; + /// \deprecated Use NestedInclude::AbsoluteParentName() instead + public: std::string absoluteParentName SDF_DEPRECATED(12); /// \brief Name relative to immediate parent as specified in /// `//include/name`. This is nullopt if `//include/name` is not set. Then the /// name of the model must be determined by the custom model parser from the /// included model file. /// Example: `my_new_model` - std::optional localModelName; + /// \deprecated Use NestedInclude::LocalModelName() instead + public: std::optional localModelName SDF_DEPRECATED(12); /// \brief Whether the model is static as defined by `//include/static`. This /// is nullopt if `//include/static` is not set. - std::optional isStatic; + /// \deprecated Use NestedInclude::IsStatic() instead + public: std::optional isStatic SDF_DEPRECATED(12); /// \brief The raw pose as specified in //include/pose. This is nullopt if /// `//include/pose` is not set. - std::optional includeRawPose; + /// \deprecated Use NestedInclude::IncludeRawPose() instead + public: std::optional includeRawPose + SDF_DEPRECATED(12); /// \brief The relative-to frame of the pose as specified in /// `//include/pose/@relative_to`. This is nullopt if /// `//include/pose/@relative_to` is not set. - std::optional includePoseRelativeTo; + /// \deprecated Use NestedInclude::IncludePoseRelativeTo() instead + public: std::optional includePoseRelativeTo SDF_DEPRECATED(12); /// \brief The placement frame as specified in `//include/placement_frame`. /// This is nullopt if `//include/placement_frame` is is not set. - std::optional placementFrame; + /// \deprecated Use NestedInclude::PlacementFrame() instead + public: std::optional placementFrame SDF_DEPRECATED(12); /// This is the `//include` element. This can be used to pass custom elements /// and attributes to the custom model parser. - sdf::ElementPtr includeElement; + /// \deprecated Use NestedInclude::IncludeElement() instead + public: sdf::ElementPtr includeElement SDF_DEPRECATED(12); + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) }; #ifdef _MSC_VER #pragma warning(pop) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 503fd7599..41a0227c1 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -33,6 +33,7 @@ set (sources Gui.cc Heightmap.cc ign.cc + InterfaceElements.cc InterfaceFrame.cc InterfaceJoint.cc InterfaceLink.cc diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 94893dcf5..d325b4497 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -1154,7 +1154,7 @@ Errors buildPoseRelativeToGraph( auto relativeToId = modelFrameId; const std::string &relativeTo = - nestedInclude->includePoseRelativeTo.value_or(""); + nestedInclude->IncludePoseRelativeTo().value_or(""); if (!relativeTo.empty()) { // look for vertex in graph that matches relative_to value @@ -1180,11 +1180,11 @@ Errors buildPoseRelativeToGraph( } ignition::math::Pose3d resolvedModelPose = - nestedInclude->includeRawPose.value_or(ignition::math::Pose3d()); + nestedInclude->IncludeRawPose().value_or(ignition::math::Pose3d()); sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame( - nestedInclude->includeRawPose.value_or(ignition::math::Pose3d()), - nestedInclude->placementFrame.value_or(""), + nestedInclude->IncludeRawPose().value_or(ignition::math::Pose3d()), + nestedInclude->PlacementFrame().value_or(""), outModel.ChildModelScope(ifaceModel->Name()), resolvedModelPose); errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); @@ -1517,7 +1517,7 @@ Errors buildPoseRelativeToGraph( auto relativeToId = worldFrameId; const std::string &relativeTo = - nestedInclude->includePoseRelativeTo.value_or(""); + nestedInclude->IncludePoseRelativeTo().value_or(""); if (!relativeTo.empty()) { // look for vertex in graph that matches relative_to value @@ -1543,11 +1543,11 @@ Errors buildPoseRelativeToGraph( } ignition::math::Pose3d resolvedModelPose = - nestedInclude->includeRawPose.value_or(ignition::math::Pose3d()); + nestedInclude->IncludeRawPose().value_or(ignition::math::Pose3d()); sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame( - nestedInclude->includeRawPose.value_or(ignition::math::Pose3d()), - nestedInclude->placementFrame.value_or(""), + nestedInclude->IncludeRawPose().value_or(ignition::math::Pose3d()), + nestedInclude->PlacementFrame().value_or(""), _out.ChildModelScope(ifaceModel->Name()), resolvedModelPose); errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); diff --git a/src/InterfaceElements.cc b/src/InterfaceElements.cc new file mode 100644 index 000000000..be42977d4 --- /dev/null +++ b/src/InterfaceElements.cc @@ -0,0 +1,146 @@ +/* + * Copyright 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "sdf/InterfaceElements.hh" + +using namespace sdf; + +class sdf::NestedInclude::Implementation +{ + +}; + +SDF_SUPPRESS_DEPRECATED_BEGIN +///////////////////////////////////////////////// +NestedInclude::NestedInclude() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +const std::string &NestedInclude::Uri() const +{ + return this->uri; +} + +///////////////////////////////////////////////// +void NestedInclude::SetUri(const std::string &_uri) +{ + this->uri = _uri; +} + +///////////////////////////////////////////////// +const std::string &NestedInclude::ResolvedFileName() const +{ + return this->resolvedFileName; +} + +///////////////////////////////////////////////// +void NestedInclude::SetResolvedFileName(const std::string &_resolvedFileName) +{ + this->resolvedFileName = _resolvedFileName; +} + +///////////////////////////////////////////////// +const std::string &NestedInclude::AbsoluteParentName() const +{ + return this->absoluteParentName; +} + +///////////////////////////////////////////////// +void NestedInclude::SetAbsoluteParentName( + const std::string &_absoluteParentName) +{ + this->absoluteParentName = _absoluteParentName; +} + +///////////////////////////////////////////////// +const std::optional &NestedInclude::LocalModelName() const +{ + return this->localModelName; +} + +///////////////////////////////////////////////// +void NestedInclude::SetLocalModelName(const std::string &_localModelName) +{ + this->localModelName = _localModelName; +} + +///////////////////////////////////////////////// +const std::optional &NestedInclude::IsStatic() const +{ + return this->isStatic; + +} + +///////////////////////////////////////////////// +void NestedInclude::SetIsStatic(bool _isStatic) +{ + this->isStatic = _isStatic; +} + +///////////////////////////////////////////////// +const std::optional &NestedInclude::IncludeRawPose() + const +{ + return this->includeRawPose; +} + +///////////////////////////////////////////////// +void NestedInclude::SetIncludeRawPose( + const ignition::math::Pose3d &_includeRawPose) +{ + this->includeRawPose = _includeRawPose; +} + +///////////////////////////////////////////////// +const std::optional &NestedInclude::IncludePoseRelativeTo() const +{ + return this->includePoseRelativeTo; +} + +///////////////////////////////////////////////// +void NestedInclude::SetIncludePoseRelativeTo( + const std::string &_includePoseRelativeTo) +{ + this->includePoseRelativeTo = _includePoseRelativeTo; +} + +///////////////////////////////////////////////// +const std::optional &NestedInclude::PlacementFrame() const +{ + return this->placementFrame; +} + +///////////////////////////////////////////////// +void NestedInclude::SetPlacementFrame(const std::string &_placementFrame) +{ + this->placementFrame = _placementFrame; +} + +///////////////////////////////////////////////// +sdf::ElementPtr NestedInclude::IncludeElement() const +{ + return this->includeElement; +} + +///////////////////////////////////////////////// +void NestedInclude::SetIncludeElement(sdf::ElementPtr _includeElement) +{ + this->includeElement = _includeElement; +} +SDF_SUPPRESS_DEPRECATED_END diff --git a/src/Utils.cc b/src/Utils.cc index 6c9dcc3f0..3fac31810 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -218,39 +218,41 @@ sdf::Errors loadIncludedInterfaceModels(sdf::ElementPtr _sdf, includeElem = includeElem->GetNextElement("include")) { sdf::NestedInclude include; - include.uri = includeElem->Get("uri"); + include.SetUri(includeElem->Get("uri")); auto absoluteParentName = computeAbsoluteName(_sdf, allErrors); if (absoluteParentName.has_value()) { - include.absoluteParentName = *absoluteParentName; + include.SetAbsoluteParentName(*absoluteParentName); } if (includeElem->HasElement("name")) { - include.localModelName = includeElem->Get("name"); + include.SetLocalModelName(includeElem->Get("name")); } if (includeElem->HasElement("static")) { - include.isStatic = includeElem->Get("static"); + include.SetIsStatic(includeElem->Get("static")); } - include.resolvedFileName = sdf::findFile(include.uri, true, true, _config); + include.SetResolvedFileName( + sdf::findFile(include.Uri(), true, true, _config)); - include.includeElement = includeElem; + include.SetIncludeElement(includeElem); if (includeElem->HasElement("pose")) { auto poseElem = includeElem->GetElement("pose"); - include.includeRawPose = poseElem->Get(); + include.SetIncludeRawPose(poseElem->Get()); if (poseElem->HasAttribute("relative_to")) { - include.includePoseRelativeTo = - poseElem->Get("relative_to"); + include.SetIncludePoseRelativeTo( + poseElem->Get("relative_to")); } } if (includeElem->HasElement("placement_frame")) { - include.placementFrame = includeElem->Get("placement_frame"); + include.SetPlacementFrame( + includeElem->Get("placement_frame")); } // Iterate through custom model parsers in reverse per the SDFormat proposal @@ -273,7 +275,7 @@ sdf::Errors loadIncludedInterfaceModels(sdf::ElementPtr _sdf, if (model->Name() == "") { allErrors.emplace_back(sdf::ErrorCode::ATTRIBUTE_INVALID, - "Missing name of custom model with URI [" + include.uri + "]"); + "Missing name of custom model with URI [" + include.Uri() + "]"); } else { diff --git a/test/integration/interface_api.cc b/test/integration/interface_api.cc index 6d7bdde83..4345f693a 100644 --- a/test/integration/interface_api.cc +++ b/test/integration/interface_api.cc @@ -82,24 +82,24 @@ sdf::InterfaceModelPtr parseModel(toml::Value &_doc, sdf::InterfaceModelPtr customTomlParser( const sdf::NestedInclude &_include, sdf::Errors &_errors) { - toml::Document doc = toml::parseToml(_include.resolvedFileName, _errors); + toml::Document doc = toml::parseToml(_include.ResolvedFileName(), _errors); if (_errors.empty()) { const std::string modelName = - _include.localModelName.value_or(doc["name"].ParamGet()); + _include.LocalModelName().value_or(doc["name"].ParamGet()); - if (_include.isStatic.has_value()) + if (_include.IsStatic().has_value()) { // if //include/static is set, override the value in the inluded model sdf::Param param("static", "bool", "false", false); - param.Set(*_include.isStatic); + param.Set(*_include.IsStatic()); doc["static"] = {param}; } - if (_include.includeRawPose.has_value()) + if (_include.IncludeRawPose().has_value()) { // if //include/static is set, override the value in the inluded model sdf::Param poseParam("pose", "pose", "", false); - poseParam.Set(*_include.includeRawPose); + poseParam.Set(*_include.IncludeRawPose()); doc["pose"] = {poseParam}; } @@ -162,26 +162,26 @@ TEST_F(InterfaceAPI, NestedIncludeData) auto testNonce1Parser = [&](const sdf::NestedInclude &_include, sdf::Errors &_errors) { - if (!endsWith(_include.uri, ".nonce_1")) + if (!endsWith(_include.Uri(), ".nonce_1")) return nullptr; const std::string fileName = "file_wont_be_parsed.nonce_1"; - EXPECT_EQ(fileName, _include.uri); + EXPECT_EQ(fileName, _include.Uri()); EXPECT_EQ(sdf::filesystem::append(this->modelDir, fileName), - _include.resolvedFileName); - EXPECT_EQ("box", *_include.localModelName); - EXPECT_TRUE(_include.isStatic.has_value()); - EXPECT_TRUE(_include.isStatic.value()); + _include.ResolvedFileName()); + EXPECT_EQ("box", *_include.LocalModelName()); + EXPECT_TRUE(_include.IsStatic().has_value()); + EXPECT_TRUE(_include.IsStatic().value()); - EXPECT_TRUE(_include.includeRawPose.has_value()); + EXPECT_TRUE(_include.IncludeRawPose().has_value()); EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), - _include.includeRawPose.value()); + _include.IncludeRawPose().value()); - EXPECT_TRUE(_include.includePoseRelativeTo.has_value()); - EXPECT_EQ("F1", _include.includePoseRelativeTo.value()); - EXPECT_TRUE(_include.includeElement->HasElement("extra")); + EXPECT_TRUE(_include.IncludePoseRelativeTo().has_value()); + EXPECT_EQ("F1", _include.IncludePoseRelativeTo().value()); + EXPECT_TRUE(_include.IncludeElement()->HasElement("extra")); - auto extra = _include.includeElement->GetElement("extra"); + auto extra = _include.IncludeElement()->GetElement("extra"); EXPECT_TRUE(extra->HasElement("info1")); EXPECT_TRUE(extra->HasElement("info2")); EXPECT_EQ("value1", extra->Get("info1")); @@ -196,14 +196,14 @@ TEST_F(InterfaceAPI, NestedIncludeData) auto testNonce2Parser = [&](const sdf::NestedInclude &_include, sdf::Errors &_errors) { - if (!endsWith(_include.uri, ".nonce_2")) + if (!endsWith(_include.Uri(), ".nonce_2")) return nullptr; const std::string fileName = "file_wont_be_parsed.nonce_2"; - EXPECT_EQ(fileName, _include.uri); - EXPECT_EQ( - sdf::filesystem::append(modelDir, fileName), _include.resolvedFileName); - EXPECT_FALSE(_include.localModelName.has_value()); - EXPECT_FALSE(_include.isStatic); + EXPECT_EQ(fileName, _include.Uri()); + EXPECT_EQ(sdf::filesystem::append(modelDir, fileName), + _include.ResolvedFileName()); + EXPECT_FALSE(_include.LocalModelName().has_value()); + EXPECT_FALSE(_include.IsStatic()); // Add error for test expectation later on. _errors.emplace_back( @@ -588,17 +588,19 @@ TEST_F(InterfaceAPI, Reposturing) auto repostureTestParser = [&](const sdf::NestedInclude &_include, sdf::Errors &) -> sdf::InterfaceModelPtr { - bool fileHasCorrectSuffix = endsWith(_include.resolvedFileName, ".nonce_1"); - EXPECT_TRUE(fileHasCorrectSuffix) << "File: " << _include.resolvedFileName; + bool fileHasCorrectSuffix = + endsWith(_include.ResolvedFileName(), ".nonce_1"); + EXPECT_TRUE(fileHasCorrectSuffix) + << "File: " << _include.ResolvedFileName(); if (!fileHasCorrectSuffix) return nullptr; - const std::string absoluteModelName = - sdf::JoinName(_include.absoluteParentName, *_include.localModelName); + const std::string absoluteModelName = sdf::JoinName( + _include.AbsoluteParentName(), *_include.LocalModelName()); - auto model = std::make_shared(*_include.localModelName, - makeRepostureFunc(absoluteModelName), false, "base_link", - _include.includeRawPose.value_or(Pose3d {})); + auto model = std::make_shared( + *_include.LocalModelName(), makeRepostureFunc(absoluteModelName), false, + "base_link", _include.IncludeRawPose().value_or(Pose3d{})); model->AddLink({"base_link", {}}); models[absoluteModelName] = model; @@ -662,8 +664,8 @@ TEST_F(InterfaceAPI, PlacementFrame) auto repostureTestParser = [&](const sdf::NestedInclude &_include, sdf::Errors &) { - std::string modelName = - sdf::JoinName(_include.absoluteParentName, *_include.localModelName); + std::string modelName = sdf::JoinName(_include.AbsoluteParentName(), + *_include.LocalModelName()); auto repostureFunc = [modelName = modelName, &modelPosesAfterReposture]( const sdf::InterfaceModelPoseGraph &_graph) { @@ -673,9 +675,9 @@ TEST_F(InterfaceAPI, PlacementFrame) modelPosesAfterReposture[modelName] = pose; }; - auto model = std::make_shared(*_include.localModelName, - repostureFunc, false, "base_link", - _include.includeRawPose.value_or(Pose3d {})); + auto model = std::make_shared( + *_include.LocalModelName(), repostureFunc, false, "base_link", + _include.IncludeRawPose().value_or(Pose3d{})); model->AddLink({"base_link", Pose3d(0, 1, 0, 0, 0, 0)}); model->AddFrame({"frame_1", "__model__", Pose3d(0, 0, 1, 0, 0, 0)}); return model; From c4b7425a29def27f7ec0390233f00e619373ec44 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 14 Sep 2021 17:28:19 -0500 Subject: [PATCH 7/9] Add unit test for NestedInclude Signed-off-by: Addisu Z. Taddese --- src/CMakeLists.txt | 1 + src/InterfaceElements_TEST.cc | 64 +++++++++++++++++++++++++++++++++++ 2 files changed, 65 insertions(+) create mode 100644 src/InterfaceElements_TEST.cc diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 41a0227c1..efa7cffb7 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -115,6 +115,7 @@ if (BUILD_SDF_TEST) Gui_TEST.cc Heightmap_TEST.cc Imu_TEST.cc + InterfaceElements_TEST.cc Joint_TEST.cc JointAxis_TEST.cc Lidar_TEST.cc diff --git a/src/InterfaceElements_TEST.cc b/src/InterfaceElements_TEST.cc new file mode 100644 index 000000000..b2f6389a2 --- /dev/null +++ b/src/InterfaceElements_TEST.cc @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + + +#include + +#include + +#include "sdf/InterfaceElements.hh" + +TEST(InterfaceElements, Construction) +{ + sdf::NestedInclude nestedInclude; + EXPECT_EQ("", nestedInclude.Uri()); + EXPECT_EQ("", nestedInclude.ResolvedFileName()); + EXPECT_EQ("", nestedInclude.AbsoluteParentName()); + EXPECT_FALSE(nestedInclude.LocalModelName().has_value()); + EXPECT_FALSE(nestedInclude.IsStatic().has_value()); + EXPECT_FALSE(nestedInclude.IncludeRawPose().has_value()); + EXPECT_FALSE(nestedInclude.IncludePoseRelativeTo().has_value()); + EXPECT_FALSE(nestedInclude.PlacementFrame().has_value()); + EXPECT_EQ(nullptr, nestedInclude.IncludeElement()); + + nestedInclude.SetUri("file://test_path"); + EXPECT_EQ("file://test_path", nestedInclude.Uri()); + + nestedInclude.SetResolvedFileName("/path/to/file"); + EXPECT_EQ("/path/to/file", nestedInclude.ResolvedFileName()); + + nestedInclude.SetAbsoluteParentName("A::B::C"); + EXPECT_EQ("A::B::C", nestedInclude.AbsoluteParentName()); + + nestedInclude.SetLocalModelName("test_model"); + EXPECT_EQ("test_model", nestedInclude.LocalModelName()); + + nestedInclude.SetIsStatic(true); + EXPECT_EQ(true, nestedInclude.IsStatic()); + nestedInclude.SetIsStatic(false); + EXPECT_EQ(false, nestedInclude.IsStatic()); + + nestedInclude.SetIncludeRawPose({1, 2, 3, 0, 0, IGN_PI_4}); + EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, IGN_PI_4), + nestedInclude.IncludeRawPose()); + + nestedInclude.SetIncludePoseRelativeTo("test_frame"); + EXPECT_EQ("test_frame", nestedInclude.IncludePoseRelativeTo()); + + nestedInclude.SetPlacementFrame("test_placement_frame"); + EXPECT_EQ("test_placement_frame", nestedInclude.PlacementFrame()); +} From 5c6bec203f729dbbc134c267d57c8654a9d376be Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 15 Sep 2021 13:14:39 -0500 Subject: [PATCH 8/9] Fix windows error Signed-off-by: Addisu Z. Taddese --- include/sdf/Model.hh | 2 +- include/sdf/World.hh | 2 +- test/integration/includes.cc | 9 +++++++++ 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 28391a4c2..8b1f7771c 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -40,7 +40,7 @@ namespace sdf class Joint; class Link; class ParserConfig; - struct NestedInclude; + class NestedInclude; struct PoseRelativeToGraph; struct FrameAttachedToGraph; template class ScopedGraph; diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 9e620c287..b0d4d6515 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -46,7 +46,7 @@ namespace sdf class Model; class ParserConfig; class Physics; - struct NestedInclude; + class NestedInclude; struct PoseRelativeToGraph; struct FrameAttachedToGraph; template class ScopedGraph; diff --git a/test/integration/includes.cc b/test/integration/includes.cc index 679e2125b..4614280f4 100644 --- a/test/integration/includes.cc +++ b/test/integration/includes.cc @@ -652,6 +652,15 @@ TEST(IncludesTest, InvalidMergeInclude) std::stringstream buffer; sdf::testing::RedirectConsoleStream redir( sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + const std::string sdfString = R"( From de373d05daec3aff6b9ac2874c240c4ad53e7896 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 20 Sep 2021 17:05:22 -0500 Subject: [PATCH 9/9] Fix typos, use merged____model__ as the name of the proxy frame Signed-off-by: Addisu Z. Taddese --- include/sdf/InterfaceElements.hh | 2 +- src/InterfaceElements_TEST.cc | 2 +- src/parser.cc | 3 ++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/sdf/InterfaceElements.hh b/include/sdf/InterfaceElements.hh index e7a881408..a8debb7c4 100644 --- a/include/sdf/InterfaceElements.hh +++ b/include/sdf/InterfaceElements.hh @@ -48,7 +48,7 @@ class SDFORMAT_VISIBLE NestedInclude // Defaulted copy, move constructors and destructors are needed to avoid // deprecation warnings on memeber variables when simply instantiating this // class. - // TODO(anyone) Remove the constructor and destructor once the depreceted + // TODO(anyone) Remove the constructor and destructor once the deprecated // members are removed. SDF_SUPPRESS_DEPRECATED_BEGIN public: NestedInclude(const NestedInclude&) = default; diff --git a/src/InterfaceElements_TEST.cc b/src/InterfaceElements_TEST.cc index b2f6389a2..ddfd3f867 100644 --- a/src/InterfaceElements_TEST.cc +++ b/src/InterfaceElements_TEST.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/src/parser.cc b/src/parser.cc index 00cfe3b1a..16abd35de 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -246,7 +246,8 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF, } ElementPtr proxyModelFrame = _parent->AddElement("frame"); - const std::string proxyModelFrameName = model->Name() + "__model__"; + const std::string proxyModelFrameName = + "merged__" + model->Name() + "__model__"; proxyModelFrame->GetAttribute("name")->Set(proxyModelFrameName);