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/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/include/sdf/InterfaceElements.hh b/include/sdf/InterfaceElements.hh index 922c77164..a8debb7c4 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 deprecated + // 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/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/sdf/1.9/model.sdf b/sdf/1.9/model.sdf index 65100326e..d16865e8e 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/CMakeLists.txt b/src/CMakeLists.txt index 503fd7599..efa7cffb7 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 @@ -114,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/Error.cc b/src/Error.cc index 2a3c3ca88..713b7f8b8 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/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/InterfaceElements_TEST.cc b/src/InterfaceElements_TEST.cc new file mode 100644 index 000000000..ddfd3f867 --- /dev/null +++ b/src/InterfaceElements_TEST.cc @@ -0,0 +1,64 @@ +/* + * 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. + * 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()); +} 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/src/parser.cc b/src/parser.cc index dac37a6f2..16abd35de 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 @@ -131,6 +168,191 @@ 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] _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. +/// \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) +{ + 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->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()); + + 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 = + "merged__" + model->Name() + "__model__"; + + 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; + + 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; + } + + 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(); + + // 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); + } + }; + + 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 ((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); + } + + 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() == "gripper") || (elem->GetName() == "plugin") || + (elem->GetName().find(':') != std::string::npos)) + { + elem->SetParent(_parent); + _parent->InsertElement(elem); + } + } +} + ////////////////////////////////////////////////// bool init(SDFPtr _sdf) { @@ -1210,6 +1432,42 @@ static bool resolveFileNameFromUri(tinyxml2::XMLElement *_includeXml, return true; } +////////////////////////////////////////////////// +// 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) @@ -1291,6 +1549,8 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, { if (std::string("include") == elemXml->Value()) { + validateIncludeElement(elemXml, _sdf, _config, _source, _errors); + tinyxml2::XMLElement *uriElement = elemXml->FirstChildElement("uri"); const std::string includeXmlPath = _sdf->XmlPath() + "/include[" + @@ -1528,7 +1788,6 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, } auto includeSDFFirstElem = includeSDF->Root()->GetFirstElement(); - includeSDFFirstElem->SetParent(_sdf); auto includeDesc = _sdf->GetElementDescription("include"); if (includeDesc) { @@ -1538,8 +1797,11 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, copyChildren(includeInfo, elemXml, false); includeSDFFirstElem->SetIncludeElement(includeInfo); } - _sdf->InsertElement(includeSDFFirstElem); + bool toMerge = elemXml->BoolAttribute("merge", false); + SourceLocation sourceLoc{includeXmlPath, _source, + elemXml->GetLineNum()}; + insertIncludedElement(includeSDF, sourceLoc, toMerge, _sdf, _errors); continue; } } diff --git a/test/integration/includes.cc b/test/integration/includes.cc index 81f16da98..4614280f4 100644 --- a/test/integration/includes.cc +++ b/test/integration/includes.cc @@ -15,14 +15,19 @@ * */ +#include + +#include #include #include -#include #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" @@ -33,6 +38,7 @@ #include "sdf/Visual.hh" #include "sdf/World.hh" #include "test_config.h" +#include "test_utils.hh" ///////////////////////////////////////////////// std::string findFileCb(const std::string &_input) @@ -388,3 +394,289 @@ TEST(IncludesTest, IncludeUrdf) EXPECT_EQ(1u, model->JointCount()); } +////////////////////////////////////////////////// +TEST(IncludesTest, MergeInclude) +{ + using ignition::math::Pose3d; + using ignition::math::Vector3d; + sdf::ParserConfig config; + config.SetFindCallback(findFileCb); + + sdf::Root root; + sdf::Errors errors = root.Load( + sdf::testing::TestFile("integration", "merge_include_model.sdf"), config); + EXPECT_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; + }; + + // 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); + 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); + 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 + // 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); + 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; + // 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")); + + // 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(4, *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(4, *errors[0].LineNumber()); + } + + // merge-include cannot be used directly under //world + { + 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 does not support parent element of type world", + errors[0].Message()); + 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); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + + 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) << buffer.str(); + } +} 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; diff --git a/test/integration/merge_include_model.sdf b/test/integration/merge_include_model.sdf new file mode 100644 index 000000000..06ab5584b --- /dev/null +++ b/test/integration/merge_include_model.sdf @@ -0,0 +1,20 @@ + + + + + + merge_robot + 100 0 0 0.7853981633974483 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.config b/test/integration/model/merge_robot/model.config new file mode 100644 index 000000000..d49445a0e --- /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..4215df4a7 --- /dev/null +++ b/test/integration/model/merge_robot/model.sdf @@ -0,0 +1,310 @@ + + + + 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 1 0 0 45 0 + + + 0 0 0 0 0 0 + + + + + + val + val2 + + + true + false + + + 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 + + + + diff --git a/test/sdf/model_invalid_frame_only.sdf b/test/sdf/model_invalid_frame_only.sdf new file mode 100644 index 000000000..9d4109326 --- /dev/null +++ b/test/sdf/model_invalid_frame_only.sdf @@ -0,0 +1,7 @@ + + + + + + +