diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index bb747d7be..dbece5917 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -529,6 +529,24 @@ namespace sdf private: const std::vector, sdf::InterfaceModelConstPtr>> &MergedInterfaceModels() const; + /// \brief Get whether the model was merge-included and needs to be + /// processed to carry out the merge. + /// \return True if the model was merge-included. + private: bool IsMerged() const; + + /// \brief Prepare the model to be merged into the parent model or world. + /// As part of the perparation, this will create the proxy frame that would + /// be need to be added to the parent object. + /// \param[out] _errors A list of errors encountered during the operation. + /// \param[in] _parentOfProxyFrame Name of parent of the proxy frame that + /// will be created. This can only be "__model__" or "world". + /// \return The proxy frame for the merged model that will need to be added + /// to the parent object. + /// \note This is a destructive call. After this call, the model will be in + /// an invalid state unless it is merged into the parent object. + private: sdf::Frame PrepareForMerge(sdf::Errors &_errors, + const std::string &_parentOfProxyFrame); + /// \brief Allow Root::Load, World::SetPoseRelativeToGraph, or /// World::SetFrameAttachedToGraph to call SetPoseRelativeToGraph and /// SetFrameAttachedToGraph diff --git a/src/Model.cc b/src/Model.cc index 4f4f7b0f3..838f05c94 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -27,6 +27,7 @@ #include "sdf/InterfaceModel.hh" #include "sdf/InterfaceModelPoseGraph.hh" #include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" #include "sdf/Link.hh" #include "sdf/Model.hh" #include "sdf/ParserConfig.hh" @@ -126,6 +127,10 @@ class sdf::Model::Implementation /// ToElement() function to output only the plugin specified in the /// tag when the ToElementUseIncludeTag policy is true.. public: std::vector includePlugins; + + /// \brief Whether the model was merge-included and needs to be processed to + /// carry out the merge. + public: bool isMerged{false}; }; ///////////////////////////////////////////////// @@ -183,6 +188,11 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) } } + // Note: this attribute is not defined in the spec. It is used internally for + // implementing merge-includes when custom parsers are present. + this->dataPtr->isMerged = + _sdf->Get("__merge__", this->dataPtr->isMerged).first; + this->dataPtr->placementFrameName = _sdf->Get("placement_frame", this->dataPtr->placementFrameName).first; @@ -212,29 +222,31 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) _config.WarningsPolicy(), err, errors); } } - - // Set of implicit and explicit frame names in this model for tracking - // name collisions - std::unordered_set frameNames; - - // Load nested models. - Errors nestedModelLoadErrors = loadUniqueRepeated(_sdf, "model", - this->dataPtr->models, _config); - errors.insert(errors.end(), - nestedModelLoadErrors.begin(), - nestedModelLoadErrors.end()); - - // Nested models are loaded first, and loadUniqueRepeated ensures there are no - // duplicate names, so these names can be added to frameNames without - // checking uniqueness. - for (const auto &model : this->dataPtr->models) + std::unordered_set nestedModelNames; + std::unordered_set linkNames; + std::unordered_set jointNames; + std::unordered_set explicitFrameNames; + auto recordUniqueName = [&errors](std::unordered_set& nameList, + const std::string &_elementName, + const std::string &_name) { - frameNames.insert(model.Name()); - } + if (nameList.count(_name) > 0) + { + errors.emplace_back(ErrorCode::DUPLICATE_NAME, + _elementName + " with name[" + _name + "] already exists."); + return false; + } + nameList.insert(_name); + return true; + }; + // Set of implicit and explicit frame names in this model for tracking + // name collisions. This is used to handle name clashes in old versions of + // SDFormat where sibling elements were allowed to have duplicate names. + std::unordered_set implicitFrameNames; // Load InterfaceModels into a temporary container so we can have special // handling for merged InterfaceModels. - std::vector> + std::vector> tmpInterfaceModels; // Load included models via the interface API Errors interfaceModelLoadErrors = loadIncludedInterfaceModels( @@ -246,7 +258,7 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) { if (!ifaceInclude.IsMerge().value_or(false)) { - frameNames.insert(ifaceModel->Name()); + implicitFrameNames.insert(ifaceModel->Name()); this->dataPtr->interfaceModels.emplace_back(ifaceInclude, ifaceModel); } else @@ -275,152 +287,198 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) nestedInclude.SetIncludePoseRelativeTo(proxyModelFrameName); nestedInclude.SetIncludeRawPose( ifaceNestedModel->ModelFramePoseInParentFrame()); + implicitFrameNames.insert(ifaceNestedModel->Name()); this->dataPtr->interfaceModels.emplace_back(nestedInclude, ifaceNestedModel); } } } - // Load all the links. - Errors linkLoadErrors = loadUniqueRepeated(_sdf, "link", - this->dataPtr->links, _config); - errors.insert(errors.end(), linkLoadErrors.begin(), linkLoadErrors.end()); - - // Check links for name collisions and modify and warn if so. - for (auto &link : this->dataPtr->links) + for (auto elem = _sdf->GetFirstElement(); elem; elem = elem->GetNextElement()) { - std::string linkName = link.Name(); - if (frameNames.count(linkName) > 0) + const std::string &elementName = elem->GetName(); + if (elementName == "model") { - // This link has a name collision - if (sdfVersion < gz::math::SemanticVersion(1, 7)) + auto model = loadSingle(errors, elem, _config); + if (!recordUniqueName(nestedModelNames, elementName, model.Name())) + { + continue; + } + if (model.IsMerged()) { - // This came from an old file, so try to workaround by renaming link - linkName += "_link"; - int i = 0; - while (frameNames.count(linkName) > 0) + sdf::Frame proxyFrame = model.PrepareForMerge(errors, "__model__"); + this->AddFrame(proxyFrame); + + auto moveElements = [](const auto &_src, auto &_dest) + { + std::move(_src.begin(), _src.end(), std::back_inserter(_dest)); + }; + moveElements(model.dataPtr->links, this->dataPtr->links); + moveElements(model.dataPtr->frames, this->dataPtr->frames); + moveElements(model.dataPtr->joints, this->dataPtr->joints); + for (const auto &nestedModel : model.dataPtr->models) + { + implicitFrameNames.insert(nestedModel.Name()); + } + for (const auto &nestedInterfaceModel : model.dataPtr->interfaceModels) + { + implicitFrameNames.insert(nestedInterfaceModel.second->Name()); + } + for (const auto &nestedMergedInterfaceModel : + model.dataPtr->mergedInterfaceModels) { - linkName = link.Name() + "_link" + std::to_string(i++); + for (const auto &nestedModel : + nestedMergedInterfaceModel.second->NestedModels()) + { + implicitFrameNames.insert(nestedModel->Name()); + } } - std::stringstream ss; - ss << "Link with name [" << link.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision, changing link name to [" - << linkName << "]."; - Error err(ErrorCode::WARNING, ss.str()); - enforceConfigurablePolicyCondition( - _config.WarningsPolicy(), err, errors); - link.SetName(linkName); + + moveElements(model.dataPtr->models, this->dataPtr->models); + moveElements(model.dataPtr->interfaceModels, + this->dataPtr->interfaceModels); + moveElements(model.dataPtr->mergedInterfaceModels, + this->dataPtr->mergedInterfaceModels); } else { - std::stringstream ss; - ss << "Link with name [" << link.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision. Please rename this link."; - errors.push_back({ErrorCode::DUPLICATE_NAME, ss.str()}); + implicitFrameNames.insert(model.Name()); + this->dataPtr->models.push_back(std::move(model)); } } - frameNames.insert(linkName); - } - - // If the model is not static and has no nested models: - // Require at least one (interface) link so the implicit model frame can be - // attached to something. - if (!this->Static() && this->dataPtr->links.empty() && - this->dataPtr->interfaceLinks.empty() && this->dataPtr->models.empty() && - this->dataPtr->interfaceModels.empty()) - { - errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, - "A model must have at least one link."}); - } - - // Load all the joints. - Errors jointLoadErrors = loadUniqueRepeated(_sdf, "joint", - this->dataPtr->joints); - errors.insert(errors.end(), jointLoadErrors.begin(), jointLoadErrors.end()); - - // Check joints for name collisions and modify and warn if so. - for (auto &joint : this->dataPtr->joints) - { - std::string jointName = joint.Name(); - if (frameNames.count(jointName) > 0) + else if (elementName == "link") { - // This joint has a name collision - if (sdfVersion < gz::math::SemanticVersion(1, 7)) + auto link = loadSingle(errors, elem, _config); + std::string linkName = link.Name(); + if (!recordUniqueName(linkNames, elementName, linkName)) { - // This came from an old file, so try to workaround by renaming joint - jointName += "_joint"; - int i = 0; - while (frameNames.count(jointName) > 0) - { - jointName = joint.Name() + "_joint" + std::to_string(i++); - } - std::stringstream ss; - ss << "Joint with name [" << joint.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision, changing joint name to [" - << jointName << "]."; - Error err(ErrorCode::WARNING, ss.str()); - enforceConfigurablePolicyCondition( - _config.WarningsPolicy(), err, errors); - - joint.SetName(jointName); + continue; } - else + + // Check links for name collisions and modify and warn if so. + if (implicitFrameNames.count(linkName) > 0) { - std::stringstream ss; - ss << "Joint with name [" << joint.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision. Please rename this joint."; - errors.push_back({ErrorCode::DUPLICATE_NAME, ss.str()}); + // This link has a name collision + if (sdfVersion < gz::math::SemanticVersion(1, 7)) + { + // This came from an old file, so try to workaround by renaming link + linkName += "_link"; + int i = 0; + while (implicitFrameNames.count(linkName) > 0) + { + linkName = link.Name() + "_link" + std::to_string(i++); + } + std::stringstream ss; + ss << "Link with name [" << link.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision, changing link name to [" << linkName + << "]."; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition(_config.WarningsPolicy(), err, + errors); + link.SetName(linkName); + } + else + { + std::stringstream ss; + ss << "Link with name [" << link.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision. Please rename this link."; + errors.push_back({ErrorCode::DUPLICATE_NAME, ss.str()}); + } } + implicitFrameNames.insert(linkName); + this->dataPtr->links.push_back(std::move(link)); } - frameNames.insert(jointName); - } - - // Load all the frames. - Errors frameLoadErrors = loadUniqueRepeated(_sdf, "frame", - this->dataPtr->frames); - errors.insert(errors.end(), frameLoadErrors.begin(), frameLoadErrors.end()); - - // Check frames for name collisions and modify and warn if so. - for (auto &frame : this->dataPtr->frames) - { - std::string frameName = frame.Name(); - if (frameNames.count(frameName) > 0) + else if (elementName == "joint") { - // This frame has a name collision - if (sdfVersion < gz::math::SemanticVersion(1, 7)) + auto joint = loadSingle(errors, elem); + std::string jointName = joint.Name(); + if (!recordUniqueName(jointNames, elementName, jointName)) + { + continue; + } + // Check joints for name collisions and modify and warn if so. + if (implicitFrameNames.count(jointName) > 0) { - // This came from an old file, so try to workaround by renaming frame - frameName += "_frame"; - int i = 0; - while (frameNames.count(frameName) > 0) + // This joint has a name collision + if (sdfVersion < gz::math::SemanticVersion(1, 7)) + { + // This came from an old file, so try to workaround by renaming joint + jointName += "_joint"; + int i = 0; + while (implicitFrameNames.count(jointName) > 0) + { + jointName = joint.Name() + "_joint" + std::to_string(i++); + } + std::stringstream ss; + ss << "Joint with name [" << joint.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision, changing joint name to [" << jointName + << "]."; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition(_config.WarningsPolicy(), err, + errors); + + joint.SetName(jointName); + } + else { - frameName = frame.Name() + "_frame" + std::to_string(i++); + std::stringstream ss; + ss << "Joint with name [" << joint.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision. Please rename this joint."; + errors.push_back({ErrorCode::DUPLICATE_NAME, ss.str()}); } - std::stringstream ss; - ss << "Frame with name [" << frame.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision, changing frame name to [" - << frameName << "]."; - Error err(ErrorCode::WARNING, ss.str()); - enforceConfigurablePolicyCondition( - _config.WarningsPolicy(), err, errors); - - frame.SetName(frameName); } - else + + implicitFrameNames.insert(jointName); + this->dataPtr->joints.push_back(std::move(joint)); + } + else if (elementName == "frame") + { + auto frame = loadSingle(errors, elem); + std::string frameName = frame.Name(); + if (!recordUniqueName(explicitFrameNames, elementName, frameName)) { - std::stringstream ss; - ss << "Frame with name [" << frame.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision. Please rename this frame."; - errors.push_back({ErrorCode::DUPLICATE_NAME, ss.str()}); + continue; } + // Check frames for name collisions and modify and warn if so. + if (implicitFrameNames.count(frameName) > 0) + { + // This frame has a name collision + if (sdfVersion < gz::math::SemanticVersion(1, 7)) + { + // This came from an old file, so try to workaround by renaming frame + frameName += "_frame"; + int i = 0; + while (implicitFrameNames.count(frameName) > 0) + { + frameName = frame.Name() + "_frame" + std::to_string(i++); + } + std::stringstream ss; + ss << "Frame with name [" << frame.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision, changing frame name to [" << frameName + << "]."; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition(_config.WarningsPolicy(), err, + errors); + + frame.SetName(frameName); + } + else + { + std::stringstream ss; + ss << "Frame with name [" << frame.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision. Please rename this frame."; + errors.push_back({ErrorCode::DUPLICATE_NAME, ss.str()}); + } + } + implicitFrameNames.insert(frameName); + this->dataPtr->frames.push_back(std::move(frame)); } - frameNames.insert(frameName); } // Load the model plugins @@ -428,6 +486,17 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) this->dataPtr->plugins); errors.insert(errors.end(), pluginErrors.begin(), pluginErrors.end()); + // If the model is not static and has no nested models: + // Require at least one (interface) link so the implicit model frame can be + // attached to something. + if (!this->Static() && this->dataPtr->links.empty() && + this->dataPtr->interfaceLinks.empty() && this->dataPtr->models.empty() && + this->dataPtr->interfaceModels.empty()) + { + errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, + "A model must have at least one link."}); + } + // Check whether the model was loaded from an tag. If so, set // the URI and capture the plugins. if (_sdf->GetIncludeElement() && _sdf->GetIncludeElement()->HasElement("uri")) @@ -755,7 +824,7 @@ std::pair Model::CanonicalLinkAndRelativeName() const auto firstModel = this->ModelByIndex(0); auto canonicalLinkAndName = firstModel->CanonicalLinkAndRelativeName(); // Prepend firstModelName if a valid link is found. - if (nullptr != canonicalLinkAndName.first) + if (canonicalLinkAndName.second != "") { canonicalLinkAndName.second = firstModel->Name() + "::" + canonicalLinkAndName.second; @@ -1173,3 +1242,158 @@ void Model::AddPlugin(const Plugin &_plugin) { this->dataPtr->plugins.push_back(_plugin); } + +///////////////////////////////////////////////// +bool Model::IsMerged() const +{ + return this->dataPtr->isMerged; +} + +///////////////////////////////////////////////// +sdf::Frame Model::PrepareForMerge(sdf::Errors &_errors, + const std::string &_parentOfProxyFrame) +{ + // Build the pose graph of the model so we can adjust the pose of the proxy + // frame to take the placement frame into account. + auto poseGraph = std::make_shared(); + sdf::ScopedGraph scopedPoseGraph(poseGraph); + sdf::Errors poseGraphErrors = + sdf::buildPoseRelativeToGraph(scopedPoseGraph, this); + _errors.insert(_errors.end(), poseGraphErrors.begin(), poseGraphErrors.end()); + sdf::Errors poseValidationErrors = + validatePoseRelativeToGraph(scopedPoseGraph); + _errors.insert(_errors.end(), poseValidationErrors.begin(), + poseValidationErrors.end()); + + auto frameGraph = std::make_shared(); + sdf::ScopedGraph scopedFrameGraph(frameGraph); + sdf::Errors frameGraphErrors = + sdf::buildFrameAttachedToGraph(scopedFrameGraph, this); + _errors.insert(_errors.end(), frameGraphErrors.begin(), + frameGraphErrors.end()); + sdf::Errors frameValidationErrors = + validateFrameAttachedToGraph(scopedFrameGraph); + _errors.insert(_errors.end(), frameValidationErrors.begin(), + frameValidationErrors.end()); + + this->SetPoseRelativeToGraph(scopedPoseGraph); + this->SetFrameAttachedToGraph(scopedFrameGraph); + + const std::string proxyModelFrameName = + computeMergedModelProxyFrameName(this->Name()); + + sdf::Frame proxyFrame; + proxyFrame.SetName(proxyModelFrameName); + proxyFrame.SetAttachedTo(this->CanonicalLinkAndRelativeName().second); + gz::math::Pose3d modelPose = this->RawPose(); + if (!this->PlacementFrameName().empty()) + { + // Build the pose graph of the model so we can adjust the pose of the proxy + // frame to take the placement frame into account. + if (poseGraphErrors.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 + // Note, when the frame graph is built for a model with placement frame, + // it is built with an pose offset to take into account the placement + // frame. Therefore, we only need to resolve the pose of the model + // relative to `R` here. We could manually compute it as + // X_RM = X_RL * X_LM; where `L` is the link specified in the placement + // frame attribute. + gz::math::Pose3d X_RM = this->RawPose(); + sdf::Errors resolveErrors = this->SemanticPose().Resolve(X_RM); + _errors.insert(_errors.end(), resolveErrors.begin(), resolveErrors.end()); + modelPose = X_RM; + } + } + + proxyFrame.SetRawPose(modelPose); + proxyFrame.SetPoseRelativeTo(this->PoseRelativeTo().empty() + ? _parentOfProxyFrame + : this->PoseRelativeTo()); + + auto isEmptyOrModelFrame = [](const std::string &_attr) + { + return _attr.empty() || _attr == "__model__"; + }; + + // Merge links, frames, joints, and nested models. + for (auto &link : this->dataPtr->links) + { + if (isEmptyOrModelFrame(link.PoseRelativeTo())) + { + link.SetPoseRelativeTo(proxyModelFrameName); + } + } + + for (auto &frame : this->dataPtr->frames) + { + if (isEmptyOrModelFrame(frame.AttachedTo())) + { + frame.SetAttachedTo(proxyModelFrameName); + } + if (frame.PoseRelativeTo() == "__model__") + { + frame.SetPoseRelativeTo(proxyModelFrameName); + } + } + + for (auto &joint : this->dataPtr->joints) + { + if (joint.PoseRelativeTo() == "__model__") + { + joint.SetPoseRelativeTo(proxyModelFrameName); + } + if (joint.ParentName() == "__model__") + { + joint.SetParentName(proxyModelFrameName); + } + if (joint.ChildName() == "__model__") + { + joint.SetChildName(proxyModelFrameName); + } + for (unsigned int ai : {0, 1}) + { + const sdf::JointAxis *axis = joint.Axis(ai); + if (axis && axis->XyzExpressedIn() == "__model__") + { + sdf::JointAxis axisCopy = *axis; + axisCopy.SetXyzExpressedIn(proxyModelFrameName); + joint.SetAxis(ai, axisCopy); + } + } + } + + for (auto &nestedModel : this->dataPtr->models) + { + if (isEmptyOrModelFrame(nestedModel.PoseRelativeTo())) + { + nestedModel.SetPoseRelativeTo(proxyModelFrameName); + } + } + // Note: Since Model::Load is called recursively, all merge-include nested + // models would already have been merged by this point, so there is no need + // to call PrepareForMerge recursively here. + + for (auto &ifaceModel : this->dataPtr->interfaceModels) + { + if (isEmptyOrModelFrame( + ifaceModel.first->IncludePoseRelativeTo().value_or(""))) + { + ifaceModel.first->SetIncludePoseRelativeTo(proxyModelFrameName); + } + } + + for (auto &ifaceModel : this->dataPtr->mergedInterfaceModels) + { + if (isEmptyOrModelFrame( + ifaceModel.first->IncludePoseRelativeTo().value_or(""))) + { + ifaceModel.first->SetIncludePoseRelativeTo(proxyModelFrameName); + } + } + + return proxyFrame; +} diff --git a/src/Utils.cc b/src/Utils.cc index 6adbdc669..77ea4d35d 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -191,6 +191,13 @@ static std::optional computeAbsoluteName( { if (parent->HasAttribute("name")) { + // Ignore this parent model if it's a merged model, since the child will + // be merged into the grand parent model/world. + if (parent->GetName() == "model" && + parent->Get("__merge__", false).first) + { + continue; + } names.push_back(parent->GetAttribute("name")->GetAsString()); } else @@ -221,7 +228,7 @@ static std::optional computeAbsoluteName( // cppcheck-suppress unusedFunction sdf::Errors loadIncludedInterfaceModels(sdf::ElementPtr _sdf, const sdf::ParserConfig &_config, - std::vector> &_models) + std::vector> &_models) { sdf::Errors allErrors; for (auto includeElem = _sdf->GetElementImpl("include"); includeElem; diff --git a/src/Utils.hh b/src/Utils.hh index e1c8e46a4..18898eadd 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -200,6 +200,23 @@ namespace sdf return errors; } + /// \brief Load a single object of a specific sdf element type. Unlike + /// \ref loadRepeated, this will load the input element instead of one of its + /// children. + /// \param[out] _errors The vector of errors. An empty vector indicates no + /// errors were experienced. + /// \param[in] _elem The SDFormat element to be loaded. + /// \param[in] _args Extra arguments forwarded to the Load memeber function. + /// \return An instance of Class loaded from the input SDFormat element. + template + Class loadSingle(sdf::Errors &_errors, sdf::ElementPtr _elem, Args &&..._args) + { + Class obj; + Errors loadErrors = obj.Load(_elem, std::forward(_args)...); + _errors.insert(_errors.end(), loadErrors.begin(), loadErrors.end()); + return obj; + } + /// \brief Load interface models from //include tags. /// \param[in] _sdf sdf::ElementPtr that contains the //include tags. /// \param[in] _config Parser configuration options. @@ -207,7 +224,7 @@ namespace sdf /// \return Errors encountered. sdf::Errors loadIncludedInterfaceModels(sdf::ElementPtr _sdf, const sdf::ParserConfig &_config, - std::vector> &_models); + std::vector> &_models); /// \brief Convenience function that returns a pointer to the value contained /// in a std::optional. diff --git a/src/World.cc b/src/World.cc index 461b3596a..09bb82a76 100644 --- a/src/World.cc +++ b/src/World.cc @@ -26,6 +26,7 @@ #include "sdf/InterfaceModel.hh" #include "sdf/InterfaceModelPoseGraph.hh" #include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" #include "sdf/Light.hh" #include "sdf/Model.hh" #include "sdf/ParserConfig.hh" @@ -87,8 +88,8 @@ class sdf::World::Implementation public: std::vector models; /// \brief The interface models specified in this world. - public: std::vector> - interfaceModels; + public: std::vector> interfaceModels; /// \brief Name of the world. public: std::string name = ""; @@ -219,22 +220,27 @@ Errors World::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) } } - // Set of implicit and explicit frame names in this model for tracking - // name collisions - std::unordered_set frameNames; - - // Load all the models. - Errors modelLoadErrors = - loadUniqueRepeated(_sdf, "model", this->dataPtr->models, _config); - errors.insert(errors.end(), modelLoadErrors.begin(), modelLoadErrors.end()); - - // Models are loaded first, and loadUniqueRepeated ensures there are no - // duplicate names, so these names can be added to frameNames without - // checking uniqueness. - for (const auto &model : this->dataPtr->models) + std::unordered_set modelNames; + std::unordered_set jointNames; + std::unordered_set explicitFrameNames; + auto recordUniqueName = [&errors](std::unordered_set& nameList, + const std::string &_elementName, + const std::string &_name) { - frameNames.insert(model.Name()); - } + if (nameList.count(_name) > 0) + { + errors.emplace_back(ErrorCode::DUPLICATE_NAME, + _elementName + " with name[" + _name + "] already exists."); + return false; + } + nameList.insert(_name); + return true; + }; + + // Set of implicit and explicit frame names in this world for tracking + // name collisions. This is used to handle name clashes in old versions of + // SDFormat where sibling elements were allowed to have duplicate names. + std::unordered_set implicitFrameNames; // Load included models via the interface API Errors interfaceModelLoadErrors = loadIncludedInterfaceModels( @@ -244,48 +250,86 @@ Errors World::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) for (const auto &ifaceModelPair : this->dataPtr->interfaceModels) { - frameNames.insert(ifaceModelPair.second->Name()); + implicitFrameNames.insert(ifaceModelPair.second->Name()); } - // Load all the physics. - if (_sdf->HasElement("physics")) + for (auto elem = _sdf->GetFirstElement(); elem; elem = elem->GetNextElement()) { - this->dataPtr->physics.clear(); - Errors physicsLoadErrors = loadUniqueRepeated(_sdf, "physics", - this->dataPtr->physics); - errors.insert(errors.end(), physicsLoadErrors.begin(), - physicsLoadErrors.end()); + const std::string elementName = elem->GetName(); + if (elementName == "model") + { + auto model = loadSingle(errors, elem, _config); + if (!recordUniqueName(modelNames, elementName, model.Name())) + { + continue; + } + implicitFrameNames.insert(model.Name()); + if (model.IsMerged()) + { + sdf::Frame proxyFrame = model.PrepareForMerge(errors, "world"); + this->AddFrame(proxyFrame); + + for (uint64_t fi = 0; fi < model.FrameCount(); ++fi) + { + this->dataPtr->frames.push_back(std::move(*model.FrameByIndex(fi))); + } + + for (uint64_t ji = 0; ji < model.JointCount(); ++ji) + { + this->dataPtr->joints.push_back(std::move(*model.JointByIndex(ji))); + } + + for (uint64_t mi = 0; mi < model.ModelCount(); ++mi) + { + this->dataPtr->models.push_back(std::move(*model.ModelByIndex(mi))); + } + + for (uint64_t imi = 0; imi < model.InterfaceModelCount(); ++imi) + { + InterfaceModelConstPtr ifaceModel = model.InterfaceModelByIndex(imi); + NestedInclude nestedInclude = + *model.InterfaceModelNestedIncludeByIndex(imi); + this->dataPtr->interfaceModels.emplace_back(std::move(nestedInclude), + ifaceModel); + } + + // TODO(azeey) Support Merge-included interface models when `World` + // supports them. + } + else + { + this->dataPtr->models.push_back(std::move(model)); + } + } + else if (elementName == "joint") + { + auto joint = loadSingle(errors, elem); + if (!recordUniqueName(jointNames, elementName, joint.Name())) + { + continue; + } + this->dataPtr->joints.push_back(std::move(joint)); + } + else if (elementName == "frame") + { + auto frame = loadSingle(errors, elem); + if (!recordUniqueName(explicitFrameNames, elementName, frame.Name())) + { + continue; + } + this->dataPtr->frames.push_back(std::move(frame)); + } } - // Load all the actors. - Errors actorLoadErrors = loadUniqueRepeated(_sdf, "actor", - this->dataPtr->actors); - errors.insert(errors.end(), actorLoadErrors.begin(), actorLoadErrors.end()); - - // Load all the joints. - Errors jointLoadErrors = loadUniqueRepeated(_sdf, "joint", - this->dataPtr->joints); - errors.insert(errors.end(), jointLoadErrors.begin(), jointLoadErrors.end()); - - // Load all the lights. - Errors lightLoadErrors = loadUniqueRepeated(_sdf, "light", - this->dataPtr->lights); - errors.insert(errors.end(), lightLoadErrors.begin(), lightLoadErrors.end()); - - // Load all the frames. - Errors frameLoadErrors = loadUniqueRepeated(_sdf, "frame", - this->dataPtr->frames); - errors.insert(errors.end(), frameLoadErrors.begin(), frameLoadErrors.end()); - // Check frames for name collisions and modify and warn if so. for (auto &frame : this->dataPtr->frames) { std::string frameName = frame.Name(); - if (frameNames.count(frameName) > 0) + if (implicitFrameNames.count(frameName) > 0) { frameName += "_frame"; int i = 0; - while (frameNames.count(frameName) > 0) + while (implicitFrameNames.count(frameName) > 0) { frameName = frame.Name() + "_frame" + std::to_string(i++); } @@ -299,9 +343,30 @@ Errors World::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) _config.WarningsPolicy(), err, errors); frame.SetName(frameName); } - frameNames.insert(frameName); + implicitFrameNames.insert(frameName); } + // Load all the physics. + if (_sdf->HasElement("physics")) + { + this->dataPtr->physics.clear(); + Errors physicsLoadErrors = loadUniqueRepeated(_sdf, "physics", + this->dataPtr->physics); + errors.insert(errors.end(), physicsLoadErrors.begin(), + physicsLoadErrors.end()); + } + + // Load all the actors. + Errors actorLoadErrors = loadUniqueRepeated(_sdf, "actor", + this->dataPtr->actors); + errors.insert(errors.end(), actorLoadErrors.begin(), actorLoadErrors.end()); + + // Load all the lights. + Errors lightLoadErrors = loadUniqueRepeated(_sdf, "light", + this->dataPtr->lights); + errors.insert(errors.end(), lightLoadErrors.begin(), lightLoadErrors.end()); + + // Load the Gui if (_sdf->HasElement("gui")) { diff --git a/src/parser.cc b/src/parser.cc index 310313ead..1dfdb532d 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -232,6 +232,26 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF, } // Validate included model's frame semantics + if (!_config.CustomModelParsers().empty()) + { + // Since we have custom parsers, we can't create a throwaway sdf::Root + // object to validate the merge-included model. This is because calling + // `sdf::Root::Load` here would call the custom parsers if this model + // contains a nested model that is custom parsed. But the custom parsers + // will be called again later when we construct the final `sdf::Root` + // object. We also can't do the merge here since we'd be doing so without + // validating the model. + // We could forego validating the model and just merge all its children to + // the parent element, but we wouldn't be able to handle placement frames + // since that requires building a frame graph for the model. + // So instead we add a hidden flag here to tell `sdf::Model` or `sdf::World` + // that this model is meant to be merged. + firstElem->AddAttribute("__merge__", "bool", "false", false, + "Indicates whether this is a merge included model"); + firstElem->GetAttribute("__merge__")->Set(true); + _parent->InsertElement(firstElem, true); + return; + } // We create a throwaway sdf::Root object in order to validate the // included entity. sdf::Root includedRoot; diff --git a/test/integration/error_output.cc b/test/integration/error_output.cc index 6c7378db5..d9529eb5c 100644 --- a/test/integration/error_output.cc +++ b/test/integration/error_output.cc @@ -452,25 +452,25 @@ TEST(ErrorOutput, ModelErrorOutput) " with name[test_model].")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, errors[1].Message().find( - "link with name[common_name] already exists.")); - EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::DUPLICATE_NAME); - EXPECT_NE(std::string::npos, errors[2].Message().find( "Link with name [common_name] in model with name [test_model] has a " "name collision. Please rename this link.")); + EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[2].Message().find( + "link with name[common_name] already exists.")); EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, errors[3].Message().find( - "joint with name[common_name] already exists.")); - EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::DUPLICATE_NAME); - EXPECT_NE(std::string::npos, errors[4].Message().find( "Joint with name [common_name] in model with name [test_model] has a " "name collision. Please rename this joint.")); + EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[4].Message().find( + "joint with name[common_name] already exists.")); EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, errors[5].Message().find( - "frame with name[common_name] already exists.")); - EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::DUPLICATE_NAME); - EXPECT_NE(std::string::npos, errors[6].Message().find( "Frame with name [common_name] in model with name [test_model] has a " "name collision. Please rename this frame.")); + EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[6].Message().find( + "frame with name[common_name] already exists.")); errors.clear(); } @@ -482,25 +482,25 @@ TEST(ErrorOutput, ModelErrorOutput) ASSERT_EQ(errors.size(), 6u); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, errors[0].Message().find( - "link with name[common_name] already exists.")); - EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); - EXPECT_NE(std::string::npos, errors[1].Message().find( "Link with name [common_name] in model with name [test_model] has a " "name collision. Please rename this link.")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "link with name[common_name] already exists.")); EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, errors[2].Message().find( - "joint with name[common_name] already exists.")); - EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); - EXPECT_NE(std::string::npos, errors[3].Message().find( "Joint with name [common_name] in model with name [test_model] has a " "name collision. Please rename this joint.")); + EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[3].Message().find( + "joint with name[common_name] already exists.")); EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, errors[4].Message().find( - "frame with name[common_name] already exists.")); - EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::DUPLICATE_NAME); - EXPECT_NE(std::string::npos, errors[5].Message().find( "Frame with name [common_name] in model with name [test_model] has a " "name collision. Please rename this frame.")); + EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[5].Message().find( + "frame with name[common_name] already exists.")); // Check printed warnings EXPECT_NE(std::string::npos, buffer.str().find( "Non-unique name[common_name] detected 7 times in XML children of model" @@ -523,27 +523,27 @@ TEST(ErrorOutput, ModelErrorOutput) EXPECT_NE(std::string::npos, errors[0].Message().find( "Non-unique name[common_name] detected 7 times in XML children of model" " with name[test_model].")); - EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::WARNING); EXPECT_NE(std::string::npos, errors[1].Message().find( - "link with name[common_name] already exists.")); - EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::WARNING); - EXPECT_NE(std::string::npos, errors[2].Message().find( "Link with name [common_name] in model with name [test_model] has a " "name collision, changing link name to [common_name_link].")); - EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[2].Message().find( + "link with name[common_name] already exists.")); + EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::WARNING); EXPECT_NE(std::string::npos, errors[3].Message().find( - "joint with name[common_name] already exists.")); - EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::WARNING); - EXPECT_NE(std::string::npos, errors[4].Message().find( "Joint with name [common_name] in model with name [test_model] has a " "name collision, changing joint name to [common_name_joint].")); - EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[4].Message().find( + "joint with name[common_name] already exists.")); + EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::WARNING); EXPECT_NE(std::string::npos, errors[5].Message().find( - "frame with name[common_name] already exists.")); - EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::WARNING); - EXPECT_NE(std::string::npos, errors[6].Message().find( "Frame with name [common_name] in model with name [test_model] has a " "name collision, changing frame name to [common_name_frame].")); + EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[6].Message().find( + "frame with name[common_name] already exists.")); // Check nothing has been printed EXPECT_TRUE(buffer.str().empty()) << buffer.str(); errors.clear(); diff --git a/test/integration/interface_api.cc b/test/integration/interface_api.cc index f85d0b7b5..d41c07c61 100644 --- a/test/integration/interface_api.cc +++ b/test/integration/interface_api.cc @@ -30,6 +30,8 @@ #include "sdf/Frame.hh" #include "sdf/InterfaceElements.hh" #include "sdf/InterfaceModel.hh" +#include "sdf/Joint.hh" +#include "sdf/Link.hh" #include "sdf/Model.hh" #include "sdf/Param.hh" #include "sdf/PrintConfig.hh" @@ -397,26 +399,56 @@ TEST_F(InterfaceAPI, TomlParserModelInclude) TomlParserTest(interfaceModel); } +///////////////////////////////////////////////// +TEST_F(InterfaceAPI, DeeplyNestedModel) +{ + const std::string testSdf = R"( + + + + model://model_include_with_interface_api.sdf + + + )"; + + this->config.AddURIPath("model://", sdf::testing::TestFile("sdf")); + this->config.RegisterCustomModelParser(this->customTomlParser); + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(testSdf, this->config); + EXPECT_TRUE(errors.empty()) << errors; + const sdf::Model *grandParentModel = root.Model(); + ASSERT_NE(nullptr, grandParentModel); + EXPECT_EQ("grand_parent_model", grandParentModel->Name()); + const sdf::Model *parentModel = grandParentModel->ModelByIndex(0); + ASSERT_NE(nullptr, parentModel); + EXPECT_EQ("parent_model", parentModel->Name()); + auto interfaceModel = parentModel->InterfaceModelByIndex(0); + ASSERT_NE(nullptr, interfaceModel); + SCOPED_TRACE("DeeplyNestedModel"); + TomlParserTest(interfaceModel); +} + +gz::math::Pose3d resolvePoseNoErrors(const sdf::SemanticPose &_semPose, + const std::string &_relativeTo = "") +{ + gz::math::Pose3d pose; + sdf::Errors resolveErrors = _semPose.Resolve(pose, _relativeTo); + EXPECT_TRUE(resolveErrors.empty()) << resolveErrors; + return pose; +} + +std::string resolveAttachedToNoErrors(const sdf::Frame &_frame) +{ + std::string resolvedBody; + sdf::Errors resolveErrors = _frame.ResolveAttachedToBody(resolvedBody); + EXPECT_TRUE(resolveErrors.empty()) << resolveErrors; + return resolvedBody; +} + void InterfaceAPI::CheckFrameSemantics(const sdf::World *world) { using gz::math::Pose3d; - auto resolvePoseNoErrors = - [](const sdf::SemanticPose &_semPose, const std::string &_relativeTo = "") - { - Pose3d pose; - sdf::Errors resolveErrors = _semPose.Resolve(pose, _relativeTo); - EXPECT_TRUE(resolveErrors.empty()) << resolveErrors; - return pose; - }; - auto resolveAttachedToNoErrors = [](const sdf::Frame &_frame) - { - std::string resolvedBody; - sdf::Errors resolveErrors = _frame.ResolveAttachedToBody(resolvedBody); - EXPECT_TRUE(resolveErrors.empty()) << resolveErrors; - return resolvedBody; - }; - { const sdf::Frame *frame = world->FrameByName("F0"); ASSERT_NE(nullptr, frame); @@ -908,6 +940,7 @@ TEST_F(InterfaceAPI, NameCollision) } } +///////////////////////////////////////////////// class InterfaceAPIMergeInclude : public InterfaceAPI { }; @@ -1272,6 +1305,297 @@ TEST_F(InterfaceAPIMergeInclude, JointModelChild) } } +///////////////////////////////////////////////// +TEST_F(InterfaceAPIMergeInclude, DeeplyNestedMergeInclude1a) +{ + auto checkParentNameParser = + [this](const sdf::NestedInclude &_include, sdf::Errors &_errors) + { + EXPECT_EQ("parent_model::intermediate_model", + _include.AbsoluteParentName()); + return this->customTomlParser(_include, _errors); + }; + + this->config.RegisterCustomModelParser(checkParentNameParser); + + const std::string testSdf = R"( + + + + + intermediate_model_with_interface_api_1.sdf + 0 10 0 0 0 0 + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(testSdf, this->config); + EXPECT_TRUE(errors.empty()) << errors; + const auto* parentModel = root.Model(); + ASSERT_NE(nullptr, parentModel); + EXPECT_NE(nullptr, parentModel->ModelByName("intermediate_model")); + using gz::math::Pose3d; + EXPECT_EQ( + Pose3d(0, 10, 10, 0, 0, 0), + resolvePoseNoErrors(parentModel->SemanticPose(), + "parent_model::intermediate_model::double_pendulum") + .Inverse()); +} + +///////////////////////////////////////////////// +TEST_F(InterfaceAPIMergeInclude, DeeplyNestedMergeInclude1b) +{ + auto checkParentNameParser = + [this](const sdf::NestedInclude &_include, sdf::Errors &_errors) + { + EXPECT_EQ("parent_model", + _include.AbsoluteParentName()); + return this->customTomlParser(_include, _errors); + }; + + this->config.RegisterCustomModelParser(checkParentNameParser); + + const std::string testSdf = R"( + + + + intermediate_model_with_interface_api_1.sdf + 0 10 0 0 0 0 + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(testSdf, this->config); + EXPECT_TRUE(errors.empty()) << errors; + const auto* parentModel = root.Model(); + ASSERT_NE(nullptr, parentModel); + EXPECT_EQ(nullptr, parentModel->ModelByName("intermediate_model")); + using gz::math::Pose3d; + EXPECT_EQ(Pose3d(0, 10, 10, 0, 0, 0), + resolvePoseNoErrors(parentModel->SemanticPose(), + "parent_model::double_pendulum") + .Inverse()); +} + +///////////////////////////////////////////////// +TEST_F(InterfaceAPIMergeInclude, DeeplyNestedMergeInclude2) +{ + auto checkParentNameParser = + [this](const sdf::NestedInclude &_include, sdf::Errors &_errors) + { + EXPECT_EQ("parent_model", _include.AbsoluteParentName()); + return this->customTomlParser(_include, _errors); + }; + + this->config.RegisterCustomModelParser(checkParentNameParser); + + const std::string testSdf = R"( + + + + intermediate_model_with_interface_api_2.sdf + 0 10 0 0 0 0 + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(testSdf, this->config); + EXPECT_TRUE(errors.empty()) << errors; + const auto* parentModel = root.Model(); + ASSERT_NE(nullptr, parentModel); + using gz::math::Pose3d; + EXPECT_EQ( + Pose3d(1, 10, 10.5, 0, 0, 0), + resolvePoseNoErrors(parentModel->SemanticPose(), "parent_model::base") + .Inverse()); +} + +///////////////////////////////////////////////// +TEST_F(InterfaceAPIMergeInclude, DeeplyNestedMergeIncludePlacementFrame) +{ + this->config.RegisterCustomModelParser(this->customTomlParser); + + const std::string testSdf = R"( + + + + intermediate_model_with_interface_api_1.sdf + double_pendulum::lower_link + 0 10 0 0 0 0 + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(testSdf, this->config); + EXPECT_TRUE(errors.empty()) << errors; + const auto* parentModel = root.Model(); + ASSERT_NE(nullptr, parentModel); + using gz::math::Pose3d; + EXPECT_EQ(Pose3d(0, 10, 0, 0, 0, 0), + resolvePoseNoErrors(parentModel->SemanticPose(), + "parent_model::double_pendulum::lower_link") + .Inverse()); +} + +///////////////////////////////////////////////// +TEST_F(InterfaceAPIMergeInclude, DeeplyNestedMergeIncludeInWorldPlacementFrame) +{ + this->config.RegisterCustomModelParser(this->customTomlParser); + + const std::string testSdf = R"( + + + + + intermediate_model_with_interface_api_1.sdf + double_pendulum::lower_link + 0 10 0 0 0 0 + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(testSdf, this->config); + EXPECT_TRUE(errors.empty()) << errors; + const auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + const auto *worldFrame = world->FrameByName("world_frame"); + ASSERT_NE(nullptr, worldFrame); + using gz::math::Pose3d; + EXPECT_EQ(Pose3d(0, 10, 0, 0, 0, 0), + resolvePoseNoErrors(worldFrame->SemanticPose(), + "double_pendulum::lower_link") + .Inverse()); +} + +///////////////////////////////////////////////// +TEST_F(InterfaceAPIMergeInclude, DeeplyNestedMergeIncludeInWorld) +{ + auto checkParentNameParser = + [this](const sdf::NestedInclude &_include, sdf::Errors &_errors) + { return this->customTomlParser(_include, _errors); }; + + this->config.RegisterCustomModelParser(checkParentNameParser); + + const std::string testSdf = R"( + + + + intermediate_model_with_interface_api_1.sdf + 0 10 0 0 0 0 + + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(testSdf, this->config); + EXPECT_TRUE(errors.empty()) << errors; + const auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + EXPECT_EQ(nullptr, world->ModelByName("intermediate_model")); + const auto *worldFrame = world->FrameByName("world_frame"); + ASSERT_NE(nullptr, worldFrame); + using gz::math::Pose3d; + EXPECT_EQ(Pose3d(0, 10, 10, 0, 0, 0), + resolvePoseNoErrors(worldFrame->SemanticPose(), "double_pendulum") + .Inverse()); +} + +///////////////////////////////////////////////// +TEST_F(InterfaceAPIMergeInclude, DeeplyNestedMergeIncludeElementOrder) +{ + auto checkParentNameParser = + [this](const sdf::NestedInclude &_include, sdf::Errors &_errors) + { + return this->customTomlParser(_include, _errors); + }; + + this->config.RegisterCustomModelParser(checkParentNameParser); + + const std::string testSdf = R"( + + + + + + world + L0 + + + test_model_with_frames + + + intermediate_model_with_interface_api_1.sdf + 0 10 0 0 0 0 + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(testSdf, this->config); + EXPECT_TRUE(errors.empty()) << errors; + const auto* parentModel = root.Model(); + ASSERT_NE(nullptr, parentModel); + EXPECT_NE(nullptr, parentModel->ModelByName("M2")); + EXPECT_NE(nullptr, parentModel->ModelByName("intermediate_model")); + ASSERT_GE(parentModel->LinkCount(), 2u); + EXPECT_EQ("L0", parentModel->LinkByIndex(0)->Name()); + EXPECT_EQ("L1", parentModel->LinkByIndex(1)->Name()); + EXPECT_NE(nullptr, parentModel->CanonicalLinkAndRelativeName().first); + EXPECT_EQ("L0", parentModel->CanonicalLinkAndRelativeName().second); + ASSERT_GE(parentModel->FrameCount(), 1u); + EXPECT_EQ("F0", parentModel->FrameByIndex(0)->Name()); + ASSERT_GE(parentModel->JointCount(), 1u); + EXPECT_EQ("J0", parentModel->JointByIndex(0)->Name()); +} + +///////////////////////////////////////////////// +TEST_F(InterfaceAPIMergeInclude, DeeplyNestedMergeIncludeElementOrderInWorld) +{ + auto checkParentNameParser = + [this](const sdf::NestedInclude &_include, sdf::Errors &_errors) + { + return this->customTomlParser(_include, _errors); + }; + + this->config.RegisterCustomModelParser(checkParentNameParser); + + const std::string testSdf = R"( + + + + + world + M1 + + + model_for_world_merge_include.sdf + + + intermediate_model_with_interface_api_1.sdf + 0 10 0 0 0 0 + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(testSdf, this->config); + EXPECT_TRUE(errors.empty()) << errors; + const auto* world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + EXPECT_NE(nullptr, world->ModelByName("M1")); + EXPECT_NE(nullptr, world->ModelByName("intermediate_model")); + ASSERT_GE(world->FrameCount(), 1u); + EXPECT_EQ("F0", world->FrameByIndex(0)->Name()); + ASSERT_GE(world->JointCount(), 1u); + EXPECT_EQ("J0", world->JointByIndex(0)->Name()); +} + ///////////////////////////////////////////////// TEST_F(InterfaceAPI, JointParentOrChildInNestedModel) { diff --git a/test/integration/model/intermediate_model_with_interface_api_1.sdf b/test/integration/model/intermediate_model_with_interface_api_1.sdf new file mode 100644 index 000000000..d8468c643 --- /dev/null +++ b/test/integration/model/intermediate_model_with_interface_api_1.sdf @@ -0,0 +1,11 @@ + + + + + + double_pendulum.toml + 0 0 10 0 0 0 + + + + diff --git a/test/integration/model/intermediate_model_with_interface_api_2.sdf b/test/integration/model/intermediate_model_with_interface_api_2.sdf new file mode 100644 index 000000000..56c0c93c8 --- /dev/null +++ b/test/integration/model/intermediate_model_with_interface_api_2.sdf @@ -0,0 +1,11 @@ + + + + + + double_pendulum.toml + 0 0 10 0 0 0 + + + +