From 731983609cdec313f3fa8256bcae57678481a9f7 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 17 Aug 2020 10:41:57 -0700 Subject: [PATCH] [sdf9] Support nested models in DOM and frame semantics (#316) Nested model elements (`//model/model`) are currently supported in the SDFormat 1.7 spec, but are not supported by the DOM API or frame semantics operations in libsdformat 9.2. Per Amendment 1 of the SDFormat 1.7 proposal (sdformat.org/tutorials?tut=pose_frame_semantics_proposal#amendment-1-directly-nested-models), this PR adds support for nested models in the DOM API and frame semantics (fixing #283) through three steps: * adding `Model::Model*` methods for accessing nested models via the DOM API (047ec96) * loading nested models in `Model::Load` (b57fea2, step 3 of model parsing stages (sdformat.org/tutorials?tut=pose_frame_semantics_proposal#1-model)) * supporting nested models (`//model/model`) in frame semantics as opaque frames to match how models in the world scope (`//world/model`) are treated (85e0b4f, steps 6-9 of model parsing stages (sdformat.org/tutorials?tut=pose_frame_semantics_proposal#1-model)) The first two steps are straightforward, while the 3rd step adds new behavior. This behavior is added to `libsdformat9` because it is compatible with the SDFormat 1.7 spec (which supports nested models) and makes the treatment of models more consistent with regard to frame semantics. In libsdformat 9.2.0, a `//world/model` supports frame semantics: they have frames that can be referenced by name with `//pose/@relative_to` and `//world/frame/@attached_to` values can resolve to a `//world/model`. This extends that same behavior to nested `//model/model` elements; they now have their own frames in the frame and pose graphs, and a `//model/frame` is permitted to attach to a model. This does not add support for referencing elements within a model via the `::` syntax in the frame semantics or DOM APIs. That will be added in SDFormat 1.8 (see #293). Signed-off-by: Steve Peters --- CMakeLists.txt | 2 +- Changelog.md | 5 + Migration.md | 6 +- include/sdf/Model.hh | 28 +++- sdf/1.7/frame.sdf | 2 +- src/FrameSemantics.cc | 151 +++++++++++++++--- src/FrameSemantics.hh | 5 + src/Model.cc | 133 +++++++++++++-- src/Model_TEST.cc | 14 ++ src/SemanticPose.cc | 2 +- src/World.cc | 5 +- src/ign_TEST.cc | 67 ++++++-- src/parser.cc | 5 +- test/integration/frame.cc | 55 ++++++- test/integration/joint_dom.cc | 2 +- test/integration/link_dom.cc | 2 +- test/integration/model_dom.cc | 106 +++++++++++- .../model_frame_attached_to_nested_model.sdf | 11 ++ test/sdf/model_invalid_nested_joint_child.sdf | 16 ++ test/sdf/model_nested_model_relative_to.sdf | 18 +++ 20 files changed, 569 insertions(+), 66 deletions(-) create mode 100644 test/sdf/model_frame_attached_to_nested_model.sdf create mode 100644 test/sdf/model_invalid_nested_joint_child.sdf create mode 100644 test/sdf/model_nested_model_relative_to.sdf diff --git a/CMakeLists.txt b/CMakeLists.txt index 309a85590..3e5264c3a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,7 @@ project (sdformat9) set (SDF_PROTOCOL_VERSION 1.7) set (SDF_MAJOR_VERSION 9) -set (SDF_MINOR_VERSION 2) +set (SDF_MINOR_VERSION 3) set (SDF_PATCH_VERSION 0) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) diff --git a/Changelog.md b/Changelog.md index 126a55c2c..82333b517 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,11 @@ ### SDFormat 9.X.X (202X-XX-XX) +### SDFormat 9.3.0 (2020-XX-XX) + +1. Support nested models in DOM and frame semantics. + * [Pull request 316](https://github.com/osrf/sdformat/pull/316) + 1. Find python3 in cmake, fix cmake warning. * [Pull request 328](https://github.com/osrf/sdformat/pull/328) diff --git a/Migration.md b/Migration.md index a9cc6a395..90385a0c9 100644 --- a/Migration.md +++ b/Migration.md @@ -45,6 +45,10 @@ but with improved human-readability.. + const Frame \*FrameByIndex(const uint64\_t) const + const Frame \*FrameByName(const std::string &) const + bool FrameNameExists(const std::string &) const + + uint64\_t ModelCount() const + + const Model \*ModelByIndex(const uint64\_t) const + + const Model \*ModelByName(const std::string &) const + + bool ModelNameExists(const std::string &) const + sdf::SemanticPose SemanticPose() const 1. **sdf/SDFImpl.hh** @@ -194,7 +198,7 @@ but with improved human-readability.. 1. **frame.sdf** `//frame/@attached_to` attribute + description: Name of the link or frame to which this frame is attached. If a frame is specified, recursively following the attached\_to attributes - of the specified frames must lead to the name of a link or the world frame. + of the specified frames must lead to the name of a link, a model, or the world frame. + type: string + default: "" + required: * diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 018801982..677407212 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -198,6 +198,29 @@ namespace sdf /// \return True if there exists an explicit frame with the given name. public: bool FrameNameExists(const std::string &_name) const; + /// \brief Get the number of nested models. + /// \return Number of nested models contained in this Model object. + public: uint64_t ModelCount() const; + + /// \brief Get a nested model based on an index. + /// \param[in] _index Index of the nested model. The index should be in the + /// range [0..ModelCount()). + /// \return Pointer to the model. Nullptr if the index does not exist. + /// \sa uint64_t ModelCount() const + public: const Model *ModelByIndex(const uint64_t _index) const; + + /// \brief Get whether a nested model name exists. + /// \param[in] _name Name of the nested model to check. + /// \return True if there exists a nested model with the given name. + public: bool ModelNameExists(const std::string &_name) const; + + /// \brief Get a nested model based on a name. + /// \param[in] _name Name of the nested model. + /// \return Pointer to the model. Nullptr if a model with the given name + /// does not exist. + /// \sa bool ModelNameExists(const std::string &_name) const + public: const Model *ModelByName(const std::string &_name) const; + /// \brief Get the pose of the model. This is the pose of the model /// as specified in SDF ( ... ), and is /// typically used to express the position and rotation of a model in a @@ -281,9 +304,10 @@ namespace sdf /// \brief Give a weak pointer to the PoseRelativeToGraph to be used /// for resolving poses. This is private and is intended to be called by - /// World::Load. + /// World::Load and Model::Load if this is a nested model. /// \param[in] _graph Weak pointer to PoseRelativeToGraph. - private: void SetPoseRelativeToGraph( + /// \return Error if graph pointer is invalid. + private: sdf::Errors SetPoseRelativeToGraph( std::weak_ptr _graph); /// \brief Allow World::Load to call SetPoseRelativeToGraph. diff --git a/sdf/1.7/frame.sdf b/sdf/1.7/frame.sdf index be28ffe9b..dcc1c3397 100644 --- a/sdf/1.7/frame.sdf +++ b/sdf/1.7/frame.sdf @@ -10,7 +10,7 @@ Name of the link or frame to which this frame is attached. If a frame is specified, recursively following the attached_to attributes - of the specified frames must lead to the name of a link or the world frame. + of the specified frames must lead to the name of a link, a model, or the world frame. diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 6f606e3a4..a3918ab7a 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -285,6 +285,23 @@ Errors buildFrameAttachedToGraph( _out.map[frame->Name()] = frameId; } + // add nested model vertices + for (uint64_t m = 0; m < _model->ModelCount(); ++m) + { + auto nestedModel = _model->ModelByIndex(m); + if (_out.map.count(nestedModel->Name()) > 0) + { + errors.push_back({ErrorCode::DUPLICATE_NAME, + "Nested model with non-unique name [" + nestedModel->Name() + + "] detected in model with name [" + _model->Name() + + "]."}); + continue; + } + auto nestedModelId = + _out.graph.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); + _out.map[nestedModel->Name()] = nestedModelId; + } + // add frame edges for (uint64_t f = 0; f < _model->FrameCount(); ++f) { @@ -302,7 +319,7 @@ Errors buildFrameAttachedToGraph( errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "attached_to name[" + attachedTo + "] specified by frame with name[" + frame->Name() + - "] does not match a link, joint, or frame name " + "] does not match a nested model, link, joint, or frame name " "in model with name[" + _model->Name() + "]."}); continue; } @@ -352,9 +369,9 @@ Errors buildFrameAttachedToGraph( } // add model vertices - for (uint64_t l = 0; l < _world->ModelCount(); ++l) + for (uint64_t m = 0; m < _world->ModelCount(); ++m) { - auto model = _world->ModelByIndex(l); + auto model = _world->ModelByIndex(m); if (_out.map.count(model->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, @@ -537,6 +554,30 @@ Errors buildPoseRelativeToGraph( } } + // add nested model vertices and default edge if relative_to is empty + for (uint64_t m = 0; m < _model->ModelCount(); ++m) + { + auto nestedModel = _model->ModelByIndex(m); + if (_out.map.count(nestedModel->Name()) > 0) + { + errors.push_back({ErrorCode::DUPLICATE_NAME, + "Nested model with non-unique name [" + nestedModel->Name() + + "] detected in model with name [" + _model->Name() + + "]."}); + continue; + } + auto nestedModelId = + _out.graph.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); + _out.map[nestedModel->Name()] = nestedModelId; + + if (nestedModel->PoseRelativeTo().empty()) + { + // relative_to is empty, so add edge from implicit model frame + // to nestedModel + _out.graph.AddEdge({modelFrameId, nestedModelId}, nestedModel->RawPose()); + } + } + // now that all vertices have been added to the graph, // add the edges that reference other vertices @@ -559,7 +600,7 @@ Errors buildPoseRelativeToGraph( errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + "] specified by link with name[" + link->Name() + - "] does not match a link, joint, or frame name " + "] does not match a nested model, link, joint, or frame name " "in model with name[" + _model->Name() + "]."}); continue; } @@ -594,7 +635,7 @@ Errors buildPoseRelativeToGraph( errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + "] specified by joint with name[" + joint->Name() + - "] does not match a link, joint, or frame name " + "] does not match a nested model, link, joint, or frame name " "in model with name[" + _model->Name() + "]."}); continue; } @@ -643,7 +684,7 @@ Errors buildPoseRelativeToGraph( errors.push_back({errorCode, typeForErrorMsg + " name[" + relativeTo + "] specified by frame with name[" + frame->Name() + - "] does not match a link, joint, or frame name " + "] does not match a nested model, link, joint, or frame name " "in model with name[" + _model->Name() + "]."}); continue; } @@ -659,6 +700,41 @@ Errors buildPoseRelativeToGraph( _out.graph.AddEdge({relativeToId, frameId}, frame->RawPose()); } + for (uint64_t m = 0; m < _model->ModelCount(); ++m) + { + auto nestedModel = _model->ModelByIndex(m); + + // check if we've already added a default edge + const std::string relativeTo = nestedModel->PoseRelativeTo(); + if (relativeTo.empty()) + { + continue; + } + + auto nestedModelId = _out.map.at(nestedModel->Name()); + + // look for vertex in graph that matches relative_to value + if (_out.map.count(relativeTo) != 1) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, + "relative_to name[" + relativeTo + + "] specified by nested model with name[" + nestedModel->Name() + + "] does not match a nested model, link, joint, or frame name " + "in model with name[" + _model->Name() + "]."}); + continue; + } + auto relativeToId = _out.map[relativeTo]; + if (nestedModel->Name() == relativeTo) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, + "relative_to name[" + relativeTo + + "] is identical to nested model name[" + nestedModel->Name() + + "], causing a graph cycle " + "in model with name[" + _model->Name() + "]."}); + } + _out.graph.AddEdge({relativeToId, nestedModelId}, nestedModel->RawPose()); + } + return errors; } @@ -916,6 +992,22 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) "in MODEL attached_to graph."}); } break; + case sdf::FrameType::MODEL: + if ("__model__" != vertexPair.second.get().Name()) + { + if (outDegree != 0) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error, " + "nested MODEL vertex with name [" + + vertexPair.second.get().Name() + + "] should have no outgoing edges " + "in MODEL attached_to graph."}); + } + break; + } + // fall through to default case for __model__ + [[fallthrough]]; default: if (outDegree == 0) { @@ -1065,22 +1157,26 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) "should not have type WORLD in MODEL relative_to graph."}); break; case sdf::FrameType::MODEL: - if (inDegree != 0) + if ("__model__" == vertexPair.second.get().Name()) { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error, " - "MODEL vertex with name [" + - vertexPair.second.get().Name() + - "] should have no incoming edges " - "in MODEL relative_to graph."}); + if (inDegree != 0) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, + "PoseRelativeToGraph error, " + "MODEL vertex with name [__model__" + "] should have no incoming edges " + "in MODEL relative_to graph."}); + } + break; } - break; + // fall through to default case for nested models + [[fallthrough]]; default: if (inDegree == 0) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " - "Non-MODEL vertex with name [" + + "Vertex with name [" + vertexPair.second.get().Name() + "] is disconnected; it should have 1 incoming edge " + "in MODEL relative_to graph."}); @@ -1206,13 +1302,26 @@ Errors resolveFrameAttachedToBody( return errors; } - if (_in.scopeName == "__model__" && sinkVertex.Data() != FrameType::LINK) + if (_in.scopeName == "__model__") { - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "Graph has __model__ scope but sink vertex named [" + - sinkVertex.Name() + "] does not have FrameType LINK " - "when starting from vertex with name [" + _vertexName + "]."}); - return errors; + if (sinkVertex.Data() == FrameType::MODEL && + sinkVertex.Name() == "__model__") + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "Graph with __model__ scope has sink vertex named [__model__] " + "when starting from vertex with name [" + _vertexName + "], " + "which is not permitted."}); + return errors; + } + else if (sinkVertex.Data() != FrameType::LINK && + sinkVertex.Data() != FrameType::MODEL) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "Graph has __model__ scope but sink vertex named [" + + sinkVertex.Name() + "] does not have FrameType LINK OR MODEL " + "when starting from vertex with name [" + _vertexName + "]."}); + return errors; + } } _attachedToBody = sinkVertex.Name(); diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index 8de5bb040..a6c1a59df 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -32,6 +32,11 @@ /// The Frame Semantics Utilities construct and operate on graphs representing /// the kinematics, frame attached_to, and pose relative_to relationships /// defined within models and world. +/// +/// Note that all graphs should only contain relative names (e.g. "my_link"), +/// not absolute names ("top_model::nested_model::my_link"). +/// Graphs inside nested models (currently via directly nested models in +/// //model or //world elements) will be explicitly separate graphs. namespace sdf { // Inline bracket to help doxygen filtering. diff --git a/src/Model.cc b/src/Model.cc index fe8be53ee..f359075ca 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -67,6 +67,9 @@ class sdf::ModelPrivate /// \brief The frames specified in this model. public: std::vector frames; + /// \brief The nested models specified in this model. + public: std::vector models; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -76,8 +79,11 @@ class sdf::ModelPrivate /// \brief Pose Relative-To Graph constructed during Load. public: std::shared_ptr poseGraph; - /// \brief Pose Relative-To Graph in parent (world) scope. + /// \brief Pose Relative-To Graph in parent (world or __model__) scope. public: std::weak_ptr parentPoseGraph; + + /// \brief Scope name of parent Pose Relative-To Graph (world or __model__). + public: std::string parentPoseGraphScopeName; }; ///////////////////////////////////////////////// @@ -112,6 +118,10 @@ Model::Model(const Model &_model) { link.SetPoseRelativeToGraph(this->dataPtr->poseGraph); } + for (auto &model : this->dataPtr->models) + { + model.SetPoseRelativeToGraph(this->dataPtr->poseGraph); + } for (auto &joint : this->dataPtr->joints) { joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph); @@ -197,14 +207,6 @@ Errors Model::Load(ElementPtr _sdf) // Load the pose. Ignore the return value since the model pose is optional. loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo); - // Nested models are not yet supported. - if (_sdf->HasElement("model")) - { - errors.push_back({ErrorCode::NESTED_MODELS_UNSUPPORTED, - "Nested models are not yet supported by DOM objects, " - "skipping model [" + this->dataPtr->name + "]."}); - } - if (!_sdf->HasUniqueChildNames()) { sdfwarn << "Non-unique names detected in XML children of model with name[" @@ -215,17 +217,56 @@ Errors Model::Load(ElementPtr _sdf) // name collisions std::unordered_set frameNames; + // Load nested models. + Errors nestedModelLoadErrors = loadUniqueRepeated(_sdf, "model", + this->dataPtr->models); + 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) + { + frameNames.insert(model.Name()); + } + // Load all the links. Errors linkLoadErrors = loadUniqueRepeated(_sdf, "link", this->dataPtr->links); errors.insert(errors.end(), linkLoadErrors.begin(), linkLoadErrors.end()); - // Links 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 &link : this->dataPtr->links) + // Check links for name collisions and modify and warn if so. + for (auto &link : this->dataPtr->links) { - frameNames.insert(link.Name()); + std::string linkName = link.Name(); + if (frameNames.count(linkName) > 0) + { + // This link has a name collision + if (sdfVersion < ignition::math::SemanticVersion(1, 7)) + { + // This came from an old file, so try to workaround by renaming link + linkName += "_link"; + int i = 0; + while (frameNames.count(linkName) > 0) + { + linkName = link.Name() + "_link" + std::to_string(i++); + } + sdfwarn << "Link with name [" << link.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision, changing link name to [" + << linkName << "].\n"; + link.SetName(linkName); + } + else + { + sdferr << "Link with name [" << link.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision. Please rename this link.\n"; + } + } + frameNames.insert(linkName); } // If the model is not static: @@ -348,6 +389,13 @@ Errors Model::Load(ElementPtr _sdf) { link.SetPoseRelativeToGraph(this->dataPtr->poseGraph); } + for (auto &model : this->dataPtr->models) + { + Errors setPoseRelativeToGraphErrors = + model.SetPoseRelativeToGraph(this->dataPtr->poseGraph); + errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), + setPoseRelativeToGraphErrors.end()); + } for (auto &joint : this->dataPtr->joints) { joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph); @@ -527,6 +575,46 @@ const Frame *Model::FrameByName(const std::string &_name) const return nullptr; } +///////////////////////////////////////////////// +uint64_t Model::ModelCount() const +{ + return this->dataPtr->models.size(); +} + +///////////////////////////////////////////////// +const Model *Model::ModelByIndex(const uint64_t _index) const +{ + if (_index < this->dataPtr->models.size()) + return &this->dataPtr->models[_index]; + return nullptr; +} + +///////////////////////////////////////////////// +bool Model::ModelNameExists(const std::string &_name) const +{ + for (auto const &m : this->dataPtr->models) + { + if (m.Name() == _name) + { + return true; + } + } + return false; +} + +///////////////////////////////////////////////// +const Model *Model::ModelByName(const std::string &_name) const +{ + for (auto const &m : this->dataPtr->models) + { + if (m.Name() == _name) + { + return &m; + } + } + return nullptr; +} + ///////////////////////////////////////////////// const Link *Model::CanonicalLink() const { @@ -601,10 +689,23 @@ void Model::SetPoseRelativeTo(const std::string &_frame) } ///////////////////////////////////////////////// -void Model::SetPoseRelativeToGraph( +Errors Model::SetPoseRelativeToGraph( std::weak_ptr _graph) { + Errors errors; + + auto graph = _graph.lock(); + if (!graph) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, + "Tried to set PoseRelativeToGraph with invalid pointer."}); + return errors; + } + + this->dataPtr->parentPoseGraphScopeName = graph->sourceName; this->dataPtr->parentPoseGraph = _graph; + + return errors; } ///////////////////////////////////////////////// @@ -613,7 +714,7 @@ sdf::SemanticPose Model::SemanticPose() const return sdf::SemanticPose( this->dataPtr->pose, this->dataPtr->poseRelativeTo, - "world", + this->dataPtr->parentPoseGraphScopeName, this->dataPtr->parentPoseGraph); } diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 2d5293e75..86584b4a5 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -47,21 +47,35 @@ TEST(DOMModel, Construction) model.SetEnableWind(true); EXPECT_TRUE(model.EnableWind()); + EXPECT_EQ(0u, model.ModelCount()); + EXPECT_EQ(nullptr, model.ModelByIndex(0)); + EXPECT_EQ(nullptr, model.ModelByIndex(1)); + EXPECT_EQ(nullptr, model.ModelByName("")); + EXPECT_EQ(nullptr, model.ModelByName("default")); + EXPECT_FALSE(model.ModelNameExists("")); + EXPECT_FALSE(model.ModelNameExists("default")); + EXPECT_EQ(0u, model.LinkCount()); EXPECT_EQ(nullptr, model.LinkByIndex(0)); EXPECT_EQ(nullptr, model.LinkByIndex(1)); + EXPECT_EQ(nullptr, model.LinkByName("")); + EXPECT_EQ(nullptr, model.LinkByName("default")); EXPECT_FALSE(model.LinkNameExists("")); EXPECT_FALSE(model.LinkNameExists("default")); EXPECT_EQ(0u, model.JointCount()); EXPECT_EQ(nullptr, model.JointByIndex(0)); EXPECT_EQ(nullptr, model.JointByIndex(1)); + EXPECT_EQ(nullptr, model.JointByName("")); + EXPECT_EQ(nullptr, model.JointByName("default")); EXPECT_FALSE(model.JointNameExists("")); EXPECT_FALSE(model.JointNameExists("default")); EXPECT_EQ(0u, model.FrameCount()); EXPECT_EQ(nullptr, model.FrameByIndex(0)); EXPECT_EQ(nullptr, model.FrameByIndex(1)); + EXPECT_EQ(nullptr, model.FrameByName("")); + EXPECT_EQ(nullptr, model.FrameByName("default")); EXPECT_FALSE(model.FrameNameExists("")); EXPECT_FALSE(model.FrameNameExists("default")); diff --git a/src/SemanticPose.cc b/src/SemanticPose.cc index 01e7f904e..eefc7d51c 100644 --- a/src/SemanticPose.cc +++ b/src/SemanticPose.cc @@ -106,7 +106,7 @@ Errors SemanticPose::Resolve( auto graph = this->dataPtr->poseRelativeToGraph.lock(); if (!graph) { - errors.push_back({ErrorCode::ELEMENT_INVALID, + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "SemanticPose has invalid pointer to PoseRelativeToGraph."}); return errors; } diff --git a/src/World.cc b/src/World.cc index c21646155..38ac9c1b3 100644 --- a/src/World.cc +++ b/src/World.cc @@ -372,7 +372,10 @@ Errors World::Load(sdf::ElementPtr _sdf) } for (auto &model : this->dataPtr->models) { - model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + Errors setPoseRelativeToGraphErrors = + model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), + setPoseRelativeToGraphErrors.end()); } for (auto &light : this->dataPtr->lights) { diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index b98c6e752..49ef7e680 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -275,6 +275,17 @@ TEST(check, SDF) std::string::npos) << output; } + // Check an SDF file with an invalid model without links. + { + std::string path = pathBase +"/model_without_links.sdf"; + + // Check model_without_links.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE(output.find("Error: A model must have at least one link."), + std::string::npos) << output; + } + // Check an SDF file with a nested model. { std::string path = pathBase +"/nested_model.sdf"; @@ -282,9 +293,7 @@ TEST(check, SDF) // Check nested_model.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(output.find("Error: Nested models are not yet supported by DOM " - "objects, skipping model [top_level_model]."), - std::string::npos) << output; + EXPECT_EQ("Valid.\n", output) << output; } // Check an invalid SDF file that uses reserved names. @@ -353,6 +362,17 @@ TEST(check, SDF) EXPECT_EQ("Valid.\n", output) << output; } + // Check an SDF file with model frames attached_to a nested model. + // This is a valid file. + { + std::string path = pathBase +"/model_frame_attached_to_nested_model.sdf"; + + // Check model_frame_attached_to_nested_model.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + // Check an SDF file with model frames with invalid attached_to attributes. { std::string path = pathBase +"/model_frame_invalid_attached_to.sdf"; @@ -361,8 +381,8 @@ TEST(check, SDF) std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_NE(output.find("Error: attached_to name[A] specified by frame with " - "name[F3] does not match a link, joint, or frame " - "name in model with " + "name[F3] does not match a nested model, link, " + "joint, or frame name in model with " "name[model_frame_invalid_attached_to]."), std::string::npos) << output; EXPECT_NE(output.find("Error: attached_to name[F4] is identical to frame " @@ -435,8 +455,8 @@ TEST(check, SDF) std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_NE(output.find("Error: relative_to name[A] specified by link with " - "name[L] does not match a link, joint, or frame " - "name in model with " + "name[L] does not match a nested model, link, " + "joint, or frame name in model with " "name[model_invalid_link_relative_to]."), std::string::npos) << output; EXPECT_NE(output.find("Error: relative_to name[self_cycle] is identical to " @@ -445,6 +465,31 @@ TEST(check, SDF) std::string::npos) << output; } + // Check an SDF file with nested_models using the relative_to attribute. + // This is a valid file. + { + std::string path = pathBase +"/model_nested_model_relative_to.sdf"; + + // Check model_nested_model_relative_to.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + + // Check an invalid SDF file with a joint that specifies a child link + // within a sibling nested model using the unsupported :: syntax. + { + std::string path = pathBase +"/model_invalid_nested_joint_child.sdf"; + + // Check model_invalid_nested_joint_child.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE(output.find("Error: Child link with name[M::C] specified by " + "joint with name[J] not found in model with " + "name[model_invalid_nested_joint_child]."), + std::string::npos) << output; + } + // Check an SDF file with joints using the relative_to attribute. // This is a valid file. { @@ -464,8 +509,8 @@ TEST(check, SDF) std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_NE(output.find("Error: relative_to name[A] specified by joint with " - "name[J] does not match a link, joint, or frame " - "name in model with " + "name[J] does not match a nested model, link, " + "joint, or frame name in model with " "name[model_invalid_joint_relative_to]."), std::string::npos) << output; EXPECT_NE(output.find("Error: relative_to name[Jcycle] is identical to " @@ -504,8 +549,8 @@ TEST(check, SDF) std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_NE(output.find("Error: relative_to name[A] specified by frame with " - "name[F] does not match a link, joint, or frame " - "name in model with " + "name[F] does not match a nested model, link, " + "joint, or frame name in model with " "name[model_invalid_frame_relative_to]."), std::string::npos) << output; EXPECT_NE(output.find("Error: relative_to name[cycle] is identical to " diff --git a/src/parser.cc b/src/parser.cc index 5e7fdbf27..911ef50c4 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1523,13 +1523,14 @@ bool checkFrameAttachedToNames(const sdf::Root *_root) modelResult = false; } else if (!_model->LinkNameExists(attachedTo) && + !_model->ModelNameExists(attachedTo) && !_model->JointNameExists(attachedTo) && !_model->FrameNameExists(attachedTo)) { std::cerr << "Error: attached_to name[" << attachedTo << "] specified by frame with name[" << frame->Name() - << "] does not match a link, joint, or frame name " - << "in model with name[" << _model->Name() + << "] does not match a nested model, link, joint, " + << "or frame name in model with name[" << _model->Name() << "]." << std::endl; modelResult = false; diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 6c9c89b12..02542f232 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -489,7 +489,7 @@ TEST(DOMFrame, LoadModelFramesInvalidAttachedTo) EXPECT_NE(std::string::npos, errors[0].Message().find( "attached_to name[A] specified by frame with name[F3] does not match a " - "link, joint, or frame name in model")); + "nested model, link, joint, or frame name in model")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_CYCLE); EXPECT_NE(std::string::npos, errors[1].Message().find( @@ -502,7 +502,7 @@ TEST(DOMFrame, LoadModelFramesInvalidAttachedTo) EXPECT_NE(std::string::npos, errors[5].Message().find( "attached_to name[A] specified by frame with name[F3] does not match a " - "link, joint, or frame name in model")); + "nested model, link, joint, or frame name in model")); EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_CYCLE); EXPECT_NE(std::string::npos, errors[6].Message().find( @@ -578,6 +578,55 @@ TEST(DOMFrame, LoadModelFramesAttachedToJoint) EXPECT_EQ("C", body); } +///////////////////////////////////////////////// +TEST(DOMFrame, LoadModelFramesAttachedToNestedModel) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_frame_attached_to_nested_model.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + // Get the first model + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ("model_frame_attached_to_nested_model", model->Name()); + EXPECT_EQ(1u, model->LinkCount()); + EXPECT_NE(nullptr, model->LinkByIndex(0)); + EXPECT_EQ(nullptr, model->LinkByIndex(1)); + + EXPECT_TRUE(model->LinkNameExists("link")); + + EXPECT_TRUE(model->CanonicalLinkName().empty()); + + EXPECT_EQ(0u, model->JointCount()); + EXPECT_EQ(nullptr, model->JointByIndex(0)); + + EXPECT_EQ(1u, model->ModelCount()); + EXPECT_NE(nullptr, model->ModelByIndex(0)); + EXPECT_EQ(nullptr, model->ModelByIndex(1)); + + EXPECT_TRUE(model->ModelNameExists("nested_model")); + + EXPECT_EQ(2u, model->FrameCount()); + EXPECT_NE(nullptr, model->FrameByIndex(0)); + EXPECT_NE(nullptr, model->FrameByIndex(1)); + EXPECT_EQ(nullptr, model->FrameByIndex(2)); + ASSERT_TRUE(model->FrameNameExists("F1")); + ASSERT_TRUE(model->FrameNameExists("F2")); + + EXPECT_EQ("nested_model", model->FrameByName("F1")->AttachedTo()); + EXPECT_EQ("F1", model->FrameByName("F2")->AttachedTo()); + + std::string body; + EXPECT_TRUE(model->FrameByName("F1")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("nested_model", body); + EXPECT_TRUE(model->FrameByName("F2")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("nested_model", body); +} + ///////////////////////////////////////////////// TEST(DOMFrame, LoadWorldFramesAttachedTo) { @@ -837,7 +886,7 @@ TEST(DOMFrame, LoadModelFramesInvalidRelativeTo) EXPECT_NE(std::string::npos, errors[0].Message().find( "relative_to name[A] specified by frame with name[F] does not match a " - "link, joint, or frame name in model")); + "nested model, link, joint, or frame name in model")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_CYCLE); EXPECT_NE(std::string::npos, errors[1].Message().find( diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index a7d7b0cd3..255a0e0fc 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -343,7 +343,7 @@ TEST(DOMJoint, LoadInvalidJointPoseRelativeTo) EXPECT_NE(std::string::npos, errors[1].Message().find( "relative_to name[A] specified by joint with name[J] does not match a " - "link, joint, or frame name in model")); + "nested model, link, joint, or frame name in model")); // errors[2] // errors[3] // errors[4] diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 0633d8f22..19b018da5 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -672,7 +672,7 @@ TEST(DOMLink, LoadInvalidLinkPoseRelativeTo) EXPECT_NE(std::string::npos, errors[0].Message().find( "relative_to name[A] specified by link with name[L] does not match a " - "link, joint, or frame name in model")); + "nested model, link, joint, or frame name in model")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_CYCLE); EXPECT_NE(std::string::npos, errors[1].Message().find( diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 171ba1edb..299100509 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -155,10 +155,7 @@ TEST(DOMRoot, NestedModel) // Load the SDF file sdf::Root root; auto errors = root.Load(testFile); - - // it should complain because nested models aren't yet supported - EXPECT_FALSE(errors.empty()); - EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::NESTED_MODELS_UNSUPPORTED); + EXPECT_TRUE(errors.empty()); EXPECT_EQ(1u, root.ModelCount()); @@ -181,6 +178,107 @@ TEST(DOMRoot, NestedModel) EXPECT_EQ(nullptr, model->JointByIndex(1)); EXPECT_TRUE(model->JointNameExists("top_level_joint")); + + ASSERT_EQ(1u, model->ModelCount()); + const sdf::Model *nestedModel = model->ModelByIndex(0); + ASSERT_NE(nullptr, nestedModel); + EXPECT_EQ(nullptr, model->ModelByIndex(1)); + + EXPECT_TRUE(model->ModelNameExists("nested_model")); + EXPECT_EQ(nestedModel, model->ModelByName("nested_model")); + EXPECT_EQ("nested_model", nestedModel->Name()); + + EXPECT_EQ(1u, nestedModel->LinkCount()); + EXPECT_NE(nullptr, nestedModel->LinkByIndex(0)); + EXPECT_EQ(nullptr, nestedModel->LinkByIndex(1)); + + EXPECT_TRUE(nestedModel->LinkNameExists("nested_link01")); + + EXPECT_EQ(0u, nestedModel->JointCount()); + EXPECT_EQ(0u, nestedModel->FrameCount()); +} + +///////////////////////////////////////////////// +TEST(DOMLink, NestedModelPoseRelativeTo) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_nested_model_relative_to.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + using Pose = ignition::math::Pose3d; + + // Get the first model + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ("model_nested_model_relative_to", model->Name()); + EXPECT_EQ(1u, model->LinkCount()); + EXPECT_NE(nullptr, model->LinkByIndex(0)); + EXPECT_EQ(nullptr, model->LinkByIndex(1)); + EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ("", model->PoseRelativeTo()); + + ASSERT_TRUE(model->LinkNameExists("L")); + EXPECT_TRUE(model->LinkByName("L")->PoseRelativeTo().empty()); + EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->LinkByName("L")->RawPose()); + + ASSERT_TRUE(model->ModelNameExists("M1")); + ASSERT_TRUE(model->ModelNameExists("M2")); + ASSERT_TRUE(model->ModelNameExists("M3")); + EXPECT_TRUE(model->ModelByName("M1")->PoseRelativeTo().empty()); + EXPECT_TRUE(model->ModelByName("M2")->PoseRelativeTo().empty()); + EXPECT_EQ("M1", model->ModelByName("M3")->PoseRelativeTo()); + + EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), model->ModelByName("M1")->RawPose()); + EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), model->ModelByName("M2")->RawPose()); + EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0), model->ModelByName("M3")->RawPose()); + + EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI / 2, 0), + model->ModelByName("M1")->SemanticPose().RawPose()); + EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), + model->ModelByName("M2")->SemanticPose().RawPose()); + EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0), + model->ModelByName("M3")->SemanticPose().RawPose()); + + // Test SemanticPose().Resolve to get each nested model pose in the + // __model__ frame + Pose pose; + EXPECT_TRUE( + model->ModelByName("M1")->SemanticPose().Resolve(pose, + "__model__").empty()); + EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), pose); + EXPECT_TRUE( + model->ModelByName("M2")->SemanticPose().Resolve(pose, + "__model__").empty()); + EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), pose); + EXPECT_TRUE( + model->ModelByName("M3")->SemanticPose().Resolve(pose, + "__model__").empty()); + EXPECT_EQ(Pose(1, 0, -3, 0, IGN_PI/2, 0), pose); + // test other API too + EXPECT_TRUE(model->ModelByName("M1")->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), pose); + EXPECT_TRUE(model->ModelByName("M2")->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), pose); + EXPECT_TRUE(model->ModelByName("M3")->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 0, -3, 0, IGN_PI/2, 0), pose); + + // resolve pose of M1 relative to M3 + // should be inverse of M3's Pose() + EXPECT_TRUE( + model->ModelByName("M1")->SemanticPose().Resolve(pose, "M3").empty()); + EXPECT_EQ(Pose(-3, 0, 0, 0, 0, 0), pose); + + EXPECT_TRUE(model->CanonicalLinkName().empty()); + + EXPECT_EQ(0u, model->JointCount()); + EXPECT_EQ(nullptr, model->JointByIndex(0)); + + EXPECT_EQ(0u, model->FrameCount()); + EXPECT_EQ(nullptr, model->FrameByIndex(0)); } ///////////////////////////////////////////////// diff --git a/test/sdf/model_frame_attached_to_nested_model.sdf b/test/sdf/model_frame_attached_to_nested_model.sdf new file mode 100644 index 000000000..995d828dd --- /dev/null +++ b/test/sdf/model_frame_attached_to_nested_model.sdf @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/test/sdf/model_invalid_nested_joint_child.sdf b/test/sdf/model_invalid_nested_joint_child.sdf new file mode 100644 index 000000000..efde66366 --- /dev/null +++ b/test/sdf/model_invalid_nested_joint_child.sdf @@ -0,0 +1,16 @@ + + + + + + 1 0 0 0 1.5707963267948966 0 + + + + P + + + M::C + + + diff --git a/test/sdf/model_nested_model_relative_to.sdf b/test/sdf/model_nested_model_relative_to.sdf new file mode 100644 index 000000000..fc3c4630b --- /dev/null +++ b/test/sdf/model_nested_model_relative_to.sdf @@ -0,0 +1,18 @@ + + + + + + 1 0 0 0 1.5707963267948966 0 + + + + 2 0 0 0 0 0 + + + + 3 0 0 0 0 0 + + + +