diff --git a/Migration.md b/Migration.md index 18ec08034..313586594 100644 --- a/Migration.md +++ b/Migration.md @@ -22,9 +22,10 @@ but with improved human-readability.. ### Modifications -1. **sdf/Model.hh**: the following methods now accept nested names - that can begin with a sequence of nested model names separated - by `::` and may end with the name of an object of the specified type. +1. **sdf/Model.hh**: the following methods now accept nested names relative to + the model's scope that can begin with a sequence of nested model names + separated by `::` and may end with the name of an object of the specified + type. + const Frame \*FrameByName(const std::string &) const + const Joint \*JointByName(const std::string &) const + const Link \*LinkByName(const std::string &) const diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 275f97164..bf66b9fc6 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -36,6 +36,7 @@ namespace sdf class Geometry; class Surface; struct PoseRelativeToGraph; + template class ScopedGraph; /// \brief A collision element descibes the collision properties associated /// with a link. This can be different from the visual properties of a link. @@ -141,12 +142,12 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph to call SetXmlParentName /// and SetPoseRelativeToGraph, but Link::SetPoseRelativeToGraph is diff --git a/include/sdf/Frame.hh b/include/sdf/Frame.hh index e5097e8ca..d9d9437d9 100644 --- a/include/sdf/Frame.hh +++ b/include/sdf/Frame.hh @@ -35,6 +35,7 @@ namespace sdf class FramePrivate; struct FrameAttachedToGraph; struct PoseRelativeToGraph; + template class ScopedGraph; /// \brief A Frame element descibes the properties associated with an /// explicit frame defined in a Model or World. @@ -133,9 +134,8 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Resolve the attached-to body of this frame from the - /// FrameAttachedToGraph. If this is in a __model__ scope, it returns - /// the name of a link. In the world scope, it returns the name of a - /// model or the world. + /// FrameAttachedToGraph. Generally, it resolves to the name of a link, but + /// if it is in the world scope, it can resolve to "world". /// \param[out] _body Name of body to which this frame is attached. /// \return Errors. public: Errors ResolveAttachedToBody(std::string &_body) const; @@ -145,19 +145,19 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give a weak pointer to the FrameAttachedToGraph to be used - /// for resolving attached bodies. This is private and is intended to - /// be called by Model::Load or World::Load. - /// \param[in] _graph Weak pointer to FrameAttachedToGraph. + /// \brief Give a scoped FrameAttachedToGraph to be used for resolving + /// attached bodies. This is private and is intended to be called by + /// Model::Load or World::Load. + /// \param[in] _graph scoped FrameAttachedToGraph object. private: void SetFrameAttachedToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by - /// Model::Load or World::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by Model::Load or + /// World::Load. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Model::Load and World::Load to call SetPoseRelativeToGraph. friend class Model; diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 407d863c3..3be388a57 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -37,6 +37,7 @@ namespace sdf class JointPrivate; struct FrameAttachedToGraph; struct PoseRelativeToGraph; + template class ScopedGraph; /// \enum JointType /// \brief The set of joint types. INVALID indicates that joint type has @@ -221,19 +222,18 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give a weak pointer to the FrameAttachedToGraph to be used - /// for resolving parent and child link names. This is private and is - /// intended to be called by Model::Load. - /// \param[in] _graph Weak pointer to FrameAttachedToGraph. + /// \brief Give the scoped FrameAttachedToGraph to be used for resolving + /// parent and child link names. This is private and is intended to be + /// called by Model::Load. + /// \param[in] _graph scoped FrameAttachedToGraph object. private: void SetFrameAttachedToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by - /// Model::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by Model::Load. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Model::Load to call SetPoseRelativeToGraph. friend class Model; diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index b9576312e..71ae8278c 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -35,6 +35,7 @@ namespace sdf // Forward declare private data class. class JointAxisPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; /// \brief Parameters related to the axis of rotation for rotational joints, /// and the axis of translation for prismatic joints. @@ -269,12 +270,12 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called - /// by Joint::SetPoseRelativeToGraph. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by + /// Joint::SetPoseRelativeToGraph. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Joint::SetPoseRelativeToGraph to propagate. friend class Joint; diff --git a/include/sdf/Light.hh b/include/sdf/Light.hh index c34f33979..41b875287 100644 --- a/include/sdf/Light.hh +++ b/include/sdf/Light.hh @@ -37,6 +37,7 @@ namespace sdf // Forward declare private data class. class LightPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; /// \enum LightType /// \brief The set of light types. INVALID indicates that light type has @@ -273,12 +274,12 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph or World::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph or World::Load to call /// SetXmlParentName and SetPoseRelativeToGraph, diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 7b8532ac5..87feaf48f 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -40,6 +40,7 @@ namespace sdf class Visual; class LinkPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; class SDFORMAT_VISIBLE Link { @@ -225,12 +226,11 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by - /// Model::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by Model::Load. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Model::Load to call SetPoseRelativeToGraph. friend class Model; diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 30b2353b6..b581aa9f2 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -39,6 +39,8 @@ namespace sdf class Link; class ModelPrivate; struct PoseRelativeToGraph; + struct FrameAttachedToGraph; + template class ScopedGraph; class SDFORMAT_VISIBLE Model { @@ -132,11 +134,14 @@ namespace sdf /// should be subject to wind. public: void SetEnableWind(bool _enableWind); - /// \brief Get the number of links. + /// \brief Get the number of links that are immediate (not nested) children + /// of this Model object. + /// \remark LinkByName() can find links that are not immediate children of + /// this Model object. /// \return Number of links contained in this Model object. public: uint64_t LinkCount() const; - /// \brief Get a link based on an index. + /// \brief Get an immediate (not nested) child link based on an index. /// \param[in] _index Index of the link. The index should be in the /// range [0..LinkCount()). /// \return Pointer to the link. Nullptr if the index does not exist. @@ -157,11 +162,14 @@ namespace sdf /// \return True if there exists a link with the given name. public: bool LinkNameExists(const std::string &_name) const; - /// \brief Get the number of joints. + /// \brief Get the number of joints that are immediate (not nested) children + /// of this Model object. + /// \remark JointByName() can find joints that are not immediate children of + /// this Model object. /// \return Number of joints contained in this Model object. public: uint64_t JointCount() const; - /// \brief Get a joint based on an index. + /// \brief Get an immediate (not nested) child joint based on an index. /// \param[in] _index Index of the joint. The index should be in the /// range [0..JointCount()). /// \return Pointer to the joint. Nullptr if the index does not exist. @@ -170,7 +178,7 @@ namespace sdf /// \brief Get whether a joint name exists. /// \param[in] _name Name of the joint to check. - /// To check for a joint in a nested model, prefix the link name with + /// To check for a joint in a nested model, prefix the joint name with /// the sequence of nested models containing this joint, delimited by "::". /// \return True if there exists a joint with the given name. public: bool JointNameExists(const std::string &_name) const; @@ -184,11 +192,15 @@ namespace sdf /// \sa bool JointNameExists(const std::string &_name) const public: const Joint *JointByName(const std::string &_name) const; - /// \brief Get the number of explicit frames. + /// \brief Get the number of explicit frames that are immediate (not nested) + /// children of this Model object. + /// \remark FrameByName() can find explicit frames that are not immediate + /// children of this Model object. /// \return Number of explicit frames contained in this Model object. public: uint64_t FrameCount() const; - /// \brief Get an explicit frame based on an index. + /// \brief Get an immediate (not nested) child explicit frame based on an + /// index. /// \param[in] _index Index of the explicit frame. The index should be in /// the range [0..FrameCount()). /// \return Pointer to the explicit frame. Nullptr if the index does not @@ -211,11 +223,15 @@ 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. + /// \brief Get the number of nested models that are immediate (not + /// recursively nested) children of this Model object. + /// \remark ModelByName() can find nested models that are not immediate + /// children of this Model object. /// \return Number of nested models contained in this Model object. public: uint64_t ModelCount() const; - /// \brief Get a nested model based on an index. + /// \brief Get an immediate (not recursively nested) child 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. @@ -298,13 +314,21 @@ namespace sdf /// \param[in] _name Name of the placement frame. public: void SetPlacementFrameName(const std::string &_name); - /// \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 and Model::Load if this is a nested model. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. - /// \return Error if graph pointer is invalid. - private: sdf::Errors SetPoseRelativeToGraph( - std::weak_ptr _graph); + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by Root::Load or + /// World::SetPoseRelativeToGraph if this is a standalone model and + /// Model::SetPoseRelativeToGraph if this is a nested model. + /// \param[in] _graph scoped PoseRelativeToGraph object. + private: void SetPoseRelativeToGraph( + sdf::ScopedGraph _graph); + + /// \brief Give the scoped FrameAttachedToGraph to be used for resolving + /// attached bodies. This is private and is intended to be called by + /// Root::Load or World::SetFrameAttachedToGraph if this is a standalone + /// model and Model::SetFrameAttachedToGraph if this is a nested model. + /// \param[in] _graph scoped FrameAttachedToGraph object. + private: void SetFrameAttachedToGraph( + sdf::ScopedGraph _graph); /// \brief Get the model's canonical link and the nested name of the link /// relative to the current model, delimited by "::". @@ -313,7 +337,10 @@ namespace sdf private: std::pair CanonicalLinkAndRelativeName() const; - /// \brief Allow World::Load to call SetPoseRelativeToGraph. + /// \brief Allow Root::Load, World::SetPoseRelativeToGraph, or + /// World::SetFrameAttachedToGraph to call SetPoseRelativeToGraph and + /// SetFrameAttachedToGraph + friend class Root; friend class World; /// \brief Allow helper function in FrameSemantics.cc to call diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index ae88cd037..a37f44ea1 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -56,6 +56,23 @@ namespace sdf /// \brief Default constructor public: Root(); + /// \brief Copy constructor is explicitly deleted to avoid copying the + /// FrameAttachedToGraph and PoseRelativeToGraphs contained in Root. + public: Root(const Root &_root) = delete; + + /// \brief Move constructor + /// \param[in] _root Root to move. + public: Root(Root &&_root) noexcept; + + /// \brief Move assignment operator. + /// \param[in] _root Root to move. + /// \return Reference to this. + public: Root &operator=(Root &&_root) noexcept; + + /// \brief Copy assignment operator is explicitly deleted to avoid copying + /// the FrameAttachedToGraph and PoseRelativeToGraphs contained in Root. + public: Root &operator=(const Root &_root) = delete; + /// \brief Destructor public: ~Root(); @@ -107,7 +124,8 @@ namespace sdf /// \return True if there exists a world with the given name. public: bool WorldNameExists(const std::string &_name) const; - /// \brief Get the number of models. + /// \brief Get the number of models that are immediate (not nested) children + /// of this Root object. /// \return Number of models contained in this Root object. public: uint64_t ModelCount() const; diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index 6baa76035..0a263180e 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -42,6 +42,7 @@ namespace sdf // Forward declare private data class. class SemanticPosePrivate; struct PoseRelativeToGraph; + template class ScopedGraph; /// \brief SemanticPose is a data structure that can be used by different /// DOM objects to resolve poses on a PoseRelativeToGraph. This object holds @@ -78,12 +79,30 @@ namespace sdf /// raw pose is applied. /// \param[in] _defaultResolveTo Default frame to resolve-to in Resolve() /// if no frame is specified. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph A scoped PoseRelativeToGraph object. private: SemanticPose( const ignition::math::Pose3d &_pose, const std::string &_relativeTo, const std::string &_defaultResolveTo, - std::weak_ptr _graph); + const sdf::ScopedGraph &_graph); + + /// \brief Private constructor that is used by object that represent a frame + /// in the PoseRelativeTo graph. Examples are Model, Frame, Link and not + /// Collision or Visual. + /// \param[in] _name Name of object. This should also be the name of the + /// frame represented by this object in the PoseRelativeTo graph. + /// \param[in] _pose Raw pose of object. + /// \param[in] _relativeTo Name of frame in graph relative-to which the + /// raw pose is applied. + /// \param[in] _defaultResolveTo Default frame to resolve-to in Resolve() + /// if no frame is specified. + /// \param[in] _graph A scoped PoseRelativeToGraph object. + private: SemanticPose( + const std::string &_name, + const ignition::math::Pose3d &_pose, + const std::string &_relativeTo, + const std::string &_defaultResolveTo, + const sdf::ScopedGraph &_graph); /// \brief Destructor public: ~SemanticPose(); diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 33a07fa16..b5b92bd7d 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -23,6 +23,7 @@ #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" +#include "sdf/sdf_config.h" #include "sdf/system_util.hh" namespace sdf @@ -40,6 +41,7 @@ namespace sdf class Magnetometer; class SensorPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; /// \enum SensorType /// \brief The set of sensor types. @@ -315,12 +317,12 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph to call SetXmlParentName /// and SetPoseRelativeToGraph, but Link::SetPoseRelativeToGraph is diff --git a/include/sdf/Types.hh b/include/sdf/Types.hh index 607f7712f..493fcb9f9 100644 --- a/include/sdf/Types.hh +++ b/include/sdf/Types.hh @@ -94,6 +94,13 @@ namespace sdf /// \brief A vector of Error. using Errors = std::vector; + /// \brief Output operator for a collection of errors. + /// \param[in,out] _out The output stream. + /// \param[in] _err The errors to output. + /// \return Reference to the given output stream + SDFORMAT_VISIBLE std::ostream &operator<<( + std::ostream &_out, const sdf::Errors &_errs); + /// \brief Defines a color class SDFORMAT_VISIBLE Color { diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index deff10053..316562dd0 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -41,6 +41,7 @@ namespace sdf class VisualPrivate; class Geometry; struct PoseRelativeToGraph; + template class ScopedGraph; class SDFORMAT_VISIBLE Visual { @@ -168,12 +169,12 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph to call SetXmlParentName /// and SetPoseRelativeToGraph, but Link::SetPoseRelativeToGraph is diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 2f513eaaa..e6eca06f5 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -41,6 +41,9 @@ namespace sdf class Model; class Physics; class WorldPrivate; + struct PoseRelativeToGraph; + struct FrameAttachedToGraph; + template class ScopedGraph; class SDFORMAT_VISIBLE World { @@ -136,11 +139,12 @@ namespace sdf /// \sa SphericalCoordinates public: void SetMagneticField(const ignition::math::Vector3d &_mag); - /// \brief Get the number of models. + /// \brief Get the number of models that are immediate (not nested) children + /// of this World object. /// \return Number of models contained in this World object. public: uint64_t ModelCount() const; - /// \brief Get a model based on an index. + /// \brief Get an immediate (not nested) child model based on an index. /// \param[in] _index Index of the model. The index should be in the /// range [0..ModelCount()). /// \return Pointer to the model. Nullptr if the index does not exist. @@ -173,11 +177,13 @@ namespace sdf /// \return True if there exists an actor with the given name. public: bool ActorNameExists(const std::string &_name) const; - /// \brief Get the number of explicit frames. + /// \brief Get the number of explicit frames that are immediate (not nested) + /// children of this World object. /// \return Number of explicit frames contained in this World object. public: uint64_t FrameCount() const; - /// \brief Get an explicit frame based on an index. + /// \brief Get an immediate (not nested) child explicit frame based on an + /// index. /// \param[in] _index Index of the explicit frame. The index should be in /// the range [0..FrameCount()). /// \return Pointer to the explicit frame. Nullptr if the index does not @@ -269,6 +275,24 @@ namespace sdf /// \return True if there exists a physics profile with the given name. public: bool PhysicsNameExists(const std::string &_name) const; + /// \brief Give the Scoped PoseRelativeToGraph to be passed on to child + /// entities for resolving poses. This is private and is intended to be + /// called by Root::Load. + /// \param[in] _graph Scoped PoseRelativeToGraph object. + private: void SetPoseRelativeToGraph( + sdf::ScopedGraph _graph); + + /// \brief Give the Scoped FrameAttachedToGraph to be passed on to child + /// entities for attached bodes. This is private and is intended to be + /// called by Root::Load. + /// \param[in] _graph Scoped FrameAttachedToGraph object. + private: void SetFrameAttachedToGraph( + sdf::ScopedGraph _graph); + + /// \brief Allow Root::Load to call SetPoseRelativeToGraph and + /// SetFrameAttachedToGraph + friend class Root; + /// \brief Private data pointer. private: WorldPrivate *dataPtr = nullptr; }; diff --git a/include/sdf/parser.hh b/include/sdf/parser.hh index d17bdcdb8..5e33a8ef7 100644 --- a/include/sdf/parser.hh +++ b/include/sdf/parser.hh @@ -185,31 +185,6 @@ namespace sdf SDFORMAT_VISIBLE std::string getModelFilePath(const std::string &_modelDirPath); - /// \brief Copy the contents of the first model element from one ElementPtr - /// to another ElementPtr, prepending the copied model name with `::` to - /// link and joint names, and apply the model pose to the copied link poses. - /// If //xyz/@expressed_in == "__model__" for the axes of any copied joints, - /// then apply the model pose rotation to those joint axes. - /// \param[in] _sdf ElementPtr for model into which the elements will be - /// copied. - /// \param[in] _includeSDF The first model element from this ElementPtr will - /// be copied to _sdf with the mentioned name and pose transformations. - SDFORMAT_VISIBLE - void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF); - - /// \brief Copy the contents of the first model element from one ElementPtr - /// to another ElementPtr, prepending the copied model name with `::` to - /// link and joint names, and apply the model pose to the copied link poses. - /// If //xyz/@expressed_in == "__model__" for the axes of any copied joints, - /// then apply the model pose rotation to those joint axes. - /// \param[in] _sdf ElementPtr for model into which the elements will be - /// copied. - /// \param[in] _includeSDF The first model element from this ElementPtr will - /// be copied to _sdf with the mentioned name and pose transformations. - /// \param[out] _errors Any errors will be appended to this variable. - SDFORMAT_VISIBLE - void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF, Errors &_errors); - /// \brief Convert an SDF file to a specific SDF version. /// \param[in] _filename Name of the SDF file to convert. /// \param[in] _version Version to convert _filename to. diff --git a/src/Collision.cc b/src/Collision.cc index ee98baca8..733b7cba9 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -22,6 +22,8 @@ #include "sdf/Geometry.hh" #include "sdf/Surface.hh" #include "sdf/Types.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -49,8 +51,8 @@ class sdf::CollisionPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model scope. + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -207,7 +209,7 @@ void Collision::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void Collision::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } diff --git a/src/Frame.cc b/src/Frame.cc index c6a5737df..5431a9132 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -20,6 +20,7 @@ #include "sdf/Error.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -41,14 +42,15 @@ class sdf::FramePrivate /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; - /// \brief Name of graph source. - std::string graphSourceName = ""; + /// \brief The context name of the scoped Pose Relative-To graph + std::string graphScopeContextName = ""; - /// \brief Weak pointer to model's or worlds's Frame Attached-To Graph. - public: std::weak_ptr frameAttachedToGraph; + /// \brief Scoped Frame Attached-To graph at the parent model or world scope. + public: sdf::ScopedGraph frameAttachedToGraph; - /// \brief Weak pointer to model's or world's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model or world scope. + /// TODO (addisu) Make const sdf::PoseRelativeToGraph + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -195,20 +197,20 @@ void Frame::SetPoseRelativeTo(const std::string &_frame) ///////////////////////////////////////////////// void Frame::SetFrameAttachedToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->frameAttachedToGraph = _graph; } ///////////////////////////////////////////////// void Frame::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; - auto graph = this->dataPtr->poseRelativeToGraph.lock(); + auto graph = this->dataPtr->poseRelativeToGraph; if (graph) { - this->dataPtr->graphSourceName = graph->sourceName; + this->dataPtr->graphScopeContextName = graph.ScopeContextName(); } } @@ -217,7 +219,7 @@ Errors Frame::ResolveAttachedToBody(std::string &_body) const { Errors errors; - auto graph = this->dataPtr->frameAttachedToGraph.lock(); + auto graph = this->dataPtr->frameAttachedToGraph; if (!graph) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -226,7 +228,7 @@ Errors Frame::ResolveAttachedToBody(std::string &_body) const } std::string body; - errors = resolveFrameAttachedToBody(body, *graph, this->dataPtr->name); + errors = resolveFrameAttachedToBody(body, graph, this->dataPtr->name); if (errors.empty()) { _body = body; @@ -243,9 +245,10 @@ sdf::SemanticPose Frame::SemanticPose() const relativeTo = this->dataPtr->attachedTo; } return sdf::SemanticPose( + this->dataPtr->name, this->dataPtr->pose, relativeTo, - this->dataPtr->graphSourceName, + this->dataPtr->graphScopeContextName, this->dataPtr->poseRelativeToGraph); } diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 8e795c9d7..136d36854 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include #include #include #include @@ -28,11 +29,23 @@ #include "sdf/World.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" namespace sdf { inline namespace SDF_VERSION_NAMESPACE { +// Helpful functions when debugging in gdb +void printGraph(const ScopedGraph &_graph) +{ + std::stringstream ss; + std::cout << _graph.Graph() << std::endl; +} +void printGraph(const ScopedGraph &_graph) +{ + std::cout << _graph.Graph() << std::endl; +} + // The following two functions were originally submitted to ign-math, // but were not accepted as they were not generic enough. // For now, they will be kept here. @@ -49,21 +62,19 @@ inline namespace SDF_VERSION_NAMESPACE { /// \return A source vertex paired with a vector of the edges leading the /// source to the starting vertex, or a NullVertex paired with an empty /// vector if a cycle or vertex with multiple incoming edges are detected. -template -std::pair &, - std::vector< ignition::math::graph::DirectedEdge > > -FindSourceVertex( - const ignition::math::graph::DirectedGraph &_graph, - const ignition::math::graph::VertexId _id, - Errors &_errors) +template +std::pair::Vertex &, + std::vector::Edge>> +FindSourceVertex(const ScopedGraph &_graph, + const ignition::math::graph::VertexId _id, Errors &_errors) { - using DirectedEdge = ignition::math::graph::DirectedEdge; - using Vertex = ignition::math::graph::Vertex; + using DirectedEdge = typename ScopedGraph::Edge; + using Vertex = typename ScopedGraph::Vertex; using VertexId = ignition::math::graph::VertexId; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; - std::reference_wrapper vertex(_graph.VertexFromId(_id)); + std::reference_wrapper vertex(_graph.Graph().VertexFromId(_id)); if (!vertex.get().Valid()) { _errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, @@ -72,10 +83,16 @@ FindSourceVertex( return PairType(Vertex::NullVertex, EdgesType()); } + if (_id == _graph.ScopeVertexId()) + { + // This is the source. + return PairType(vertex, EdgesType()); + } + std::set visited; visited.insert(vertex.get().Id()); - auto incidentsTo = _graph.IncidentsTo(vertex); + auto incidentsTo = _graph.Graph().IncidentsTo(vertex); while (!incidentsTo.empty()) { if (incidentsTo.size() != 1) @@ -86,7 +103,7 @@ FindSourceVertex( return PairType(Vertex::NullVertex, EdgesType()); } auto const &edge = incidentsTo.begin()->second; - vertex = _graph.VertexFromId(edge.get().Vertices().first); + vertex = _graph.Graph().VertexFromId(edge.get().Vertices().first); edges.push_back(edge); if (visited.count(vertex.get().Id())) { @@ -95,8 +112,18 @@ FindSourceVertex( vertex.get().Name() + "]."}); return PairType(Vertex::NullVertex, EdgesType()); } + if (vertex.get().Id() == _graph.ScopeVertexId()) + { + // This is the source. + break; + } visited.insert(vertex.get().Id()); - incidentsTo = _graph.IncidentsTo(vertex); + incidentsTo = _graph.Graph().IncidentsTo(vertex); + } + if (vertex.get().Id() != _graph.ScopeVertexId()) + { + // Error, the root vertex is not the same as the the source + return PairType(Vertex::NullVertex, EdgesType()); } return PairType(vertex, edges); @@ -113,21 +140,21 @@ FindSourceVertex( /// \return A sink vertex paired with a vector of the edges leading the /// sink to the starting vertex, or a NullVertex paired with an empty /// vector if a cycle or vertex with multiple outgoing edges are detected. -template -std::pair &, - std::vector< ignition::math::graph::DirectedEdge > > +template +std::pair::Vertex &, + std::vector::Edge>> FindSinkVertex( - const ignition::math::graph::DirectedGraph &_graph, + const ScopedGraph &_graph, const ignition::math::graph::VertexId _id, Errors &_errors) { - using DirectedEdge = ignition::math::graph::DirectedEdge; - using Vertex = ignition::math::graph::Vertex; + using DirectedEdge = typename ScopedGraph::Edge; + using Vertex = typename ScopedGraph::Vertex; using VertexId = ignition::math::graph::VertexId; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; - std::reference_wrapper vertex(_graph.VertexFromId(_id)); + std::reference_wrapper vertex(_graph.Graph().VertexFromId(_id)); if (!vertex.get().Valid()) { _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, @@ -139,7 +166,7 @@ FindSinkVertex( std::set visited; visited.insert(vertex.get().Id()); - auto incidentsFrom = _graph.IncidentsFrom(vertex); + auto incidentsFrom = _graph.Graph().IncidentsFrom(vertex); while (!incidentsFrom.empty()) { if (incidentsFrom.size() != 1) @@ -150,7 +177,7 @@ FindSinkVertex( return PairType(Vertex::NullVertex, EdgesType()); } auto const &edge = incidentsFrom.begin()->second; - vertex = _graph.VertexFromId(edge.get().Vertices().second); + vertex = _graph.Graph().VertexFromId(edge.get().Vertices().second); edges.push_back(edge); if (visited.count(vertex.get().Id())) { @@ -160,7 +187,7 @@ FindSinkVertex( return PairType(Vertex::NullVertex, EdgesType()); } visited.insert(vertex.get().Id()); - incidentsFrom = _graph.IncidentsFrom(vertex); + incidentsFrom = _graph.Graph().IncidentsFrom(vertex); } return PairType(vertex, edges); @@ -178,18 +205,79 @@ std::pair } ///////////////////////////////////////////////// -Errors buildFrameAttachedToGraph( - FrameAttachedToGraph &_out, const Model *_model) +/// \brief Resolve the pose of a model taking into account the placement frame +/// attribute. This function is used to calculate the pose of the edge between a +/// model and its parent. +/// +/// Since this function calls sdf::resolvePoseRelativeToRoot to initially +/// resolve the pose of the placement frame, the passed in graph must already +/// have an edge between the input model and its parent frame. This edge will +/// later be updated with the pose calculated by this function. +/// +/// Notation used in this function: +/// Pf - The placement frame inside the model +/// R - The `relative_to` frame of the placement frame's //pose element. +/// M - The input model's implicit frame (__model__) +/// X_RPf - The pose of frame Pf relative to frame R +/// X_RM - The pose of the frame M relative to frame R +/// X_MPf - The pose of the frame Pf relative to frame M +/// +/// Example: +/// +/// +/// +/// +/// {X_RPf} +/// +/// +/// +/// +/// +/// When the placement_frame is specified in an SDFormat file, //model/pose +/// becomes the pose of the placement frame (X_RPf) instead of the pose of the +/// model frame (X_RM). However, when the model is inserted into a pose graph of +/// the parent scope, only the pose (X_RM) of the __model__ frame can be used. +/// i.e, X_RPf cannot be represented in the PoseRelativeTo graph. +/// Thus, the X_RM has to be calculated such that X_RPf = X_RM * X_MPf. +// +/// \param[in] _model Input model whose pose to resolve relative to its parent +/// \param[in] _graph The PoseRelativeTo graph with the same scope as the +/// frame referenced by the placement frame attribute of the input model. i.e, +/// the input model's graph. +/// \param[out] _resolvedPose The resolved pose (X_RM). If an error was +/// encountered during `sdf::resolvePoseRelativeToRoot`, _resolvedPose will not +/// be modified. +static Errors resolveModelPoseWithPlacementFrame(const sdf::Model &_model, + const sdf::ScopedGraph &_graph, + ignition::math::Pose3d &_resolvedPose) { - Errors errors; + sdf::Errors errors; + + // Alias for better pose notation + ignition::math::Pose3d &X_RM = _resolvedPose; + const ignition::math::Pose3d &X_RPf = _model.RawPose(); + + // If the model has a placement frame, calculate the necessary pose to use + if (!_model.PlacementFrameName().empty()) + { + ignition::math::Pose3d X_MPf; + + errors = sdf::resolvePoseRelativeToRoot( + X_MPf, _graph, _model.PlacementFrameName()); + if (errors.empty()) + { + X_RM = X_RPf * X_MPf.Inverse(); + } + } - // add implicit model frame vertex first - const std::string scopeName = "__model__"; - _out.scopeName = scopeName; - auto modelFrameId = - _out.graph.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); - _out.map[scopeName] = modelFrameId; + return errors; +} +///////////////////////////////////////////////// +Errors buildFrameAttachedToGraph( + ScopedGraph &_out, const Model *_model, bool _root) +{ + Errors errors; if (!_model) { @@ -203,52 +291,52 @@ Errors buildFrameAttachedToGraph( "Invalid model element in sdf::Model."}); return errors; } - else if (_model->LinkCount() < 1 && _model->ModelCount() < 1) + else if (_model->LinkCount() == 0 && _model->ModelCount() == 0 && + !_model->Static()) { errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, "A model must have at least one link."}); return errors; } - // identify canonical link, which may be nested - auto canonicalLinkAndName = modelCanonicalLinkAndRelativeName(_model); - const sdf::Link *canonicalLink = canonicalLinkAndName.first; - const std::string canonicalLinkName = canonicalLinkAndName.second; - if (nullptr == canonicalLink) - { - if (canonicalLinkName.empty()) - { - errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, - "A model must have at least one link."}); - } - else - { - errors.push_back({ErrorCode::MODEL_CANONICAL_LINK_INVALID, - "canonical_link with name[" + canonicalLinkName + - "] not found in model with name[" + _model->Name() + "]."}); - } - // return early - return errors; - } + auto frameType = + _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; + + const std::string scopeContextName = "__model__"; - // Identify if the canonical link is in a nested model. - if (_model->LinkByName(canonicalLink->Name()) != canonicalLink) + auto rootId = ignition::math::graph::kNullId; + if (_root) { - // The canonical link is nested, so its vertex should be added - // here with an edge from __model__. - // The nested canonical link name should be a nested name - // relative to _model, delimited by "::". - auto linkId = - _out.graph.AddVertex(canonicalLinkName, sdf::FrameType::LINK).Id(); - _out.map[canonicalLinkName] = linkId; - _out.graph.AddEdge({modelFrameId, linkId}, true); + // The __root__ vertex identifies the scope that contains a root level + // model. In the PoseRelativeTo graph, this vertex is connected to the model + // with an edge that holds the value of //model/pose. However, in the + // FrameAttachedTo graph, the vertex is disconnected because nothing is + // attached to it. Since only links and static models are allowed to be + // disconnected in this graph, the STATIC_MODEL was chosen as its frame + // type. A different frame type could potentially be used, but that adds + // more complexity to the validateFrameAttachedToGraph code. + _out = _out.AddScopeVertex( + "", "__root__", scopeContextName, sdf::FrameType::STATIC_MODEL); + rootId = _out.ScopeVertexId(); } + const auto modelId = _out.AddVertex(_model->Name(), frameType).Id(); + + auto outModel = _out.AddScopeVertex( + _model->Name(), scopeContextName, scopeContextName, frameType); + const auto modelFrameId = outModel.ScopeVertexId(); + + // Add an aliasing edge from the model vertex to the + // corresponding vertex in the FrameAttachedTo graph of the child model, + auto &edge = outModel.AddEdge({modelId, modelFrameId}, true); + // Set the edge weight to 0 to indicate that this is an aliasing edge. + edge.SetWeight(0); + // add link vertices for (uint64_t l = 0; l < _model->LinkCount(); ++l) { auto link = _model->LinkByIndex(l); - if (_out.map.count(link->Name()) > 0) + if (outModel.Count(link->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Link with non-unique name [" + link->Name() + @@ -256,22 +344,14 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto linkId = - _out.graph.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); - _out.map[link->Name()] = linkId; - - // add edge from implicit model frame vertex to canonical link - if (link == canonicalLink) - { - _out.graph.AddEdge({modelFrameId, linkId}, true); - } + outModel.AddVertex(link->Name(), sdf::FrameType::LINK); } // add joint vertices for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); - if (_out.map.count(joint->Name()) > 0) + if (outModel.Count(joint->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Joint with non-unique name [" + joint->Name() + @@ -279,16 +359,14 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto jointId = - _out.graph.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); - _out.map[joint->Name()] = jointId; + outModel.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); } // add frame vertices for (uint64_t f = 0; f < _model->FrameCount(); ++f) { auto frame = _model->FrameByIndex(f); - if (_out.map.count(frame->Name()) > 0) + if (outModel.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -296,16 +374,14 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto frameId = - _out.graph.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - _out.map[frame->Name()] = frameId; + outModel.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); } // 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) + if (outModel.Count(nestedModel->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Nested model with non-unique name [" + nestedModel->Name() + @@ -313,18 +389,17 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto nestedModelId = - _out.graph.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); - _out.map[nestedModel->Name()] = nestedModelId; + auto nestedErrors = buildFrameAttachedToGraph(outModel, nestedModel, false); + errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); } // add edges from joint to child frames for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); - auto jointId = _out.map.at(joint->Name()); + auto jointId = outModel.VertexIdByName(joint->Name()); auto childFrameName = joint->ChildLinkName(); - if (_out.map.count(childFrameName) != 1) + if (outModel.Count(childFrameName) != 1) { errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, "Child frame with name[" + childFrameName + @@ -332,23 +407,23 @@ Errors buildFrameAttachedToGraph( "] not found in model with name[" + _model->Name() + "]."}); continue; } - auto childFrameId = _out.map.at(childFrameName); - _out.graph.AddEdge({jointId, childFrameId}, true); + auto childFrameId = outModel.VertexIdByName(childFrameName); + outModel.AddEdge({jointId, childFrameId}, true); } // add frame edges for (uint64_t f = 0; f < _model->FrameCount(); ++f) { auto frame = _model->FrameByIndex(f); - auto frameId = _out.map.at(frame->Name()); + auto frameId = outModel.VertexIdByName(frame->Name()); // look for vertex in graph that matches attached_to value std::string attachedTo = frame->AttachedTo(); if (attachedTo.empty()) { - // if the attached-to name is empty, use the scope name - attachedTo = scopeName; + // if the attached-to name is empty, use the scope context name + attachedTo = scopeContextName; } - if (_out.map.count(attachedTo) != 1) + if (outModel.Count(attachedTo) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "attached_to name[" + attachedTo + @@ -357,7 +432,7 @@ Errors buildFrameAttachedToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto attachedToId = _out.map[attachedTo]; + auto attachedToId = outModel.VertexIdByName(attachedTo); bool edgeData = true; if (frame->Name() == frame->AttachedTo()) { @@ -369,7 +444,49 @@ Errors buildFrameAttachedToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({frameId, attachedToId}, edgeData); + outModel.AddEdge({frameId, attachedToId}, edgeData); + } + + // identify canonical link, which may be nested + const auto[canonicalLink, canonicalLinkName] = + modelCanonicalLinkAndRelativeName(_model); + if (!_model->Static()) + { + if (nullptr == canonicalLink) + { + if (canonicalLinkName.empty()) + { + if (_model->ModelCount() == 0) + { + errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, + "A model must have at least one link."}); + } + else + { + // The canonical link was not found, but the model could have a + // descendant that has a static model, so simply create an edge to the + // first model and let the attached_to frame resolution take care of + // finding the canonical link + auto firstChildModelId = + outModel.VertexIdByName(_model->ModelByIndex(0)->Name()); + outModel.AddEdge({modelFrameId, firstChildModelId }, true); + } + } + else + { + errors.push_back({ErrorCode::MODEL_CANONICAL_LINK_INVALID, + "canonical_link with name[" + canonicalLinkName + + "] not found in model with name[" + _model->Name() + "]."}); + } + // return early + return errors; + } + else + { + // Add an edge from the implicit model frame to the canonical link found. + auto linkId = outModel.VertexIdByName(canonicalLinkName); + outModel.AddEdge({modelFrameId, linkId}, true); + } } return errors; @@ -377,18 +494,10 @@ Errors buildFrameAttachedToGraph( ///////////////////////////////////////////////// Errors buildFrameAttachedToGraph( - FrameAttachedToGraph &_out, const World *_world) + ScopedGraph &_out, const World *_world) { Errors errors; - // add implicit world frame vertex first - const std::string scopeName = "world"; - _out.scopeName = scopeName; - auto worldFrameId = - _out.graph.AddVertex(scopeName, sdf::FrameType::WORLD).Id(); - _out.map[scopeName] = worldFrameId; - - if (!_world) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -402,11 +511,16 @@ Errors buildFrameAttachedToGraph( return errors; } + // add implicit world frame vertex first + const std::string scopeContextName = "world"; + _out = _out.AddScopeVertex( + "", scopeContextName, scopeContextName, sdf::FrameType::WORLD); + // add model vertices for (uint64_t m = 0; m < _world->ModelCount(); ++m) { auto model = _world->ModelByIndex(m); - if (_out.map.count(model->Name()) > 0) + if (_out.Count(model->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Model with non-unique name [" + model->Name() + @@ -414,16 +528,15 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto modelId = - _out.graph.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); - _out.map[model->Name()] = modelId; + auto modelErrors = buildFrameAttachedToGraph(_out, model, false); + errors.insert(errors.end(), modelErrors.begin(), modelErrors.end()); } // add frame vertices for (uint64_t f = 0; f < _world->FrameCount(); ++f) { auto frame = _world->FrameByIndex(f); - if (_out.map.count(frame->Name()) > 0) + if (_out.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -431,31 +544,29 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto frameId = - _out.graph.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - _out.map[frame->Name()] = frameId; + _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); } // add frame edges for (uint64_t f = 0; f < _world->FrameCount(); ++f) { auto frame = _world->FrameByIndex(f); - auto frameId = _out.map.at(frame->Name()); + auto frameId = _out.VertexIdByName(frame->Name()); // look for vertex in graph that matches attached_to value std::string attachedTo = frame->AttachedTo(); if (attachedTo.empty()) { - // if the attached-to name is empty, use the scope name - attachedTo = scopeName; - if (_out.map.count(scopeName) != 1) + // if the attached-to name is empty, use the scope context name + attachedTo = scopeContextName; + if (_out.Count(scopeContextName) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + - scopeName + "] not found in map."}); + scopeContextName + "] not found in map."}); continue; } } - if (_out.map.count(attachedTo) != 1) + if (_out.Count(attachedTo) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "attached_to name[" + attachedTo + @@ -464,7 +575,7 @@ Errors buildFrameAttachedToGraph( "in world with name[" + _world->Name() + "]."}); continue; } - auto attachedToId = _out.map[attachedTo]; + auto attachedToId = _out.VertexIdByName(attachedTo); bool edgeData = true; if (frame->Name() == frame->AttachedTo()) { @@ -476,7 +587,7 @@ Errors buildFrameAttachedToGraph( "], causing a graph cycle " "in world with name[" + _world->Name() + "]."}); } - _out.graph.AddEdge({frameId, attachedToId}, edgeData); + _out.AddEdge({frameId, attachedToId}, edgeData); } return errors; @@ -484,7 +595,7 @@ Errors buildFrameAttachedToGraph( ///////////////////////////////////////////////// Errors buildPoseRelativeToGraph( - PoseRelativeToGraph &_out, const Model *_model) + ScopedGraph &_out, const Model *_model, bool _root) { Errors errors; @@ -501,18 +612,32 @@ Errors buildPoseRelativeToGraph( return errors; } - // add implicit model frame vertex first - const std::string sourceName = "__model__"; - _out.sourceName = sourceName; - auto modelFrameId = - _out.graph.AddVertex(sourceName, sdf::FrameType::MODEL).Id(); - _out.map[sourceName] = modelFrameId; + const std::string scopeContextName = "__model__"; + auto rootId = ignition::math::graph::kNullId; + // add the model frame vertex first + if (_root) + { + _out = _out.AddScopeVertex( + "", "__root__", scopeContextName, sdf::FrameType::MODEL); + rootId = _out.ScopeVertexId(); + } + auto modelId = _out.AddVertex(_model->Name(), sdf::FrameType::MODEL).Id(); + auto outModel = _out.AddScopeVertex(_model->Name(), scopeContextName, + scopeContextName, sdf::FrameType::MODEL); + auto modelFrameId = outModel.ScopeVertexId(); + + // Add an aliasing edge from the model vertex to the + // corresponding vertex in the PoseRelativeTo graph of the child model, + // i.e, to the ::__model__ vertex. + auto &edge = _out.AddEdge({modelId, modelFrameId}, {}); + // Set the edge weight to 0 to indicate that this is an aliasing edge. + edge.SetWeight(0); // add link vertices and default edge if relative_to is empty for (uint64_t l = 0; l < _model->LinkCount(); ++l) { auto link = _model->LinkByIndex(l); - if (_out.map.count(link->Name()) > 0) + if (outModel.Count(link->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Link with non-unique name [" + link->Name() + @@ -521,13 +646,12 @@ Errors buildPoseRelativeToGraph( continue; } auto linkId = - _out.graph.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); - _out.map[link->Name()] = linkId; + outModel.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); if (link->PoseRelativeTo().empty()) { // relative_to is empty, so add edge from implicit model frame to link - _out.graph.AddEdge({modelFrameId, linkId}, link->RawPose()); + outModel.AddEdge({modelFrameId, linkId}, link->RawPose()); } } @@ -535,7 +659,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); - if (_out.map.count(joint->Name()) > 0) + if (outModel.Count(joint->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Joint with non-unique name [" + joint->Name() + @@ -543,9 +667,7 @@ Errors buildPoseRelativeToGraph( "]."}); continue; } - auto jointId = - _out.graph.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); - _out.map[joint->Name()] = jointId; + outModel.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); } // add frame vertices and default edge if both @@ -553,7 +675,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t f = 0; f < _model->FrameCount(); ++f) { auto frame = _model->FrameByIndex(f); - if (_out.map.count(frame->Name()) > 0) + if (outModel.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -562,13 +684,12 @@ Errors buildPoseRelativeToGraph( continue; } auto frameId = - _out.graph.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - _out.map[frame->Name()] = frameId; + outModel.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); if (frame->PoseRelativeTo().empty() && frame->AttachedTo().empty()) { // add edge from implicit model frame to frame - _out.graph.AddEdge({modelFrameId, frameId}, frame->RawPose()); + outModel.AddEdge({modelFrameId, frameId}, frame->RawPose()); } } @@ -576,7 +697,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t m = 0; m < _model->ModelCount(); ++m) { auto nestedModel = _model->ModelByIndex(m); - if (_out.map.count(nestedModel->Name()) > 0) + if (outModel.Count(nestedModel->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Nested model with non-unique name [" + nestedModel->Name() + @@ -584,16 +705,9 @@ Errors buildPoseRelativeToGraph( "]."}); 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()); - } + auto nestedErrors = buildPoseRelativeToGraph(outModel, nestedModel, false); + errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); } // now that all vertices have been added to the graph, @@ -604,16 +718,16 @@ Errors buildPoseRelativeToGraph( auto link = _model->LinkByIndex(l); // check if we've already added a default edge - const std::string relativeTo = link->PoseRelativeTo(); + const std::string &relativeTo = link->PoseRelativeTo(); if (relativeTo.empty()) { continue; } - auto linkId = _out.map.at(link->Name()); + auto linkId = outModel.VertexIdByName(link->Name()); // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (outModel.Count(relativeTo) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + @@ -622,7 +736,7 @@ Errors buildPoseRelativeToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = outModel.VertexIdByName(relativeTo); if (link->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -631,7 +745,7 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, linkId}, link->RawPose()); + outModel.AddEdge({relativeToId, linkId}, link->RawPose()); } for (uint64_t j = 0; j < _model->JointCount(); ++j) @@ -645,10 +759,10 @@ Errors buildPoseRelativeToGraph( relativeTo = joint->ChildLinkName(); } - auto jointId = _out.map.at(joint->Name()); + auto jointId = outModel.VertexIdByName(joint->Name()); // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (outModel.Count(relativeTo) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + @@ -657,7 +771,7 @@ Errors buildPoseRelativeToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = outModel.VertexIdByName(relativeTo); if (joint->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -666,7 +780,7 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, jointId}, joint->RawPose()); + outModel.AddEdge({relativeToId, jointId}, joint->RawPose()); } for (uint64_t f = 0; f < _model->FrameCount(); ++f) @@ -679,7 +793,7 @@ Errors buildPoseRelativeToGraph( continue; } - auto frameId = _out.map.at(frame->Name()); + auto frameId = outModel.VertexIdByName(frame->Name()); std::string relativeTo; std::string typeForErrorMsg; ErrorCode errorCode; @@ -697,7 +811,7 @@ Errors buildPoseRelativeToGraph( } // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (outModel.Count(relativeTo) != 1) { errors.push_back({errorCode, typeForErrorMsg + " name[" + relativeTo + @@ -706,7 +820,7 @@ Errors buildPoseRelativeToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = outModel.VertexIdByName(relativeTo); if (frame->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -715,50 +829,71 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, frameId}, frame->RawPose()); + outModel.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()) + auto nestedModelId = outModel.VertexIdByName(nestedModel->Name()); + // if relative_to is empty, add edge from implicit model frame to + // nestedModel + auto relativeToId = modelFrameId; + + const std::string &relativeTo = nestedModel->PoseRelativeTo(); + if (!relativeTo.empty()) { - continue; + // look for vertex in graph that matches relative_to value + if (outModel.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; + } + + relativeToId = outModel.VertexIdByName(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() + "]."}); + } } - auto nestedModelId = _out.map.at(nestedModel->Name()); + ignition::math::Pose3d resolvedModelPose = nestedModel->RawPose(); + sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame(*nestedModel, + outModel.ChildModelScope(nestedModel->Name()), resolvedModelPose); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - // 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()); + outModel.AddEdge({relativeToId, nestedModelId}, resolvedModelPose); } + if (_root) + { + // We have to add this edge now with an identity pose to be able to call + // resolveModelPoseWithPlacementFrame, which in turn calls + // sdf::resolvePoseRelativeToRoot. We will later update the edge after the + // pose is calculated. + auto rootToModel = outModel.AddEdge({rootId, modelId}, {}); + ignition::math::Pose3d resolvedModelPose = _model->RawPose(); + sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame( + *_model, outModel, resolvedModelPose); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + + outModel.UpdateEdge(rootToModel, resolvedModelPose); + } return errors; } ///////////////////////////////////////////////// Errors buildPoseRelativeToGraph( - PoseRelativeToGraph &_out, const World *_world) + ScopedGraph &_out, const World *_world) { Errors errors; @@ -774,19 +909,23 @@ Errors buildPoseRelativeToGraph( "Invalid world element in sdf::World."}); return errors; } - // add implicit world frame vertex first - const std::string sourceName = "world"; - _out.sourceName = sourceName; - auto worldFrameId = - _out.graph.AddVertex(sourceName, sdf::FrameType::WORLD).Id(); - _out.map[sourceName] = worldFrameId; + const std::string scopeContextName = "world"; + + _out = _out.AddScopeVertex( + "", "__root__", scopeContextName, sdf::FrameType::WORLD); + auto rootId = _out.ScopeVertexId(); + + _out = _out.AddScopeVertex( + "", scopeContextName, scopeContextName, sdf::FrameType::WORLD); + auto worldFrameId = _out.ScopeVertexId(); + _out.AddEdge({rootId, worldFrameId}, {}); // add model vertices and default edge if relative_to is empty for (uint64_t m = 0; m < _world->ModelCount(); ++m) { auto model = _world->ModelByIndex(m); - if (_out.map.count(model->Name()) > 0) + if (_out.Count(model->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Model with non-unique name [" + model->Name() + @@ -794,15 +933,9 @@ Errors buildPoseRelativeToGraph( "]."}); continue; } - auto modelId = - _out.graph.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); - _out.map[model->Name()] = modelId; - if (model->PoseRelativeTo().empty()) - { - // relative_to is empty, so add edge from implicit world frame to model - _out.graph.AddEdge({worldFrameId, modelId}, model->RawPose()); - } + auto modelErrors = buildPoseRelativeToGraph(_out , model, false); + errors.insert(errors.end(), modelErrors .begin(), modelErrors .end()); } // add frame vertices and default edge if both @@ -810,7 +943,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t f = 0; f < _world->FrameCount(); ++f) { auto frame = _world->FrameByIndex(f); - if (_out.map.count(frame->Name()) > 0) + if (_out.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -819,13 +952,12 @@ Errors buildPoseRelativeToGraph( continue; } auto frameId = - _out.graph.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - _out.map[frame->Name()] = frameId; + _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); if (frame->PoseRelativeTo().empty() && frame->AttachedTo().empty()) { // add edge from implicit world frame to frame - _out.graph.AddEdge({worldFrameId, frameId}, frame->RawPose()); + _out.AddEdge({worldFrameId, frameId}, frame->RawPose()); } } @@ -836,35 +968,43 @@ Errors buildPoseRelativeToGraph( { auto model = _world->ModelByIndex(m); + auto modelId = _out.VertexIdByName(model->Name()); + // if relative_to is empty, add edge from implicit model frame to + // world + auto relativeToId = worldFrameId; + // check if we've already added a default edge - const std::string relativeTo = model->PoseRelativeTo(); - if (relativeTo.empty()) + const std::string &relativeTo = model->PoseRelativeTo(); + if (!relativeTo.empty()) { - continue; + // look for vertex in graph that matches relative_to value + if (_out.Count(relativeTo) != 1) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, + "relative_to name[" + relativeTo + + "] specified by model with name[" + model->Name() + + "] does not match a model or frame name " + "in world with name[" + _world->Name() + "]."}); + continue; + } + + relativeToId = _out.VertexIdByName(relativeTo); + if (model->Name() == relativeTo) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, + "relative_to name[" + relativeTo + + "] is identical to model name[" + model->Name() + + "], causing a graph cycle " + "in world with name[" + _world->Name() + "]."}); + } } - auto modelId = _out.map.at(model->Name()); + ignition::math::Pose3d resolvedModelPose = model->RawPose(); + sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame(*model, + _out.ChildModelScope(model->Name()), resolvedModelPose); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - // 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 model with name[" + model->Name() + - "] does not match a model or frame name " - "in world with name[" + _world->Name() + "]."}); - continue; - } - auto relativeToId = _out.map[relativeTo]; - if (model->Name() == relativeTo) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to model name[" + model->Name() + - "], causing a graph cycle " - "in world with name[" + _world->Name() + "]."}); - } - _out.graph.AddEdge({relativeToId, modelId}, model->RawPose()); + _out.AddEdge({relativeToId, modelId}, resolvedModelPose); } for (uint64_t f = 0; f < _world->FrameCount(); ++f) @@ -877,7 +1017,7 @@ Errors buildPoseRelativeToGraph( continue; } - auto frameId = _out.map.at(frame->Name()); + auto frameId = _out.VertexIdByName(frame->Name()); std::string relativeTo; std::string typeForErrorMsg; ErrorCode errorCode; @@ -895,7 +1035,7 @@ Errors buildPoseRelativeToGraph( } // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (_out.Count(relativeTo) != 1) { errors.push_back({errorCode, typeForErrorMsg + " name[" + relativeTo + @@ -904,7 +1044,7 @@ Errors buildPoseRelativeToGraph( "in world with name[" + _world->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = _out.VertexIdByName(relativeTo); if (frame->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -913,54 +1053,51 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in world with name[" + _world->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, frameId}, frame->RawPose()); + _out.AddEdge({relativeToId, frameId}, frame->RawPose()); } return errors; } ///////////////////////////////////////////////// -Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) +Errors validateFrameAttachedToGraph( + const ScopedGraph &_in) { Errors errors; - // Expect scopeName to be either "__model__" or "world" - if (_in.scopeName != "__model__" && _in.scopeName != "world") + // Expect ScopeContextName to be either "__model__" or "world" + if (_in.ScopeContextName() != "__model__" && + _in.ScopeContextName() != "world") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error: scope frame[" + _in.scopeName + "] " - " does not match __model__ or world."}); + "FrameAttachedToGraph error: scope context name[" + + _in.ScopeContextName() + "] does not match __model__ or world."}); return errors; } - // Expect one vertex with name "__model__" and FrameType MODEL - // or with name "world" and FrameType WORLD - auto scopeVertices = _in.graph.Vertices(_in.scopeName); - if (scopeVertices.empty()) + // Expect the scope vertex to be valid and check that the scope context + // matches the frame type. + auto scopeVertex = _in.ScopeVertex(); + if (!scopeVertex.Valid()) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + - _in.scopeName + "] not found in graph."}); - return errors; - } - else if (scopeVertices.size() > 1) - { - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error, " - "more than one vertex with scope name " + _in.scopeName}); + _in.ScopeContextName() + "] not found in graph."}); return errors; } - auto scopeVertex = scopeVertices.begin()->second.get(); sdf::FrameType scopeFrameType = scopeVertex.Data(); - if (_in.scopeName == "__model__" && scopeFrameType != sdf::FrameType::MODEL) + if (_in.ScopeContextName() == "__model__" && + scopeFrameType != sdf::FrameType::MODEL && + scopeFrameType != sdf::FrameType::STATIC_MODEL) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "scope vertex with name __model__ should have FrameType MODEL."}); + "scope vertex with name __model__ should have FrameType MODEL or " + "STATIC_MODEL."}); return errors; } - else if (_in.scopeName == "world" && + else if (_in.ScopeContextName() == "world" && scopeFrameType != sdf::FrameType::WORLD) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, @@ -970,33 +1107,54 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) } // Check number of outgoing edges for each vertex - auto vertices = _in.graph.Vertices(); + auto vertices = _in.Graph().Vertices(); for (auto vertexPair : vertices) { + const std::string vertexName = _in.VertexLocalName(vertexPair.second.get()); // Vertex names should not be empty - if (vertexPair.second.get().Name().empty()) + if (vertexName.empty()) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "vertex with empty name detected."}); } + auto outDegree = _in.Graph().OutDegree(vertexPair.first); - auto outDegree = _in.graph.OutDegree(vertexPair.first); if (outDegree > 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "too many outgoing edges at a vertex with name [" + - vertexPair.second.get().Name() + "]."}); + vertexName + "]."}); + } + else if (vertexName == "__root__" ) + { + if (sdf::FrameType::STATIC_MODEL != scopeFrameType && + sdf::FrameType::WORLD != scopeFrameType) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error," + " __root__ should have FrameType STATIC_MODEL or WORLD."}); + } + else if (outDegree != 0) + { + std::string graphType = + scopeFrameType == sdf::FrameType::WORLD ? "WORLD" : "MODEL"; + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error," + " __root__ should have no outgoing edges in " + + graphType + " attached_to graph."}); + } } - else if (sdf::FrameType::MODEL == scopeFrameType) + else if (sdf::FrameType::MODEL == scopeFrameType || + sdf::FrameType::STATIC_MODEL == scopeFrameType) { switch (vertexPair.second.get().Data()) { case sdf::FrameType::WORLD: errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "vertex with name [" + vertexPair.second.get().Name() + "]" + + "vertex with name [" + vertexName + "]" + "should not have type WORLD in MODEL attached_to graph."}); break; case sdf::FrameType::LINK: @@ -1005,34 +1163,39 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "LINK vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] should have no outgoing edges " "in MODEL attached_to graph."}); } break; - case sdf::FrameType::MODEL: - if ("__model__" != vertexPair.second.get().Name()) + case sdf::FrameType::STATIC_MODEL: + if (outDegree != 0) { - if (outDegree != 0) + auto edges = _in.Graph().IncidentsFrom(vertexPair.first); + // Filter out alias edges (edge with a weight of 0) + auto outDegreeNoAliases = std::count_if( + edges.begin(), edges.end(), [](const auto &_edge) + { + return _edge.second.get().Weight() >= 1.0; + }); + if (outDegreeNoAliases) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "nested MODEL vertex with name [" + - vertexPair.second.get().Name() + + "STATIC_MODEL vertex with name [" + + vertexName + "] should have no outgoing edges " "in MODEL attached_to graph."}); } - break; } - // fall through to default case for __model__ - [[fallthrough]]; + break; default: if (outDegree == 0) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "Non-LINK vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] is disconnected; it should have 1 outgoing edge " + "in MODEL attached_to graph."}); } @@ -1041,7 +1204,7 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "Non-LINK vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] has " + std::to_string(outDegree) + " outgoing edges; it should only have 1 " "outgoing edge in MODEL attached_to graph."}); @@ -1054,29 +1217,57 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) // scopeFrameType must be sdf::FrameType::WORLD switch (vertexPair.second.get().Data()) { - case sdf::FrameType::JOINT: - case sdf::FrameType::LINK: - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error, " - "No JOINT or LINK vertex should be in WORLD attached_to graph."}); - break; - case sdf::FrameType::MODEL: case sdf::FrameType::WORLD: if (outDegree != 0) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "MODEL and WORLD vertices should have no outgoing edges " + "WORLD vertices should have no outgoing edges " "in WORLD attached_to graph."}); } break; + case sdf::FrameType::LINK: + if (outDegree != 0) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error, " + "LINK vertex with name [" + + vertexName + + "] should have no outgoing edges " + "in WORLD attached_to graph."}); + } + break; + case sdf::FrameType::STATIC_MODEL: + if (outDegree != 0) + { + auto edges = _in.Graph().IncidentsFrom(vertexPair.first); + // Filter out alias edges (edge with a weight of 0) + auto outDegreeNoAliases = std::count_if( + edges.begin(), edges.end(), [](const auto &_edge) + { + return _edge.second.get().Weight() >= 1.0; + }); + if (outDegreeNoAliases) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error, " + "STATIC_MODEL vertex with name [" + + vertexName + + "] should have no outgoing edges " + "in WORLD attached_to graph."}); + } + } + break; default: if (outDegree != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "FRAME vertices in WORLD attached_to graph should have " - "1 outgoing edge."}); + "Non-LINK vertex with name [" + + vertexName + + "] has " + std::to_string(outDegree) + + " outgoing edges; it should only have 1 " + "outgoing edge in WORLD attached_to graph."}); } break; } @@ -1084,10 +1275,10 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) } // check graph for cycles by finding sink from each vertex - for (auto const &namePair : _in.map) + for (auto const &name : _in.VertexNames()) { std::string resolvedBody; - Errors e = resolveFrameAttachedToBody(resolvedBody, _in, namePair.first); + Errors e = resolveFrameAttachedToBody(resolvedBody, _in, name); errors.insert(errors.end(), e.begin(), e.end()); } @@ -1095,47 +1286,42 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) } ///////////////////////////////////////////////// -Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) +Errors validatePoseRelativeToGraph( + const ScopedGraph &_in) { Errors errors; - // Expect sourceName to be either "__model__" or "world" - if (_in.sourceName != "__model__" && _in.sourceName != "world") + // Expect scopeContextName to be either "__model__" or "world" + if (_in.ScopeContextName() != "__model__" && + _in.ScopeContextName() != "world") { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error: source vertex name " + _in.sourceName + - " does not match __model__ or world."}); + "PoseRelativeToGraph error: scope context name " + + _in.ScopeContextName() + " does not match __model__ or world."}); return errors; } - // Expect one vertex with name "__model__" and FrameType MODEL - // or with name "world" and FrameType WORLD - auto sourceVertices = _in.graph.Vertices(_in.sourceName); - if (sourceVertices.empty()) + // Expect the scope vertex (which is the source vertex for this graph) to be + // valid and check that the scope context matches the frame type. + auto sourceVertex = _in.ScopeVertex(); + if (!sourceVertex.Valid()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error: source frame[" + - _in.sourceName + "] not found in graph."}); - return errors; - } - else if (sourceVertices.size() > 1) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error, " - "more than one vertex with source name " + _in.sourceName}); + _in.ScopeContextName() + "] not found in graph."}); return errors; } - auto sourceVertex = sourceVertices.begin()->second.get(); sdf::FrameType sourceFrameType = sourceVertex.Data(); - if (_in.sourceName == "__model__" && sourceFrameType != sdf::FrameType::MODEL) + if (_in.ScopeContextName() == "__model__" && + sourceFrameType != sdf::FrameType::MODEL) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "source vertex with name __model__ should have FrameType MODEL."}); return errors; } - else if (_in.sourceName == "world" && + else if (_in.ScopeContextName() == "world" && sourceFrameType != sdf::FrameType::WORLD) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, @@ -1145,24 +1331,46 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) } // Check number of incoming edges for each vertex - auto vertices = _in.graph.Vertices(); + auto vertices = _in.Graph().Vertices(); for (auto vertexPair : vertices) { + const std::string vertexName = _in.VertexLocalName(vertexPair.second.get()); // Vertex names should not be empty - if (vertexPair.second.get().Name().empty()) + if (vertexName.empty()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "vertex with empty name detected."}); } - auto inDegree = _in.graph.InDegree(vertexPair.first); + std::size_t inDegree = _in.Graph().InDegree(vertexPair.first); + if (inDegree > 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "too many incoming edges at a vertex with name [" + - vertexPair.second.get().Name() + "]."}); + vertexName + "]."}); + } + else if (vertexName == "__root__" ) + { + if (sdf::FrameType::MODEL != sourceFrameType && + sdf::FrameType::STATIC_MODEL != sourceFrameType && + sdf::FrameType::WORLD != sourceFrameType) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, + "PoseRelativeToGraph error," + " __root__ should have FrameType MODEL, STATIC_MODEL or WORLD."}); + } + else if (inDegree != 0) + { + std::string graphType = + sourceFrameType == sdf::FrameType::WORLD ? "WORLD" : "MODEL"; + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, + "PoseRelativeToGraph error," + " __root__ vertex should have no incoming edges in " + + graphType + " relative_to graph."}); + } } else if (sdf::FrameType::MODEL == sourceFrameType) { @@ -1171,11 +1379,11 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) case sdf::FrameType::WORLD: errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " - "vertex with name [" + vertexPair.second.get().Name() + "]" + + "vertex with name [" + vertexName + "]" + "should not have type WORLD in MODEL relative_to graph."}); break; case sdf::FrameType::MODEL: - if ("__model__" == vertexPair.second.get().Name()) + if (_in.ScopeVertexId() == vertexPair.first) { if (inDegree != 0) { @@ -1195,7 +1403,7 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "Vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] is disconnected; it should have 1 incoming edge " + "in MODEL relative_to graph."}); } @@ -1204,7 +1412,7 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "Non-MODEL vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] has " + std::to_string(inDegree) + " incoming edges; it should only have 1 " "incoming edge in MODEL relative_to graph."}); @@ -1217,18 +1425,12 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) // sourceFrameType must be sdf::FrameType::WORLD switch (vertexPair.second.get().Data()) { - case sdf::FrameType::JOINT: - case sdf::FrameType::LINK: - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error, " - "No JOINT or LINK vertex should be in WORLD relative_to graph."}); - break; case sdf::FrameType::WORLD: - if (inDegree != 0) + if (inDegree != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " - "WORLD vertices should have no incoming edges " + "WORLD vertices should have 1 incoming edge " "in WORLD relative_to graph."}); } break; @@ -1238,7 +1440,7 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "MODEL / FRAME vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] is disconnected; it should have 1 incoming edge " + "in WORLD relative_to graph."}); } @@ -1247,7 +1449,7 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "MODEL / FRAME vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] has " + std::to_string(inDegree) + " incoming edges; it should only have 1 " "incoming edge in WORLD relative_to graph."}); @@ -1258,10 +1460,12 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) } // check graph for cycles by resolving pose of each vertex relative to root - for (auto const &namePair : _in.map) + for (auto const &name : _in.VertexNames()) { + if (name == "__root__") + continue; ignition::math::Pose3d pose; - Errors e = resolvePoseRelativeToRoot(pose, _in, namePair.first); + Errors e = resolvePoseRelativeToRoot(pose, _in, name); errors.insert(errors.end(), e.begin(), e.end()); } @@ -1271,29 +1475,30 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) ///////////////////////////////////////////////// Errors resolveFrameAttachedToBody( std::string &_attachedToBody, - const FrameAttachedToGraph &_in, + const ScopedGraph &_in, const std::string &_vertexName) { Errors errors; - if (_in.scopeName != "__model__" && _in.scopeName != "world") + if (_in.ScopeContextName() != "__model__" && + _in.ScopeContextName() != "world") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error: scope frame[" + _in.scopeName + "] " - " does not match __model__ or world."}); + "FrameAttachedToGraph error: scope context name[" + + _in.ScopeContextName() + "] does not match __model__ or world."}); return errors; } - if (_in.map.count(_vertexName) != 1) + if (_in.Count(_vertexName) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "FrameAttachedToGraph unable to find unique frame with name [" + _vertexName + "] in graph."}); return errors; } - auto vertexId = _in.map.at(_vertexName); + auto vertexId = _in.VertexIdByName(_vertexName); - auto sinkVertexEdges = FindSinkVertex(_in.graph, vertexId, errors); + auto sinkVertexEdges = FindSinkVertex(_in, vertexId, errors); auto sinkVertex = sinkVertexEdges.first; if (!errors.empty()) @@ -1309,18 +1514,20 @@ Errors resolveFrameAttachedToBody( return errors; } - if (_in.scopeName == "world" && + if (_in.ScopeContextName() == "world" && !(sinkVertex.Data() == FrameType::WORLD || - sinkVertex.Data() == FrameType::MODEL)) + sinkVertex.Data() == FrameType::STATIC_MODEL || + sinkVertex.Data() == FrameType::LINK)) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "Graph has world scope but sink vertex named [" + - sinkVertex.Name() + "] does not have FrameType WORLD or MODEL " - "when starting from vertex with name [" + _vertexName + "]."}); + "Graph has world scope but sink vertex named [" + sinkVertex.Name() + + "] does not have FrameType WORLD, LINK or STATIC_MODEL " + "when starting from vertex with name [" + + _vertexName + "]."}); return errors; } - if (_in.scopeName == "__model__") + if (_in.ScopeContextName() == "__model__") { if (sinkVertex.Data() == FrameType::MODEL && sinkVertex.Name() == "__model__") @@ -1332,17 +1539,17 @@ Errors resolveFrameAttachedToBody( return errors; } else if (sinkVertex.Data() != FrameType::LINK && - sinkVertex.Data() != FrameType::MODEL) + sinkVertex.Data() != FrameType::STATIC_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 " + sinkVertex.Name() + "] does not have FrameType LINK OR STATIC_MODEL " "when starting from vertex with name [" + _vertexName + "]."}); return errors; } } - _attachedToBody = sinkVertex.Name(); + _attachedToBody = _in.VertexLocalName(sinkVertex); return errors; } @@ -1350,21 +1557,31 @@ Errors resolveFrameAttachedToBody( ///////////////////////////////////////////////// Errors resolvePoseRelativeToRoot( ignition::math::Pose3d &_pose, - const PoseRelativeToGraph &_graph, + const ScopedGraph &_graph, const std::string &_vertexName) { Errors errors; - if (_graph.map.count(_vertexName) != 1) + if (_graph.Count(_vertexName) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "PoseRelativeToGraph unable to find unique frame with name [" + _vertexName + "] in graph."}); return errors; } - auto vertexId = _graph.map.at(_vertexName); - auto incomingVertexEdges = FindSourceVertex(_graph.graph, vertexId, errors); + return resolvePoseRelativeToRoot( + _pose, _graph, _graph.VertexIdByName(_vertexName)); +} +///////////////////////////////////////////////// +Errors resolvePoseRelativeToRoot( + ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, + const ignition::math::graph::VertexId &_vertexId) +{ + Errors errors; + + auto incomingVertexEdges = FindSourceVertex(_graph, _vertexId, errors); if (!errors.empty()) { @@ -1374,16 +1591,17 @@ Errors resolvePoseRelativeToRoot( { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph unable to find path to source vertex " - "when starting from vertex with name [" + _vertexName + "]."}); + "when starting from vertex with id [" + + std::to_string(_vertexId) + "]."}); return errors; } - else if (incomingVertexEdges.first.Name() != _graph.sourceName) + else if (incomingVertexEdges.first.Id() != _graph.ScopeVertex().Id()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph frame with name [" + _vertexName + "] " - "is disconnected; its source vertex has name [" + - incomingVertexEdges.first.Name() + - "], but its source name should be " + _graph.sourceName + "."}); + "PoseRelativeToGraph frame with name [" + std::to_string(_vertexId) + + "] is disconnected; its source vertex has name [" + + incomingVertexEdges.first.Name() + "], but its source name should " + "be " + _graph.ScopeContextName() + "."}); return errors; } @@ -1402,70 +1620,55 @@ Errors resolvePoseRelativeToRoot( } ///////////////////////////////////////////////// -Errors resolvePose( - ignition::math::Pose3d &_pose, - const PoseRelativeToGraph &_graph, - const std::string &_frameName, - const std::string &_resolveTo) +Errors resolvePose(ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, + const ignition::math::graph::VertexId &_frameVertexId, + const ignition::math::graph::VertexId &_resolveToVertexId) { - Errors errors = resolvePoseRelativeToRoot(_pose, _graph, _frameName); - - ignition::math::Pose3d poseR; - Errors errorsR = resolvePoseRelativeToRoot(poseR, _graph, _resolveTo); - errors.insert(errors.end(), errorsR.begin(), errorsR.end()); + Errors errors = resolvePoseRelativeToRoot(_pose, _graph, _frameVertexId); - if (errors.empty()) + // If the resolveTo is empty, we're resolving to the Root, so we're done + if (_resolveToVertexId != ignition::math::graph::kNullId) { - _pose = poseR.Inverse() * _pose; + ignition::math::Pose3d poseR; + Errors errorsR = + resolvePoseRelativeToRoot(poseR, _graph, _resolveToVertexId); + errors.insert(errors.end(), errorsR.begin(), errorsR.end()); + + if (errors.empty()) + { + _pose = poseR.Inverse() * _pose; + } } return errors; } ///////////////////////////////////////////////// -Errors updateGraphPose( - PoseRelativeToGraph &_graph, +Errors resolvePose( + ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, const std::string &_frameName, - const ignition::math::Pose3d &_pose) + const std::string &_resolveTo) { Errors errors; - - if (_graph.map.count(_frameName) != 1) + if (_graph.Count(_frameName) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "PoseRelativeToGraph unable to find unique frame with name [" + - _frameName+ "] in graph."}); + _frameName + "] in graph."}); return errors; } - auto vertexId = _graph.map.at(_frameName); - auto incidentsTo = _graph.graph.IncidentsTo(vertexId); - if (incidentsTo.size() == 1) + if (_graph.Count(_resolveTo) != 1) { - // There's no API to update the data of an edge, so we remove the edge and - // insert a new one with the new pose. - auto &edge = incidentsTo.begin()->second; - auto tailVertexId = edge.get().Tail(); - auto headVertexId = edge.get().Head(); - _graph.graph.RemoveEdge(edge.get().Id()); - _graph.graph.AddEdge({tailVertexId, headVertexId}, _pose); - } - else if (incidentsTo.empty()) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error: no incoming edge to " - "vertex [" + _frameName + "]."}); - } - else - { - // This is an error because there should be only one way to define the pose - // of a frame. This would be like a frame having two //pose elements with - // different //pose/@relative_to attributes. - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error: multiple incoming edges to " - "vertex [" + _frameName + "]."}); + errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, + "PoseRelativeToGraph unable to find unique frame with name [" + + _resolveTo + "] in graph."}); + return errors; } - return errors; + return resolvePose(_pose, _graph, _graph.VertexIdByName(_frameName), + _graph.VertexIdByName(_resolveTo)); } } } diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index 80ebf84ff..416580a10 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -18,6 +18,7 @@ #define SDF_FRAMESEMANTICS_HH_ #include +#include #include #include @@ -45,6 +46,7 @@ namespace sdf // Forward declaration. class Model; class World; + template class ScopedGraph; /// \enum FrameType /// \brief The set of frame types. INVALID indicates that frame type has @@ -65,6 +67,9 @@ namespace sdf /// \brief An explicit frame. FRAME = 4, + + /// \brief An implicit static model frame. + STATIC_MODEL = 5, }; /// \brief Data structure for frame attached_to graphs for Model or World. @@ -108,42 +113,44 @@ namespace sdf /// \param[out] _out Graph object to write. /// \param[in] _model Model from which to build attached_to graph. /// \return Errors. - Errors buildFrameAttachedToGraph( - FrameAttachedToGraph &_out, const Model *_model); + Errors buildFrameAttachedToGraph(ScopedGraph &_out, + const Model *_model, bool _root = true); /// \brief Build a FrameAttachedToGraph for a world. /// \param[out] _out Graph object to write. /// \param[in] _world World from which to build attached_to graph. /// \return Errors. Errors buildFrameAttachedToGraph( - FrameAttachedToGraph &_out, const World *_world); + ScopedGraph &_out, const World *_world); /// \brief Build a PoseRelativeToGraph for a model. /// \param[out] _out Graph object to write. /// \param[in] _model Model from which to build attached_to graph. /// \return Errors. - Errors buildPoseRelativeToGraph( - PoseRelativeToGraph &_out, const Model *_model); + Errors buildPoseRelativeToGraph(ScopedGraph &_out, + const Model *_model, bool _root = true); /// \brief Build a PoseRelativeToGraph for a world. /// \param[out] _out Graph object to write. /// \param[in] _world World from which to build attached_to graph. /// \return Errors. Errors buildPoseRelativeToGraph( - PoseRelativeToGraph &_out, const World *_world); + ScopedGraph &_out, const World *_world); /// \brief Confirm that FrameAttachedToGraph is valid by checking the number /// of outbound edges for each vertex and checking for graph cycles. /// \param[in] _in Graph object to validate. /// \return Errors. - Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in); + Errors validateFrameAttachedToGraph( + const ScopedGraph &_in); /// \brief Confirm that PoseRelativeToGraph is valid by checking the number /// of outbound edges for each vertex and checking for graph cycles. /// \param[in] _in Graph object to validate. /// \return Errors. - Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in); + Errors validatePoseRelativeToGraph( + const ScopedGraph &_in); /// \brief Resolve the attached-to body for a given frame. Following the /// edges of the frame attached-to graph from a given frame must lead @@ -157,7 +164,7 @@ namespace sdf /// a link or world frame. Errors resolveFrameAttachedToBody( std::string &_attachedToBody, - const FrameAttachedToGraph &_in, + const ScopedGraph &_in, const std::string &_vertexName); /// \brief Resolve pose of a vertex relative to its outgoing ancestor @@ -168,9 +175,14 @@ namespace sdf /// \return Errors. Errors resolvePoseRelativeToRoot( ignition::math::Pose3d &_pose, - const PoseRelativeToGraph &_graph, + const ScopedGraph &_graph, const std::string &_vertexName); + Errors resolvePoseRelativeToRoot( + ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, + const ignition::math::graph::VertexId &_vertexId); + /// \brief Resolve pose of a frame relative to named frame. /// \param[out] _pose Pose object to write. /// \param[in] _graph PoseRelativeToGraph to read from. @@ -180,21 +192,15 @@ namespace sdf /// \return Errors. Errors resolvePose( ignition::math::Pose3d &_pose, - const PoseRelativeToGraph &_graph, + const ScopedGraph &_graph, const std::string &_frameName, const std::string &_resolveTo); - /// \brief Update the pose of a frame in the pose graph. This updates the - /// content of the edge incident to the vertex identified by the given - /// _frameName. - /// \param[in] _graph PoseRelativeToGraph to update. - /// \param[in] _frameName Name of frame whose pose is to be updated. - /// \param[in] _pose New pose. - /// \return Errors. - Errors updateGraphPose( - PoseRelativeToGraph &_graph, - const std::string &_frameName, - const ignition::math::Pose3d &_pose); + Errors resolvePose( + ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, + const ignition::math::graph::VertexId &_frameVertexId, + const ignition::math::graph::VertexId &_resolveToVertexId); } } #endif diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index 262ee4c12..752033610 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -32,6 +32,7 @@ #include "sdf/sdf_config.h" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "test_config.h" ///////////////////////////////////////////////// @@ -43,29 +44,32 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_Model) // Load the SDF file sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; // Get the first model const sdf::Model *model = root.ModelByIndex(0); - sdf::FrameAttachedToGraph graph; - auto errors = sdf::buildFrameAttachedToGraph(graph, model); - EXPECT_TRUE(errors.empty()); + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + errors = sdf::buildFrameAttachedToGraph(graph, model); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_TRUE(sdf::validateFrameAttachedToGraph(graph).empty()); EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); - EXPECT_EQ(6u, graph.map.size()); - EXPECT_EQ(6u, graph.graph.Vertices().size()); - EXPECT_EQ(5u, graph.graph.Edges().size()); + graph = graph.ChildModelScope(model->Name()); + EXPECT_EQ(8u, graph.Map().size()); + EXPECT_EQ(8u, graph.Graph().Vertices().size()); + EXPECT_EQ(6u, graph.Graph().Edges().size()); - EXPECT_EQ(1u, graph.map.count("__model__")); - EXPECT_EQ(1u, graph.map.count("L")); - EXPECT_EQ(1u, graph.map.count("F00")); - EXPECT_EQ(1u, graph.map.count("F0")); - EXPECT_EQ(1u, graph.map.count("F1")); - EXPECT_EQ(1u, graph.map.count("F2")); - EXPECT_EQ(0u, graph.map.count("invalid")); + EXPECT_EQ(1u, graph.Count("__model__")); + EXPECT_EQ(1u, graph.Count("L")); + EXPECT_EQ(1u, graph.Count("F00")); + EXPECT_EQ(1u, graph.Count("F0")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(0u, graph.Count("invalid")); std::string resolvedBody; EXPECT_TRUE( @@ -108,29 +112,33 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) // Load the SDF file sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; // Get the first world const sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(world); - sdf::FrameAttachedToGraph graph; - auto errors = sdf::buildFrameAttachedToGraph(graph, world); + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + errors = sdf::buildFrameAttachedToGraph(graph, world); EXPECT_TRUE(errors.empty()); - EXPECT_TRUE(sdf::validateFrameAttachedToGraph(graph).empty()); + errors = sdf::validateFrameAttachedToGraph(graph); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); - EXPECT_EQ(6u, graph.map.size()); - EXPECT_EQ(6u, graph.graph.Vertices().size()); - EXPECT_EQ(4u, graph.graph.Edges().size()); + EXPECT_EQ(9u, graph.Map().size()); + EXPECT_EQ(9u, graph.Graph().Vertices().size()); + EXPECT_EQ(7u, graph.Graph().Edges().size()); - EXPECT_EQ(1u, graph.map.count("world")); - EXPECT_EQ(1u, graph.map.count("world_frame")); - EXPECT_EQ(1u, graph.map.count("F0")); - EXPECT_EQ(1u, graph.map.count("F1")); - EXPECT_EQ(1u, graph.map.count("F2")); - EXPECT_EQ(1u, graph.map.count("M1")); - EXPECT_EQ(0u, graph.map.count("invalid")); + EXPECT_EQ(1u, graph.Count("world")); + EXPECT_EQ(1u, graph.Count("world_frame")); + EXPECT_EQ(1u, graph.Count("F0")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(1u, graph.Count("M1")); + EXPECT_EQ(0u, graph.Count("invalid")); std::string resolvedBody; EXPECT_TRUE( @@ -148,10 +156,10 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) EXPECT_EQ("world", resolvedBody); EXPECT_TRUE( sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1").empty()); - EXPECT_EQ("M1", resolvedBody); + EXPECT_EQ("M1::L", resolvedBody); EXPECT_TRUE( sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F2").empty()); - EXPECT_EQ("M1", resolvedBody); + EXPECT_EQ("M1::L", resolvedBody); // Try to resolve invalid frame name errors = sdf::resolveFrameAttachedToBody(resolvedBody, graph, "invalid"); @@ -168,19 +176,22 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) ASSERT_NE(nullptr, world); const sdf::Model *model = world->ModelByIndex(0); - sdf::FrameAttachedToGraph modelGraph; + auto ownedModelGraph = std::make_shared(); + sdf::ScopedGraph modelGraph(ownedModelGraph); errors = sdf::buildFrameAttachedToGraph(modelGraph, model); EXPECT_TRUE(errors.empty()); EXPECT_TRUE(sdf::validateFrameAttachedToGraph(modelGraph).empty()); - EXPECT_EQ(3u, modelGraph.map.size()); - EXPECT_EQ(3u, modelGraph.graph.Vertices().size()); - EXPECT_EQ(2u, modelGraph.graph.Edges().size()); + modelGraph = modelGraph.ChildModelScope(model->Name()); + + EXPECT_EQ(5u, modelGraph.Map().size()); + EXPECT_EQ(5u, modelGraph.Graph().Vertices().size()); + EXPECT_EQ(3u, modelGraph.Graph().Edges().size()); - EXPECT_EQ(1u, modelGraph.map.count("L")); - EXPECT_EQ(1u, modelGraph.map.count("__model__")); - EXPECT_EQ(1u, modelGraph.map.count("F0")); - EXPECT_EQ(0u, modelGraph.map.count("invalid")); + EXPECT_EQ(1u, modelGraph.Count("L")); + EXPECT_EQ(1u, modelGraph.Count("__model__")); + EXPECT_EQ(1u, modelGraph.Count("F0")); + EXPECT_EQ(0u, modelGraph.Count("invalid")); EXPECT_TRUE( sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "L").empty()); @@ -208,23 +219,25 @@ TEST(FrameSemantics, buildPoseRelativeToGraph) // Get the first model const sdf::Model *model = root.ModelByIndex(0); - sdf::PoseRelativeToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildPoseRelativeToGraph(graph, model); EXPECT_TRUE(errors.empty()); EXPECT_TRUE(sdf::validatePoseRelativeToGraph(graph).empty()); + graph = graph.ChildModelScope(model->Name()); - EXPECT_EQ(8u, graph.map.size()); - EXPECT_EQ(8u, graph.graph.Vertices().size()); - EXPECT_EQ(7u, graph.graph.Edges().size()); + EXPECT_EQ(10u, graph.Map().size()); + EXPECT_EQ(10u, graph.Graph().Vertices().size()); + EXPECT_EQ(9u, graph.Graph().Edges().size()); - EXPECT_EQ(1u, graph.map.count("__model__")); - EXPECT_EQ(1u, graph.map.count("P")); - EXPECT_EQ(1u, graph.map.count("C")); - EXPECT_EQ(1u, graph.map.count("J")); - EXPECT_EQ(1u, graph.map.count("F1")); - EXPECT_EQ(1u, graph.map.count("F2")); - EXPECT_EQ(1u, graph.map.count("F3")); - EXPECT_EQ(1u, graph.map.count("F4")); + EXPECT_EQ(1u, graph.Count("__model__")); + EXPECT_EQ(1u, graph.Count("P")); + EXPECT_EQ(1u, graph.Count("C")); + EXPECT_EQ(1u, graph.Count("J")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(1u, graph.Count("F3")); + EXPECT_EQ(1u, graph.Count("F4")); // Test resolvePoseRelativeToRoot for each frame. ignition::math::Pose3d pose; @@ -288,84 +301,332 @@ TEST(FrameSemantics, buildPoseRelativeToGraph) } ///////////////////////////////////////////////// -TEST(FrameSemantics, updateGraphPose) +TEST(NestedFrameSemantics, buildFrameAttachedToGraph_Model) { - const std::string testFile = sdf::filesystem::append( - PROJECT_SOURCE_PATH, "test", "sdf", "model_link_relative_to.sdf"); + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_nested_frame_attached_to.sdf"); // Load the SDF file sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; // Get the first model const sdf::Model *model = root.ModelByIndex(0); - sdf::PoseRelativeToGraph graph; - { - sdf::Errors errors = sdf::buildPoseRelativeToGraph(graph, model); - EXPECT_TRUE(errors.empty()); - EXPECT_TRUE(sdf::validatePoseRelativeToGraph(graph).empty()); - } - - using ignition::math::Pose3d; - - EXPECT_EQ(Pose3d::Zero, model->RawPose()); - - { - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L1", "__model__").empty()); - EXPECT_EQ(Pose3d(1, 0, 0, 0, IGN_PI_2, 0), pose); - } - { - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L3", "__model__").empty()); - EXPECT_EQ(Pose3d(1, 0, -3, 0, IGN_PI_2, 0), pose); - } - // Try updating the pose of a frame that doesn't exist - { - sdf::Errors errors = sdf::updateGraphPose(graph, "nonexistent", {}); - ASSERT_FALSE(errors.empty()); - EXPECT_EQ(sdf::ErrorCode::POSE_RELATIVE_TO_INVALID, errors[0].Code()); - } - - // Update L1's pose in the graph. This will be relative to __model__ - Pose3d l1NewPose(0, 5, 0, 0, 0, 0); - EXPECT_TRUE(sdf::updateGraphPose(graph, "L1", l1NewPose).empty()); - - { - // L1 relative to __model__ is l1NewPose - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L1", "__model__").empty()); - EXPECT_EQ(l1NewPose, pose); - } - { - // Since L3's pose is given relative to L1, updating L1 affects L3 - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L3", "__model__").empty()); - EXPECT_EQ(l1NewPose * Pose3d(3, 0, 0, 0, 0, 0), pose); - } - - // Update L3's pose. This will be relative to L1 - Pose3d l3NewPose(0, 0, 2, 0, 0, 0); - EXPECT_TRUE(sdf::updateGraphPose(graph, "L3", l3NewPose).empty()); - - { - // L3 relative to L1 is l3NewPose - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L3", "L1").empty()); - EXPECT_EQ(l3NewPose, pose); - } - { - // L1's pose shouldn't have changed - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L1", "__model__").empty()); - EXPECT_EQ(l1NewPose, pose); - } - { - // The new pose of L3 relative to __model__ is affected by the new relative - // poses of L1 and L3 - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L3", "__model__").empty()); - EXPECT_EQ(l1NewPose * l3NewPose, pose); - } + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + errors = sdf::buildFrameAttachedToGraph(graph, model); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_TRUE(sdf::validateFrameAttachedToGraph(graph).empty()); + EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); + EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); + + graph = graph.ChildModelScope(model->Name()); + EXPECT_EQ(1u, graph.Count("__model__")); + EXPECT_EQ(1u, graph.Count("L")); + EXPECT_EQ(1u, graph.Count("M1")); + EXPECT_EQ(1u, graph.Count("M1::__model__")); + EXPECT_EQ(1u, graph.Count("M1::L")); + EXPECT_EQ(1u, graph.Count("M1::M2")); + EXPECT_EQ(1u, graph.Count("M1::M2::L")); + EXPECT_EQ(1u, graph.Count("M1::F")); + EXPECT_EQ(1u, graph.Count("F0")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(1u, graph.Count("F3")); + EXPECT_EQ(1u, graph.Count("F4")); + EXPECT_EQ(0u, graph.Count("invalid")); + + std::string resolvedBody; + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "__model__").empty()); + EXPECT_EQ("L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "L").empty()); + EXPECT_EQ("L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::__model__") + .empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::L").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2::L").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::F").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F0").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F1").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F2").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F3").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F4").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + + // Try to resolve invalid frame name + errors = sdf::resolveFrameAttachedToBody(resolvedBody, graph, "invalid"); + for (auto &e : errors) + std::cerr << e.Message() << std::endl; + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "FrameAttachedToGraph unable to find unique frame with name [" + "invalid] in graph.")); +} + +///////////////////////////////////////////////// +TEST(NestedFrameSemantics, buildFrameAttachedToGraph_World) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "world_nested_frame_attached_to.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + // Get the first world + const sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(world); + + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + errors = sdf::buildFrameAttachedToGraph(graph, world); + EXPECT_TRUE(errors.empty()); + errors = sdf::validateFrameAttachedToGraph(graph); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); + EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); + + EXPECT_EQ(1u, graph.Count("world")); + EXPECT_EQ(1u, graph.Count("world_frame")); + EXPECT_EQ(1u, graph.Count("M1")); + EXPECT_EQ(1u, graph.Count("M1::__model__")); + EXPECT_EQ(1u, graph.Count("M1::L")); + EXPECT_EQ(1u, graph.Count("M1::M2")); + EXPECT_EQ(1u, graph.Count("M1::M2::__model__")); + EXPECT_EQ(1u, graph.Count("M1::M2::L")); + EXPECT_EQ(1u, graph.Count("M1::F0")); + EXPECT_EQ(1u, graph.Count("F0")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(1u, graph.Count("F3")); + EXPECT_EQ(1u, graph.Count("F4")); + EXPECT_EQ(1u, graph.Count("F5")); + EXPECT_EQ(1u, graph.Count("F6")); + EXPECT_EQ(0u, graph.Count("invalid")); + + std::string resolvedBody; + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "world").empty()); + EXPECT_EQ("world", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody( + resolvedBody, graph, "world_frame").empty()); + EXPECT_EQ("world", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::__model__") + .empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::L").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2::__model__") + .empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2::L").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::F0").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F0").empty()); + EXPECT_EQ("world", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F1").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F2").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F3").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F4").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F5").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F6").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + + // Try to resolve invalid frame name + errors = sdf::resolveFrameAttachedToBody(resolvedBody, graph, "invalid"); + for (auto &e : errors) + std::cerr << e.Message() << std::endl; + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "FrameAttachedToGraph unable to find unique frame with name [" + "invalid] in graph.")); + + // Create graph from embedded model + ASSERT_NE(nullptr, world); + const sdf::Model *model = world->ModelByIndex(0); + + auto ownedModelGraph = std::make_shared(); + sdf::ScopedGraph modelGraph(ownedModelGraph); + errors = sdf::buildFrameAttachedToGraph(modelGraph, model); + EXPECT_TRUE(errors.empty()); + EXPECT_TRUE(sdf::validateFrameAttachedToGraph(modelGraph).empty()); + + modelGraph = modelGraph.ChildModelScope(model->Name()); + + EXPECT_EQ(1u, modelGraph.Count("__model__")); + EXPECT_EQ(1u, modelGraph.Count("L")); + EXPECT_EQ(1u, modelGraph.Count("M2")); + EXPECT_EQ(1u, modelGraph.Count("M2::__model__")); + EXPECT_EQ(1u, modelGraph.Count("M2::L")); + EXPECT_EQ(1u, modelGraph.Count("F0")); + EXPECT_EQ(0u, modelGraph.Count("invalid")); + // The following Count expectations are 0 because the frames are out of scope. + EXPECT_EQ(0u, modelGraph.Count("world")); + EXPECT_EQ(0u, modelGraph.Count("world_frame")); + EXPECT_EQ(0u, modelGraph.Count("F1")); + EXPECT_EQ(0u, modelGraph.Count("F2")); + EXPECT_EQ(0u, modelGraph.Count("F3")); + EXPECT_EQ(0u, modelGraph.Count("F4")); + EXPECT_EQ(0u, modelGraph.Count("F5")); + EXPECT_EQ(0u, modelGraph.Count("F6")); + + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody( + resolvedBody, modelGraph, "__model__").empty()); + EXPECT_EQ("L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "L").empty()); + EXPECT_EQ("L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "M2").empty()); + EXPECT_EQ("M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "M2::__model__") + .empty()); + EXPECT_EQ("M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "M2::L").empty()); + EXPECT_EQ("M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F0").empty()); + EXPECT_EQ("M2::L", resolvedBody); + + errors = sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "world"); + ASSERT_FALSE(errors.empty()); + + EXPECT_EQ( + "FrameAttachedToGraph unable to find unique frame with name " + "[world] in graph.", + errors[0].Message()); + + errors = + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "world_frame"); + ASSERT_FALSE(errors.empty()); + EXPECT_EQ( + "FrameAttachedToGraph unable to find unique frame with name " + "[world_frame] in graph.", + errors[0].Message()); + + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F1").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F2").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F3").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F4").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F5").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F6").empty()); +} + +///////////////////////////////////////////////// +TEST(NestedFrameSemantics, ModelWithoutLinksWithNestedStaticModel) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_nested_static_model.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + + auto ownedModelGraph = std::make_shared(); + sdf::ScopedGraph modelGraph(ownedModelGraph); + errors = sdf::buildFrameAttachedToGraph(modelGraph, model); + EXPECT_TRUE(errors.empty()) << errors; + errors = sdf::validateFrameAttachedToGraph(modelGraph); + EXPECT_TRUE(errors.empty()) << errors; + + std::string resolvedBody; + errors = sdf::resolveFrameAttachedToBody( + resolvedBody, modelGraph, "model_nested_static_model"); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ("model_nested_static_model::child_model::static_model::__model__", + resolvedBody); +} + +///////////////////////////////////////////////// +TEST(NestedFrameSemantics, InvalidAttachedToScope) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "world_frame_invalid_attached_to_scope.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + ASSERT_FALSE(errors.empty()); + const std::string expectedMsg = + "attached_to name[M2::M1::L1] specified by frame with name[F] " + "does not match a model or frame name in world with " + "name[world_frame_invalid_attached_to_scope]."; + EXPECT_EQ(expectedMsg, errors[0].Message()); + + EXPECT_FALSE(sdf::checkFrameAttachedToGraph(&root)); + EXPECT_FALSE(sdf::checkFrameAttachedToNames(&root)); } diff --git a/src/Joint.cc b/src/Joint.cc index 4a467cbfc..711dffb9d 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -25,6 +25,7 @@ #include "sdf/JointAxis.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -74,11 +75,11 @@ class sdf::JointPrivate /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; - /// \brief Weak pointer to model's Frame Attached-To Graph. - public: std::weak_ptr frameAttachedToGraph; + /// \brief Scoped Frame Attached-To graph at the parent model scope + public: sdf::ScopedGraph frameAttachedToGraph; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model scope. + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -182,6 +183,12 @@ Errors Joint::Load(ElementPtr _sdf) if (parentPair.second) { this->dataPtr->parentLinkName = parentPair.first; + if (!isValidFrameReference(this->dataPtr->parentLinkName)) + { + errors.push_back({ErrorCode::RESERVED_NAME, + "The supplied joint parent name [" + this->dataPtr->parentLinkName + + "] is not valid."}); + } } else { @@ -194,6 +201,12 @@ Errors Joint::Load(ElementPtr _sdf) if (childPair.second) { this->dataPtr->childLinkName = childPair.first; + if (!isValidFrameReference(this->dataPtr->childLinkName)) + { + errors.push_back({ErrorCode::RESERVED_NAME, + "The supplied joint child name [" + this->dataPtr->childLinkName + + "] is not valid."}); + } } else { @@ -361,14 +374,14 @@ void Joint::SetPoseRelativeTo(const std::string &_frame) ///////////////////////////////////////////////// void Joint::SetFrameAttachedToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->frameAttachedToGraph = _graph; } ///////////////////////////////////////////////// void Joint::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; @@ -387,7 +400,7 @@ Errors Joint::ResolveChildLink(std::string &_link) const { Errors errors; - auto graph = this->dataPtr->frameAttachedToGraph.lock(); + auto graph = this->dataPtr->frameAttachedToGraph; if (!graph) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -396,7 +409,7 @@ Errors Joint::ResolveChildLink(std::string &_link) const } std::string link; - errors = resolveFrameAttachedToBody(link, *graph, this->ChildLinkName()); + errors = resolveFrameAttachedToBody(link, graph, this->ChildLinkName()); if (errors.empty()) { _link = link; @@ -417,7 +430,7 @@ Errors Joint::ResolveParentLink(std::string &_link) const return errors; } - auto graph = this->dataPtr->frameAttachedToGraph.lock(); + auto graph = this->dataPtr->frameAttachedToGraph; if (!graph) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -426,7 +439,7 @@ Errors Joint::ResolveParentLink(std::string &_link) const } std::string link; - errors = resolveFrameAttachedToBody(link, *graph, this->ParentLinkName()); + errors = resolveFrameAttachedToBody(link, graph, this->ParentLinkName()); if (errors.empty()) { _link = link; @@ -438,6 +451,7 @@ Errors Joint::ResolveParentLink(std::string &_link) const sdf::SemanticPose Joint::SemanticPose() const { return sdf::SemanticPose( + this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, this->ChildLinkName(), diff --git a/src/JointAxis.cc b/src/JointAxis.cc index f9cd705f3..e7b410974 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -24,6 +24,7 @@ #include "sdf/Error.hh" #include "sdf/JointAxis.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" using namespace sdf; @@ -85,8 +86,8 @@ class sdf::JointAxisPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model scope. + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -377,7 +378,7 @@ void JointAxis::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void JointAxis::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } @@ -388,7 +389,7 @@ Errors JointAxis::ResolveXyz( const std::string &_resolveTo) const { Errors errors; - auto graph = this->dataPtr->poseRelativeToGraph.lock(); + auto graph = this->dataPtr->poseRelativeToGraph; if (!graph) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -417,7 +418,7 @@ Errors JointAxis::ResolveXyz( } ignition::math::Pose3d pose; - errors = resolvePose(pose, *graph, axisExpressedIn, resolveTo); + errors = resolvePose(pose, graph, axisExpressedIn, resolveTo); if (errors.empty()) { diff --git a/src/Light.cc b/src/Light.cc index 1c65922fd..98b8c453c 100644 --- a/src/Light.cc +++ b/src/Light.cc @@ -18,6 +18,8 @@ #include #include "sdf/Error.hh" #include "sdf/Light.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -43,8 +45,8 @@ class sdf::LightPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model or world scope. + public: sdf::ScopedGraph poseRelativeToGraph; /// \brief True if the light should cast shadows. public: bool castShadows = false; @@ -317,7 +319,7 @@ void Light::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void Light::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } diff --git a/src/Link.cc b/src/Link.cc index 9ec72fc60..cb8b58365 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -28,6 +28,9 @@ #include "sdf/Sensor.hh" #include "sdf/Types.hh" #include "sdf/Visual.hh" + +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -66,8 +69,8 @@ class sdf::LinkPrivate /// \brief True if this link should be subject to wind, false otherwise. public: bool enableWind = false; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model scope. + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -374,8 +377,7 @@ void Link::SetPoseRelativeTo(const std::string &_frame) } ///////////////////////////////////////////////// -void Link::SetPoseRelativeToGraph( - std::weak_ptr _graph) +void Link::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; @@ -406,6 +408,7 @@ void Link::SetPoseRelativeToGraph( sdf::SemanticPose Link::SemanticPose() const { return sdf::SemanticPose( + this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, "__model__", diff --git a/src/Model.cc b/src/Model.cc index 55b17fa37..d1e68f197 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -27,6 +27,7 @@ #include "sdf/Model.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -76,17 +77,14 @@ class sdf::ModelPrivate /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; - /// \brief Frame Attached-To Graph constructed during Load. - public: std::shared_ptr frameAttachedToGraph; + /// \brief Scoped Frame Attached-To graph at the parent model or world scope. + public: sdf::ScopedGraph frameAttachedToGraph; - /// \brief Pose Relative-To Graph constructed during Load. - public: std::shared_ptr poseGraph; - - /// \brief Pose Relative-To Graph in parent (world or __model__) scope. - public: std::weak_ptr parentPoseGraph; + /// \brief Scoped Pose Relative-To graph at the parent model or world scope. + public: sdf::ScopedGraph poseGraph; /// \brief Scope name of parent Pose Relative-To Graph (world or __model__). - public: std::string parentPoseGraphScopeName; + public: std::string poseGraphScopeVertexName; }; ///////////////////////////////////////////////// @@ -106,35 +104,6 @@ Model::~Model() Model::Model(const Model &_model) : dataPtr(new ModelPrivate(*_model.dataPtr)) { - if (_model.dataPtr->frameAttachedToGraph) - { - this->dataPtr->frameAttachedToGraph = - std::make_shared( - *_model.dataPtr->frameAttachedToGraph); - } - if (_model.dataPtr->poseGraph) - { - this->dataPtr->poseGraph = std::make_shared( - *_model.dataPtr->poseGraph); - } - for (auto &link : this->dataPtr->links) - { - link.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - for (auto &model : this->dataPtr->models) - { - model.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - for (auto &joint : this->dataPtr->joints) - { - joint.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - frame.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } } ///////////////////////////////////////////////// @@ -360,144 +329,6 @@ Errors Model::Load(ElementPtr _sdf) frameNames.insert(frameName); } - // Build the graphs. - - // Build the FrameAttachedToGraph if the model is not static. - // Re-enable this when the buildFrameAttachedToGraph implementation handles - // static models. - if (!this->Static()) - { - this->dataPtr->frameAttachedToGraph - = std::make_shared(); - Errors frameAttachedToGraphErrors = - buildFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph, this); - errors.insert(errors.end(), frameAttachedToGraphErrors.begin(), - frameAttachedToGraphErrors.end()); - Errors validateFrameAttachedGraphErrors = - validateFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph); - errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), - validateFrameAttachedGraphErrors.end()); - for (auto &joint : this->dataPtr->joints) - { - joint.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } - } - - // Build the PoseRelativeToGraph - this->dataPtr->poseGraph = std::make_shared(); - Errors poseGraphErrors = - buildPoseRelativeToGraph(*this->dataPtr->poseGraph, this); - errors.insert(errors.end(), poseGraphErrors.begin(), - poseGraphErrors.end()); - Errors validatePoseGraphErrors = - validatePoseRelativeToGraph(*this->dataPtr->poseGraph); - errors.insert(errors.end(), validatePoseGraphErrors.begin(), - validatePoseGraphErrors.end()); - for (auto &link : this->dataPtr->links) - { - 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); - } - for (auto &frame : this->dataPtr->frames) - { - frame.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - - // Update the model pose to account for the placement frame. - if (!this->dataPtr->placementFrameName.empty()) - { - ignition::math::Pose3d X_MPf; - - sdf::Errors resolveErrors = - sdf::resolvePose(X_MPf, *this->dataPtr->poseGraph, - this->dataPtr->placementFrameName, "__model__"); - if (resolveErrors.empty()) - { - // Before this update, i.e, as specified in the SDFormat, the model pose - // (X_RPf) is the pose of the placement frame (Pf) relative to a frame (R) - // in the parent scope of the model. However, when this model (M) is - // inserted into a pose graph of the parent scope, only the pose (X_RM) - // of the __model__ frame can be used. Thus, the model pose has to be - // updated to X_RM. - // - // Note that X_RPf is the raw pose specified in //model/pose before this - // update. - const auto &X_RPf = this->dataPtr->pose; - auto &X_RM = this->dataPtr->pose; - X_RM = X_RPf * X_MPf.Inverse(); - } - else - { - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - } - } - - // The placement_frame attributes of included child models are currently lost - // during parsing because the parser expands the included models into the - // parent model. As a temporary workardound to preserve this information, the - // parser injects ::__placement_frame__ for every included child - // model. This serves as an identifier that the model frame of the child model - // should be treated as if it has its placement_frame attribute set. - // This will not be necessary when included nested models work as directly - // nested models. See - // https://github.com/osrf/sdformat/issues/319#issuecomment-665214004 - // TODO (addisu) Remove placementFrameIdentifier once PR addressing - // https://github.com/osrf/sdformat/issues/284 lands - for (auto &frame : this->dataPtr->frames) - { - auto placementSubstrInd = frame.Name().rfind("::__placement_frame__"); - if (placementSubstrInd != std::string::npos) - { - const std::string childModelName = - frame.Name().substr(0, placementSubstrInd); - // Find the model frame associated with this placement frame - const Frame *childModelFrame = - this->FrameByName(childModelName + "::__model__"); - if (nullptr != childModelFrame) - { - // The RawPose of the child model frame is the desired pose of the - // placement frame relative to a frame (R) in the scope of the parent - // model. We don't need to resolve the pose because the relative_to - // frame remains unchanged after the pose update here. i.e, the updated - // pose of the child model frame will still be relative to R. - const auto &X_RPf = childModelFrame->RawPose(); - - ignition::math::Pose3d X_MPf; - sdf::Errors resolveErrors = - frame.SemanticPose().Resolve(X_MPf, childModelFrame->Name()); - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - - // We need to update childModelFrame's pose relative to the parent - // frame as well as the corresponding edge in the pose graph because - // just updating childModelFrame doesn't update the pose graph. - auto X_RM = X_RPf * X_MPf.Inverse(); - frame.SetRawPose(X_RM); - sdf::updateGraphPose( - *this->dataPtr->poseGraph, childModelFrame->Name(), X_RM); - } - else - { - errors.push_back({ErrorCode::MODEL_PLACEMENT_FRAME_INVALID, - "Found a __placement_frame__ for child model with name [" + - childModelName + "], but the model does not exist in the" + - " parent model with name [" + this->Name() + "]"}); - } - } - } return errors; } @@ -814,33 +645,63 @@ void Model::SetPoseRelativeTo(const std::string &_frame) } ///////////////////////////////////////////////// -Errors Model::SetPoseRelativeToGraph( - std::weak_ptr _graph) +void Model::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { - Errors errors; + this->dataPtr->poseGraph = _graph; + this->dataPtr->poseGraphScopeVertexName = + _graph.VertexLocalName(_graph.ScopeVertexId()); - auto graph = _graph.lock(); - if (!graph) + auto childPoseGraph = + this->dataPtr->poseGraph.ChildModelScope(this->Name()); + for (auto &model : this->dataPtr->models) { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "Tried to set PoseRelativeToGraph with invalid pointer."}); - return errors; + model.SetPoseRelativeToGraph(childPoseGraph); } + for (auto &link : this->dataPtr->links) + { + link.SetPoseRelativeToGraph(childPoseGraph); + } + for (auto &joint : this->dataPtr->joints) + { + joint.SetPoseRelativeToGraph(childPoseGraph); + } + for (auto &frame : this->dataPtr->frames) + { + frame.SetPoseRelativeToGraph(childPoseGraph); + } +} - this->dataPtr->parentPoseGraphScopeName = graph->sourceName; - this->dataPtr->parentPoseGraph = _graph; +///////////////////////////////////////////////// +void Model::SetFrameAttachedToGraph( + sdf::ScopedGraph _graph) +{ + this->dataPtr->frameAttachedToGraph = _graph; - return errors; + auto childFrameAttachedToGraph = + this->dataPtr->frameAttachedToGraph.ChildModelScope(this->Name()); + for (auto &joint : this->dataPtr->joints) + { + joint.SetFrameAttachedToGraph(childFrameAttachedToGraph); + } + for (auto &frame : this->dataPtr->frames) + { + frame.SetFrameAttachedToGraph(childFrameAttachedToGraph); + } + for (auto &model : this->dataPtr->models) + { + model.SetFrameAttachedToGraph(childFrameAttachedToGraph); + } } ///////////////////////////////////////////////// sdf::SemanticPose Model::SemanticPose() const { return sdf::SemanticPose( + this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, - this->dataPtr->parentPoseGraphScopeName, - this->dataPtr->parentPoseGraph); + this->dataPtr->poseGraphScopeVertexName, + this->dataPtr->poseGraph); } ///////////////////////////////////////////////// diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index cafa44788..fed8a3109 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -22,6 +22,7 @@ #include "sdf/Model.hh" ///////////////////////////////////////////////// +/// Test default construction of sdf::Model. TEST(DOMModel, Construction) { sdf::Model model; diff --git a/src/Root.cc b/src/Root.cc index 916bd879d..30a08203c 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -26,6 +26,8 @@ #include "sdf/World.hh" #include "sdf/parser.hh" #include "sdf/sdf_config.h" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -48,16 +50,82 @@ class sdf::RootPrivate /// \brief The actors specified under the root SDF element public: std::vector actors; + /// \brief Frame Attached-To Graphs constructed when loading Worlds. + public: std::vector> + worldFrameAttachedToGraphs; + + /// \brief Frame Attached-To Graphs constructed when loading Models. + public: std::vector> + modelFrameAttachedToGraphs; + + /// \brief Pose Relative-To Graphs constructed when loading Worlds. + public: std::vector> + worldPoseRelativeToGraphs; + + /// \brief Pose Relative-To Graphs constructed when loading Models. + public: std::vector> + modelPoseRelativeToGraphs; + /// \brief The SDF element pointer generated during load. public: sdf::ElementPtr sdf; }; +///////////////////////////////////////////////// +template +sdf::ScopedGraph addFrameAttachedToGraph( + std::vector> &_graphList, + const T &_domObj, sdf::Errors &_errors) +{ + auto &frameGraph = + _graphList.emplace_back(std::make_shared()); + + sdf::Errors buildErrors = + sdf::buildFrameAttachedToGraph(frameGraph, &_domObj); + _errors.insert(_errors.end(), buildErrors.begin(), buildErrors.end()); + + sdf::Errors validateErrors = sdf::validateFrameAttachedToGraph(frameGraph); + _errors.insert(_errors.end(), validateErrors.begin(), validateErrors.end()); + + return frameGraph; +} + +///////////////////////////////////////////////// +template +ScopedGraph addPoseRelativeToGraph( + std::vector> &_graphList, + const T &_domObj, Errors &_errors) +{ + auto &poseGraph = + _graphList.emplace_back(std::make_shared()); + + Errors buildErrors = buildPoseRelativeToGraph(poseGraph, &_domObj); + _errors.insert(_errors.end(), buildErrors.begin(), buildErrors.end()); + + Errors validateErrors = validatePoseRelativeToGraph(poseGraph); + _errors.insert(_errors.end(), validateErrors.begin(), validateErrors.end()); + + return poseGraph; +} + ///////////////////////////////////////////////// Root::Root() : dataPtr(new RootPrivate) { } +///////////////////////////////////////////////// +Root::Root(Root &&_root) noexcept + : dataPtr(std::exchange(_root.dataPtr, nullptr)) +{ +} + +///////////////////////////////////////////////// +Root &Root::operator=(Root &&_root) noexcept +{ + std::swap(this->dataPtr, _root.dataPtr); + return *this; +} + ///////////////////////////////////////////////// Root::~Root() { @@ -151,6 +219,16 @@ Errors Root::Load(SDFPtr _sdf) World world; Errors worldErrors = world.Load(elem); + + // Build the graphs. + auto frameAttachedToGraph = addFrameAttachedToGraph( + this->dataPtr->worldFrameAttachedToGraphs, world, worldErrors); + world.SetFrameAttachedToGraph(frameAttachedToGraph); + + auto poseRelativeToGraph = addPoseRelativeToGraph( + this->dataPtr->worldPoseRelativeToGraphs, world, worldErrors); + world.SetPoseRelativeToGraph(poseRelativeToGraph); + // Attempt to load the world if (worldErrors.empty()) { @@ -161,10 +239,6 @@ Errors Root::Load(SDFPtr _sdf) "World with name[" + world.Name() + "] already exists." " Each world must have a unique name. Skipping this world."}); } - else - { - this->dataPtr->worlds.push_back(std::move(world)); - } } else { @@ -173,15 +247,30 @@ Errors Root::Load(SDFPtr _sdf) errors.push_back({ErrorCode::ELEMENT_INVALID, "Failed to load a world."}); } + + this->dataPtr->worlds.push_back(std::move(world)); elem = elem->GetNextElement("world"); } } // Load all the models. - Errors modelLoadErrors = loadUniqueRepeated(this->dataPtr->sdf, - "model", this->dataPtr->models); + Errors modelLoadErrors = loadUniqueRepeated( + this->dataPtr->sdf, "model", this->dataPtr->models); errors.insert(errors.end(), modelLoadErrors.begin(), modelLoadErrors.end()); + // Build the graphs. + for (sdf::Model &model : this->dataPtr->models) + { + auto frameAttachedToGraph = addFrameAttachedToGraph( + this->dataPtr->modelFrameAttachedToGraphs, model, errors); + + model.SetFrameAttachedToGraph(frameAttachedToGraph); + + auto poseRelativeToGraph = addPoseRelativeToGraph( + this->dataPtr->modelPoseRelativeToGraphs, model, errors); + model.SetPoseRelativeToGraph(poseRelativeToGraph); + } + // Load all the lights. Errors lightLoadErrors = loadUniqueRepeated(this->dataPtr->sdf, "light", this->dataPtr->lights); diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index 55fb747db..60e2e3746 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -23,6 +23,8 @@ #include "sdf/Link.hh" #include "sdf/Light.hh" #include "sdf/Model.hh" +#include "sdf/World.hh" +#include "sdf/Frame.hh" #include "sdf/Root.hh" ///////////////////////////////////////////////// @@ -56,6 +58,27 @@ TEST(DOMRoot, Construction) EXPECT_TRUE(root.ActorByIndex(1) == nullptr); } +///////////////////////////////////////////////// +TEST(DOMRoot, MoveConstructor) +{ + sdf::Root root; + root.SetVersion("test_root"); + + sdf::Root root2(std::move(root)); + EXPECT_EQ("test_root", root2.Version()); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, MoveAssignmentOperator) +{ + sdf::Root root; + root.SetVersion("test_root"); + + sdf::Root root2; + root2 = std::move(root); + EXPECT_EQ("test_root", root2.Version()); +} + ///////////////////////////////////////////////// TEST(DOMRoot, StringParse) { @@ -140,3 +163,91 @@ TEST(DOMRoot, Set) root.SetVersion(SDF_PROTOCOL_VERSION); EXPECT_STREQ(SDF_PROTOCOL_VERSION, root.Version().c_str()); } + +///////////////////////////////////////////////// +TEST(DOMRoot, FrameSemanticsOnMove) +{ + const std::string sdfString1 = R"( + + + + 0 1 0 0 0 0 + + + )"; + + const std::string sdfString2 = R"( + + + + 1 1 0 0 0 0 + + + )"; + + auto testFrame1 = [](const sdf::Root &_root) + { + auto *world = _root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + auto *frame = world->FrameByIndex(0); + ASSERT_NE(nullptr, frame); + EXPECT_EQ("frame1", frame->Name()); + ignition::math::Pose3d pose; + sdf::Errors errors = frame->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, 1, 0, 0, 0, 0), pose); + }; + + auto testFrame2 = [](const sdf::Root &_root) + { + auto *world = _root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + auto *frame = world->FrameByIndex(0); + ASSERT_NE(nullptr, frame); + EXPECT_EQ("frame2", frame->Name()); + ignition::math::Pose3d pose; + sdf::Errors errors = frame->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(1, 1, 0, 0, 0, 0), pose); + }; + + { + sdf::Root root1; + sdf::Errors errors = root1.LoadSdfString(sdfString1); + EXPECT_TRUE(errors.empty()) << errors; + testFrame1(root1); + } + + { + sdf::Root root2; + sdf::Errors errors = root2.LoadSdfString(sdfString2); + EXPECT_TRUE(errors.empty()) << errors; + testFrame2(root2); + } + + { + sdf::Root root1; + sdf::Errors errors = root1.LoadSdfString(sdfString1); + EXPECT_TRUE(errors.empty()) << errors; + + // then root1 is moved into root2 via the move constructor + sdf::Root root2(std::move(root1)); + testFrame1(root2); + } + + { + sdf::Root root1; + sdf::Errors errors = root1.LoadSdfString(sdfString1); + EXPECT_TRUE(errors.empty()) << errors; + sdf::Root root2; + errors = root2.LoadSdfString(sdfString2); + EXPECT_TRUE(errors.empty()) << errors; + + testFrame1(root1); + testFrame2(root2); + + // root1 is moved into root2 via the move assignment operator. + root2 = std::move(root1); + testFrame1(root2); + } +} diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh new file mode 100644 index 000000000..0c30ae588 --- /dev/null +++ b/src/ScopedGraph.hh @@ -0,0 +1,471 @@ +/* + * Copyright 2020 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. + * + */ + +#ifndef SDF_SCOPED_GRAPH_HH +#define SDF_SCOPED_GRAPH_HH + +#include +#include +#include +#include +#include + +#include +#include + +#include "sdf/sdf_config.h" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { + +/// \brief Data structure that holds information associated with each scope +struct ScopedGraphData +{ + /// \brief The vertex ID of the designated scope vertex + ignition::math::graph::VertexId scopeVertexId { + ignition::math::graph::kNullId}; + + /// \brief The prefix for all names under this scope + std::string prefix {}; + + /// \brief The context name of this scope. Either world or __model__ + std::string scopeContextName {}; +}; + +// Forward declarations for static_assert +struct PoseRelativeToGraph; +struct FrameAttachedToGraph; + +/// \brief The ScopedGraph allows manipulating FrameAttachedTo and +/// PoseRelativeTo graphs within a smaller scope such as the scope of a model +/// inside a world graph. +/// +/// Each valid instance of ScopedGraph has a pointer to either a +/// FrameAttachedToGraph or a PoseRelativeToGraph. In addition, each instance +/// contains the following: +/// - The prefix of the scope, which is empty for a world scope and the +/// name of a model for a model scope. +/// - The context name of the scope, which can be either "world" or "__model__". +/// - ID of the vertex on which the scope is anchored. For +/// PoseRelativeToGraph's, this vertex is the root of the sub tree represented +/// by the ScopedGraph instance. For FrameAttachedToGraph, this vertex only +/// represents the scope boundary and does not imply that it's the root of the +/// sub tree because these graphs can have multiple disconnected trees. +/// +/// When a vertex is added to the graph via ScopedGraph, the prefix is prepended +/// to the input name of the vertex. The name of the vertex with the prefix is +/// known as the absolute name of the vertex and the name of the vertex without +/// the prefix is known as the local name of the vertex. +/// +/// \remark ScopedGraph has pointer semantic. i.e, a ScopedGraph object and its +/// copy point to the same underlying data and modifying one will affect the +/// other. +/// +/// \tparam T Either FrameAttachedTo or PoseRelativeTo graph +template +class ScopedGraph +{ + static_assert( + std::is_same_v || + std::is_same_v, + "Template parameter has to be either sdf::PoseRelativeToGraph or " + "sdf::FrameAttachedToGraph"); + + /// \brief template for extracting the graph type from the template parameter. + /// \tparam G ignition::math::DirectedGraph type + /// \return Vertex and Edge types of G + public: template + struct GraphTypeExtracter + { + using Vertex = void; + using Edge = void; + }; + + /// \brief Specialization for GraphTypeExtracter on + /// ignition::math::DirectedGraph + public: template + struct GraphTypeExtracter> + { + using Vertex = V; + using Edge = E; + }; + + // Type aliases + public: using MathGraphType = typename T::GraphType; + public: using VertexId = ignition::math::graph::VertexId; + public: using VertexType = typename GraphTypeExtracter::Vertex; + public: using EdgeType = typename GraphTypeExtracter::Edge; + public: using Vertex = ignition::math::graph::Vertex; + public: using Edge = ignition::math::graph::DirectedEdge; + public: using MapType = typename T::MapType; + + /// \brief Default constructor. The constructed object is invalid as it + /// doesn't point to any graph. + public: ScopedGraph() = default; + + /// \brief Constructor. The constructed object holds a weak pointer to the + /// passed in graph. + /// \param[in] _graph A shared pointer to PoseRelativeTo or FrameAttachedTo + /// graph. + /// \remark The state of the ScopedGraph after construction is invalid because + /// member variables such as "ScopedGraphData::prefix" and + /// "ScopedGraphData::scopeName" will be empty. Therefore, it is imperative + /// that this constructor should only be used right before calling either + /// buildPoseRelativeToGraph or buildFrameAttachedToGraph. + public: explicit ScopedGraph(const std::shared_ptr &_graph); + + /// \brief Creates a scope anchored at an existing child model vertex. + /// \param[in] _name Name of child model vertex. The prefix of the current + /// scope will be prepended to this name before searching for the vertex that + /// matches the name. The new scope will have a new prefix formed by appending + /// _name to the existing prefix of the current scope. + /// \return A new child scope. + public: ScopedGraph ChildModelScope(const std::string &_name) const; + + /// \brief Checks if the scope points to a valid graph. + /// \return True if the scope points to a valid graph. + public: explicit operator bool() const; + + /// \brief Immutable reference to the underlying PoseRelativeTo::graph or + /// FrameAttachedTo::graph. + public: const MathGraphType &Graph() const; + + /// \brief Immutable reference to the underlying PoseRelativeTo::map or + /// FrameAttachedTo::map. + public: const MapType &Map() const; + + /// \brief Adds a scope vertex to the graph. This creates a new + /// scope by making a copy of the current scope with a new prefix and scope + /// type name. A new scope vertex is then added to the graph. + /// \param[in] _prefix The new prefix of the scope. + /// \param[in] _name The Name of the scope vertex to be added. The full name + /// of the vertex will be newPrefix::_name, where newPrefix is the prefix + /// obtained by adding _prefix to the prefix of the current scope. + /// \param[in] _scopeTypeName Name of scope type (either __model__ or world) + /// \param[in] _data Vertex data + public: ScopedGraph AddScopeVertex(const std::string &_prefix, + const std::string &_name, const std::string &_scopeTypeName, + const VertexType &_data); + + /// \brief Adds a vertex to the graph. + /// \param[in] _name The local name of the vertex. The absolute name of the + /// vertex will be _prefix::_name. + /// \param[in] _data Vertex data. + /// \return The newly created vertex. + public: Vertex &AddVertex(const std::string &_name, const VertexType &_data); + + /// \brief Adds an edge to the graph. + /// \param[in] _vertexPair Pair of vertex IDs between which the edge will be + /// created. + /// \param[in] _data Edge data. + /// \return The newly created edge. + public: Edge &AddEdge(const ignition::math::graph::VertexId_P &_vertexPair, + const EdgeType &_data); + + /// \brief Gets all the local names of the vertices in the current scope. + /// \return A list of vertex names in the current scope. + public: std::vector VertexNames() const; + + /// \brief Get the local name of a vertex. + /// \param[in] _id ID of the vertex. + /// \return The local name of the vertex. If the vertex was not found in the + /// graph, "__null__" will be returned. + public: std::string VertexLocalName(const VertexId &_id) const; + + /// \brief Get the local name of a vertex. + /// \param[in] _vertex Vertex object. + /// \return The local name of the vertex. If the vertex was not found in the + /// graph, "__null__" will be returned. + public: std::string VertexLocalName(const Vertex &_vertex) const; + + /// \brief Update the information contained by an edge + /// \remark Since this function has to remove the edge from the graph and add + /// a new one, the original edge will have a new ID. + /// \param[in] _edge The edge to update. + /// \param[in] _data The new data. + public: void UpdateEdge(Edge &_edge, const EdgeType &_data); + + /// \brief Count the number of vertices with a given local name in the scope. + /// \param[in] _name Local name query + /// \return Number of vertices that have the given local name in the scope. + public: size_t Count(const std::string &_name) const; + + /// \brief Get the vertex ID of a vertex with a give local name. + /// \param[in] _name Local name of the vertex + /// \return Vertex ID of the vertex if found in this scope of the graph. + /// Otherwise, kNullId is returned. + public: VertexId VertexIdByName(const std::string &_name) const; + + /// \brief Get the scope vertex. + /// \return Immutable reference to the scope vertex of this scope. + public: const Vertex &ScopeVertex() const; + + /// \brief Get the scope vertex ID. + /// \return The ID of the scope vertex of this scope. + public: VertexId ScopeVertexId() const; + + /// \brief Check if the graph on which this scope is based is the same as the + /// input graph. + /// \param[in] _graph Graph object to check. + /// \return True if this scope points to the same graph as the input. + public: bool PointsTo(const std::shared_ptr &_graph) const; + + /// \brief Set the context name of the scope. + /// \param[in] _name New context name. + public: void SetScopeContextName(const std::string &_name); + + /// \brief Get the current context name. + /// \return The current context name. + public: const std::string &ScopeContextName() const; + + /// \brief Add the current prefix to the given name. + /// \param[in] _name Input name. + /// \return The name with the prefix prepended. + public: std::string AddPrefix(const std::string &_name) const; + + /// \brief Find the current prefix in the given name. If the prefix is found, + /// remove it. + /// \param[in] _name Input name. + /// \return The name with the prefix removed and true, if the prefix is found. + /// Otherwise, the original name and false + public: std::pair FindAndRemovePrefix( + const std::string &_name) const; + + /// \brief Shared pointer to either a FrameAttachedToGraph or + /// PoseRelativeToGraph. + private: std::shared_ptr graphPtr; + + /// \brief Shared pointer to the scope's data. A shared_ptr is used because + /// this data has to be shared among many DOM objects. + private: std::shared_ptr dataPtr; +}; + +///////////////////////////////////////////////// +template +ScopedGraph::ScopedGraph(const std::shared_ptr &_graph) + : graphPtr(_graph) + , dataPtr(std::make_shared()) +{ +} + +///////////////////////////////////////////////// +template +ScopedGraph ScopedGraph::ChildModelScope(const std::string &_name) const +{ + auto newScopedGraph = *this; + newScopedGraph.dataPtr = std::make_shared(); + newScopedGraph.dataPtr->prefix = this->AddPrefix(_name); + newScopedGraph.dataPtr->scopeVertexId = + newScopedGraph.VertexIdByName("__model__"); + newScopedGraph.dataPtr->scopeContextName = "__model__"; + return newScopedGraph; +} + +///////////////////////////////////////////////// +template +ScopedGraph::operator bool() const +{ + return (this->graphPtr != nullptr) && (this->dataPtr != nullptr); +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::Graph() const -> const MathGraphType & +{ + return this->graphPtr->graph; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::Map() const -> const MapType & +{ + return this->graphPtr->map; +} + +///////////////////////////////////////////////// +template +ScopedGraph ScopedGraph::AddScopeVertex(const std::string &_prefix, + const std::string &_name, const std::string &_scopeTypeName, + const VertexType &_data) +{ + auto newScopedGraph = *this; + newScopedGraph.dataPtr = std::make_shared(); + newScopedGraph.dataPtr->prefix = this->AddPrefix(_prefix); + Vertex &vert = newScopedGraph.AddVertex(_name, _data); + newScopedGraph.dataPtr->scopeVertexId = vert.Id(); + newScopedGraph.SetScopeContextName(_scopeTypeName); + return newScopedGraph; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::AddVertex( + const std::string &_name, const VertexType &_data) -> Vertex & +{ + const std::string newName = this->AddPrefix(_name); + Vertex &vert = this->graphPtr->graph.AddVertex(newName, _data); + this->graphPtr->map[newName] = vert.Id(); + return vert; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::AddEdge( + const ignition::math::graph::VertexId_P &_vertexPair, const EdgeType &_data) + -> Edge & +{ + Edge &edge = this->graphPtr->graph.AddEdge(_vertexPair, _data); + return edge; +} + +///////////////////////////////////////////////// +template +std::vector ScopedGraph::VertexNames() const +{ + std::vector out; + for (const auto &namePair : this->Map()) + { + const auto &idNamePair = this->FindAndRemovePrefix(namePair.first); + if (idNamePair.second) + { + out.push_back(idNamePair.first); + } + } + return out; +} + +///////////////////////////////////////////////// +template +std::string ScopedGraph::VertexLocalName(const Vertex &_vert) const +{ + return this->FindAndRemovePrefix(_vert.Name()).first; +} + +///////////////////////////////////////////////// +template +std::string ScopedGraph::VertexLocalName(const VertexId &_id) const +{ + return this->VertexLocalName(this->Graph().VertexFromId(_id)); +} + +///////////////////////////////////////////////// +template +void ScopedGraph::UpdateEdge(Edge &_edge, const EdgeType &_data) +{ + // There's no API to update the data of an edge, so we remove the edge and + // insert a new one with the new pose. + auto tailVertexId = _edge.Tail(); + auto headVertexId = _edge.Head(); + auto &graph = this->graphPtr->graph; + graph.RemoveEdge(_edge.Id()); + _edge = graph.AddEdge({tailVertexId, headVertexId}, _data); +} + +///////////////////////////////////////////////// +template +const std::string &ScopedGraph::ScopeContextName() const +{ + return this->dataPtr->scopeContextName; +} + +///////////////////////////////////////////////// +template +void ScopedGraph::SetScopeContextName(const std::string &_name) +{ + this->dataPtr->scopeContextName = _name; +} + +///////////////////////////////////////////////// +template +std::size_t ScopedGraph::Count(const std::string &_name) const +{ + return this->graphPtr->map.count(this->AddPrefix(_name)); +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::VertexIdByName(const std::string &_name) const -> VertexId +{ + auto &map = this->Map(); + auto it = map.find(this->AddPrefix(_name)); + if (it != map.end()) + return it->second; + else + return ignition::math::graph::kNullId; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::ScopeVertex() const -> const Vertex & +{ + return this->graphPtr->graph.VertexFromId( + this->dataPtr->scopeVertexId); +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::ScopeVertexId() const -> VertexId +{ + return this->dataPtr->scopeVertexId; +} + +///////////////////////////////////////////////// +template +bool ScopedGraph::PointsTo(const std::shared_ptr &_graph) const +{ + return this->graphPtr == _graph; +} + +///////////////////////////////////////////////// +template +std::string ScopedGraph::AddPrefix(const std::string &_name) const +{ + if (this->dataPtr->prefix.empty()) + { + return _name; + } + else + { + return this->dataPtr->prefix + "::" + _name; + } +} + +///////////////////////////////////////////////// +template +std::pair +ScopedGraph::FindAndRemovePrefix(const std::string &_name) const +{ + if (this->dataPtr->prefix.empty()) + { + return std::make_pair(_name, true); + } + + // Convenient alias + const std::string &prefix = this->dataPtr->prefix; + if (_name.size() > prefix.size() + 2 && + (0 == _name.compare(0, prefix.size(), prefix))) + { + return std::make_pair(_name.substr(prefix.size() + 2), true); + } + return std::make_pair(_name, false); +} +} +} + +#endif /* SDF_SCOPED_GRAPH_HH */ diff --git a/src/SemanticPose.cc b/src/SemanticPose.cc index eefc7d51c..83e86bfa2 100644 --- a/src/SemanticPose.cc +++ b/src/SemanticPose.cc @@ -21,6 +21,7 @@ #include "sdf/Error.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" namespace sdf @@ -30,6 +31,8 @@ inline namespace SDF_VERSION_NAMESPACE { /// \brief Private data for the SemanticPose class. class SemanticPosePrivate { + public: std::string name = ""; + /// \brief Raw pose of the SemanticPose object. public: ignition::math::Pose3d rawPose = ignition::math::Pose3d::Zero; @@ -39,24 +42,40 @@ class SemanticPosePrivate /// \brief Name of the default frame to resolve to. public: std::string defaultResolveTo = ""; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model or world scope. + /// TODO (addisu) Make this const + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// SemanticPose::SemanticPose( + const std::string &_name, const ignition::math::Pose3d &_pose, const std::string &_relativeTo, const std::string &_defaultResolveTo, - const std::weak_ptr _graph) + const sdf::ScopedGraph &_graph) : dataPtr(std::make_unique()) { + this->dataPtr->name = _name; this->dataPtr->rawPose = _pose; this->dataPtr->relativeTo = _relativeTo; this->dataPtr->defaultResolveTo = _defaultResolveTo; this->dataPtr->poseRelativeToGraph = _graph; } +///////////////////////////////////////////////// +SemanticPose::SemanticPose( + const ignition::math::Pose3d &_pose, + const std::string &_relativeTo, + const std::string &_defaultResolveTo, + const sdf::ScopedGraph &_graph) + : dataPtr(std::make_unique()) +{ + this->dataPtr->rawPose = _pose; + this->dataPtr->relativeTo = _relativeTo; + this->dataPtr->defaultResolveTo = _defaultResolveTo; + this->dataPtr->poseRelativeToGraph = _graph; +} ///////////////////////////////////////////////// SemanticPose::~SemanticPose() = default; @@ -103,7 +122,7 @@ Errors SemanticPose::Resolve( { Errors errors; - auto graph = this->dataPtr->poseRelativeToGraph.lock(); + auto graph = this->dataPtr->poseRelativeToGraph; if (!graph) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, @@ -124,8 +143,15 @@ Errors SemanticPose::Resolve( } ignition::math::Pose3d pose; - errors = resolvePose(pose, *graph, relativeTo, resolveTo); - pose *= this->RawPose(); + if (this->dataPtr->name.empty()) + { + errors = resolvePose(pose, graph, relativeTo, resolveTo); + pose *= this->RawPose(); + } + else + { + errors = resolvePose(pose, graph, this->dataPtr->name, resolveTo); + } if (errors.empty()) { diff --git a/src/Sensor.cc b/src/Sensor.cc index 1d3c467f3..60f892431 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -27,6 +27,8 @@ #include "sdf/Lidar.hh" #include "sdf/Sensor.hh" #include "sdf/Types.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -128,8 +130,8 @@ class sdf::SensorPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model scope. + public: sdf::ScopedGraph poseRelativeToGraph; /// \brief Pointer to a magnetometer. public: std::unique_ptr magnetometer; @@ -474,7 +476,7 @@ void Sensor::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void Sensor::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } diff --git a/src/Types.cc b/src/Types.cc index 13df3060c..ff724394b 100644 --- a/src/Types.cc +++ b/src/Types.cc @@ -78,5 +78,15 @@ std::string lowercase(const std::string &_in) out[i] = std::tolower(out[i], std::locale()); return out; } + +///////////////////////////////////////////////// +std::ostream &operator<<(std::ostream &_out, const sdf::Errors &_errs) +{ + for (const auto &e : _errs) + { + _out << e << std::endl; + } + return _out; +} } } diff --git a/src/Types_TEST.cc b/src/Types_TEST.cc index 1a406970e..bf1f80930 100644 --- a/src/Types_TEST.cc +++ b/src/Types_TEST.cc @@ -18,8 +18,10 @@ #include #include +#include #include +#include "sdf/Error.hh" #include "sdf/Types.hh" ///////////////////////////////////////////////// @@ -99,6 +101,25 @@ TEST(Types, trim_nothing) EXPECT_EQ(out, "xyz"); } +///////////////////////////////////////////////// +TEST(Types, ErrorsOutputStream) +{ + sdf::Errors errors; + errors.emplace_back(sdf::ErrorCode::FILE_READ, "Error reading file"); + errors.emplace_back(sdf::ErrorCode::DUPLICATE_NAME, "Found duplicate name"); + std::string expected = "Error Code "; + expected += + std::to_string(static_cast(sdf::ErrorCode::FILE_READ)); + expected += " Msg: Error reading file\nError Code "; + expected += + std::to_string(static_cast(sdf::ErrorCode::DUPLICATE_NAME)); + expected += " Msg: Found duplicate name\n"; + + std::stringstream output; + output << errors; + EXPECT_EQ(expected, output.str()); +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) diff --git a/src/Utils.cc b/src/Utils.cc index 9ed658039..b28ac26ee 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -74,5 +74,11 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, // on the pose element value. return posePair.second; } + +///////////////////////////////////////////////// +bool isValidFrameReference(const std::string &_name) +{ + return "__root__" != _name; +} } } diff --git a/src/Utils.hh b/src/Utils.hh index 73740ed1d..f46be539e 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -37,6 +37,12 @@ namespace sdf /// \returns True if the name is a reserved name and thus invalid. bool isReservedName(const std::string &_name); + /// \brief Check if the passed string is a valid frame reference. + /// Currently it's only invalid if the name is __root__ + /// \param[in] _name Name to check. + /// \returns True if the name is a valid frame reference. + bool isValidFrameReference(const std::string &_name); + /// \brief Read the "name" attribute from an element. /// \param[in] _sdf SDF element pointer which contains the name. /// \param[out] _name String to hold the name value. @@ -65,7 +71,7 @@ namespace sdf /// exists. /// \return The vector of errors. An empty vector indicates no errors were /// experienced. - template + template sdf::Errors loadUniqueRepeated(sdf::ElementPtr _sdf, const std::string &_sdfName, std::vector &_objs) { @@ -128,9 +134,10 @@ namespace sdf /// vector, unless an error is encountered during load. /// \return The vector of errors. An empty vector indicates no errors were /// experienced. - template - sdf::Errors loadRepeated(sdf::ElementPtr _sdf, - const std::string &_sdfName, std::vector &_objs) + template + sdf::Errors loadRepeated(sdf::ElementPtr _sdf, const std::string &_sdfName, + std::vector &_objs, + const std::function &_beforeLoadFunc = {}) { Errors errors; @@ -142,6 +149,10 @@ namespace sdf while (elem) { Class obj; + if (_beforeLoadFunc) + { + _beforeLoadFunc(obj); + } // Load the model and capture the errors. Errors loadErrors = obj.Load(elem); diff --git a/src/Visual.cc b/src/Visual.cc index 8d99b5f68..ac7c4ae47 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -21,6 +21,8 @@ #include "sdf/Types.hh" #include "sdf/Visual.hh" #include "sdf/Geometry.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -64,8 +66,8 @@ class sdf::VisualPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model scope. + public: sdf::ScopedGraph poseRelativeToGraph; /// \brief Visibility flags of a visual. Defaults to 0xFFFFFFFF public: uint32_t visibilityFlags = 4294967295u; @@ -276,7 +278,7 @@ void Visual::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void Visual::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } diff --git a/src/World.cc b/src/World.cc index 38ac9c1b3..704408e13 100644 --- a/src/World.cc +++ b/src/World.cc @@ -27,6 +27,7 @@ #include "sdf/Types.hh" #include "sdf/World.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -88,11 +89,13 @@ class sdf::WorldPrivate public: ignition::math::Vector3d windLinearVelocity = ignition::math::Vector3d::Zero; - /// \brief Frame Attached-To Graph constructed during Load. - public: std::shared_ptr frameAttachedToGraph; + /// \brief Scoped Frame Attached-To graph that points to a graph owned + /// by this world. + public: sdf::ScopedGraph frameAttachedToGraph; - /// \brief Pose Relative-To Graph constructed during Load. - public: std::shared_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph that points to a graph owned by this + /// world. + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -107,7 +110,9 @@ WorldPrivate::WorldPrivate(const WorldPrivate &_worldPrivate) name(_worldPrivate.name), physics(_worldPrivate.physics), sdf(_worldPrivate.sdf), - windLinearVelocity(_worldPrivate.windLinearVelocity) + windLinearVelocity(_worldPrivate.windLinearVelocity), + frameAttachedToGraph(_worldPrivate.frameAttachedToGraph), + poseRelativeToGraph(_worldPrivate.poseRelativeToGraph) { if (_worldPrivate.atmosphere) { @@ -118,16 +123,6 @@ WorldPrivate::WorldPrivate(const WorldPrivate &_worldPrivate) { this->gui = std::make_unique(*(_worldPrivate.gui)); } - if (_worldPrivate.frameAttachedToGraph) - { - this->frameAttachedToGraph = std::make_shared( - *(_worldPrivate.frameAttachedToGraph)); - } - if (_worldPrivate.poseRelativeToGraph) - { - this->poseRelativeToGraph = std::make_shared( - *(_worldPrivate.poseRelativeToGraph)); - } if (_worldPrivate.scene) { this->scene = std::make_unique(*(_worldPrivate.scene)); @@ -152,20 +147,6 @@ World::~World() World::World(const World &_world) : dataPtr(new WorldPrivate(*_world.dataPtr)) { - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - for (auto &model : this->dataPtr->models) - { - model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - for (auto &light : this->dataPtr->lights) - { - light.SetXmlParentName("world"); - light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } } ///////////////////////////////////////////////// @@ -266,8 +247,8 @@ Errors World::Load(sdf::ElementPtr _sdf) std::unordered_set frameNames; // Load all the models. - Errors modelLoadErrors = loadUniqueRepeated(_sdf, "model", - this->dataPtr->models); + Errors modelLoadErrors = + loadUniqueRepeated(_sdf, "model", this->dataPtr->models); errors.insert(errors.end(), modelLoadErrors.begin(), modelLoadErrors.end()); // Models are loaded first, and loadUniqueRepeated ensures there are no @@ -341,48 +322,6 @@ Errors World::Load(sdf::ElementPtr _sdf) errors.insert(errors.end(), sceneLoadErrors.begin(), sceneLoadErrors.end()); } - // Build the graphs. - this->dataPtr->frameAttachedToGraph = - std::make_shared(); - Errors frameAttachedToGraphErrors = - buildFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph, this); - errors.insert(errors.end(), frameAttachedToGraphErrors.begin(), - frameAttachedToGraphErrors.end()); - Errors validateFrameAttachedGraphErrors = - validateFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph); - errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), - validateFrameAttachedGraphErrors.end()); - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } - - this->dataPtr->poseRelativeToGraph = std::make_shared(); - Errors poseRelativeToGraphErrors = - buildPoseRelativeToGraph(*this->dataPtr->poseRelativeToGraph, this); - errors.insert(errors.end(), poseRelativeToGraphErrors.begin(), - poseRelativeToGraphErrors.end()); - Errors validatePoseGraphErrors = - validatePoseRelativeToGraph(*this->dataPtr->poseRelativeToGraph); - errors.insert(errors.end(), validatePoseGraphErrors.begin(), - validatePoseGraphErrors.end()); - for (auto &frame : this->dataPtr->frames) - { - frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - for (auto &model : this->dataPtr->models) - { - Errors setPoseRelativeToGraphErrors = - model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), - setPoseRelativeToGraphErrors.end()); - } - for (auto &light : this->dataPtr->lights) - { - light.SetXmlParentName("world"); - light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - return errors; } @@ -666,3 +605,39 @@ bool World::PhysicsNameExists(const std::string &_name) const return false; } + +///////////////////////////////////////////////// +void World::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) +{ + this->dataPtr->poseRelativeToGraph = _graph; + + for (auto &model : this->dataPtr->models) + { + model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } + for (auto &frame : this->dataPtr->frames) + { + frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } + for (auto &light : this->dataPtr->lights) + { + light.SetXmlParentName("world"); + light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } +} + +///////////////////////////////////////////////// +void World::SetFrameAttachedToGraph( + sdf::ScopedGraph _graph) +{ + this->dataPtr->frameAttachedToGraph = _graph; + + for (auto &frame : this->dataPtr->frames) + { + frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } + for (auto &model : this->dataPtr->models) + { + model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } +} diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 4d354c3b9..52130611d 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -38,9 +38,11 @@ COMMANDS = { 'sdf' => "Utilities for SDF files.\n\n"\ " ign sdf [options]\n\n"\ "Options:\n\n"\ - " -k [ --check ] arg Check if an SDFormat file is valid.\n" + - " -d [ --describe ] [SPEC VERSION] Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" + - " -p [ --print ] arg Print converted arg.\n" + + " -k [ --check ] arg Check if an SDFormat file is valid.\n" + + " -d [ --describe ] [SPEC VERSION] Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" + + " -g [ --graph ] arg Print the PoseRelativeTo or FrameAttachedTo graph. (WARNING: This is for advanced\n" + + " use only and the output may change without any promise of stability)\n" + + " -p [ --print ] arg Print converted arg.\n" + COMMON_OPTIONS } @@ -77,6 +79,10 @@ class Cmd 'Print converted arg') do |arg| options['print'] = arg end + opts.on('-g arg', '--graph type', String, + 'Print PoseRelativeTo or FrameAttachedTo graph') do |graph_type| + options['graph'] = {:type => graph_type} + end end begin opt_parser.parse!(args) @@ -156,6 +162,9 @@ class Cmd elsif options.key?('print') Importer.extern 'int cmdPrint(const char *)' exit(Importer.cmdPrint(File.expand_path(options['print']))) + elsif options.key?('graph') + Importer.extern 'int cmdGraph(const char *, const char *)' + exit(Importer.cmdGraph(options['graph'][:type], File.expand_path(ARGV[1]))) else puts 'Command error: I do not have an implementation '\ 'for this command.' diff --git a/src/ign.cc b/src/ign.cc index 90262276b..dfef44a81 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -15,6 +15,7 @@ * */ +#include #include #include @@ -24,6 +25,8 @@ #include "sdf/parser.hh" #include "sdf/system_util.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "ign.hh" ////////////////////////////////////////////////// @@ -157,3 +160,68 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path) return 0; } + +////////////////////////////////////////////////// +// cppcheck-suppress unusedFunction +extern "C" SDFORMAT_VISIBLE int cmdGraph( + const char *_graphType, const char *_path) +{ + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } + + sdf::Root root; + sdf::Errors errors = root.Load(_path); + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + + if (std::strcmp(_graphType, "pose") == 0) + { + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + if (root.WorldCount() > 0) + { + errors = sdf::buildPoseRelativeToGraph(graph, root.WorldByIndex(0)); + } + else if (root.ModelCount() > 0) + { + errors = sdf::buildPoseRelativeToGraph(graph, root.ModelByIndex(0)); + } + + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + std::cout << graph.Graph() << std::endl; + } + else if (std::strcmp(_graphType, "frame") == 0) + { + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + if (root.WorldCount() > 0) + { + errors = sdf::buildFrameAttachedToGraph(graph, root.WorldByIndex(0)); + } + else if (root.ModelCount() > 0) + { + errors = sdf::buildFrameAttachedToGraph(graph, root.ModelByIndex(0)); + } + + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + std::cout << graph.Graph() << std::endl; + } + else + { + std::cerr << R"(Only "pose" and "frame" graph types are supported)" + << std::endl; + } + + return 0; +} diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 741eba8b2..ecf31ae9c 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -223,8 +223,8 @@ TEST(check, SDF) // Check joint_invalid_self_child.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(output.find("Error: FrameAttachedToGraph cycle detected, " - "already visited vertex [self]."), + EXPECT_NE(output.find("Error: FrameAttachedToGraph cycle detected, already " + "visited vertex [joint_invalid_self_child::self]."), std::string::npos) << output; } @@ -493,11 +493,13 @@ TEST(check, SDF) // Check model_frame_invalid_attached_to_cycle.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(std::string::npos, output.find( - "FrameAttachedToGraph cycle detected, already visited vertex [F1].")) + EXPECT_NE(std::string::npos, + output.find("FrameAttachedToGraph cycle detected, already visited " + "vertex [model_frame_invalid_attached_to_cycle::F1].")) << output; - EXPECT_NE(std::string::npos, output.find( - "FrameAttachedToGraph cycle detected, already visited vertex [F2].")) + EXPECT_NE(std::string::npos, + output.find("FrameAttachedToGraph cycle detected, already visited " + "vertex [model_frame_invalid_attached_to_cycle::F2].")) << output; } @@ -571,18 +573,16 @@ TEST(check, SDF) 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. + // Check an SDF file with nested_models using nested links/frames as joint + // parent or child frames. + // This is a valid file. { - std::string path = pathBase +"/model_invalid_nested_joint_child.sdf"; + std::string path = pathBase +"/joint_nested_parent_child.sdf"; - // Check model_invalid_nested_joint_child.sdf + // Check model_nested_model_relative_to.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(output.find("Error: Child frame 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; + EXPECT_EQ("Valid.\n", output) << output; } // Check an SDF file with joints using the relative_to attribute. @@ -661,11 +661,13 @@ TEST(check, SDF) // Check model_invalid_frame_relative_to_cycle.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(std::string::npos, output.find( - "PoseRelativeToGraph cycle detected, already visited vertex [F1].")) + EXPECT_NE(std::string::npos, + output.find("PoseRelativeToGraph cycle detected, already visited " + "vertex [model_invalid_frame_relative_to_cycle::F1].")) << output; - EXPECT_NE(std::string::npos, output.find( - "PoseRelativeToGraph cycle detected, already visited vertex [F2].")) + EXPECT_NE(std::string::npos, + output.find("PoseRelativeToGraph cycle detected, already visited " + "vertex [model_invalid_frame_relative_to_cycle::F2].")) << output; } @@ -721,6 +723,90 @@ TEST(check, SDF) std::string::npos) << output; } + + // Check an SDF file with an valid nested model cross references + { + std::string path = pathBase + "/nested_model_cross_references.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + + // Check an SDF model file with an invalid usage of __root__ + { + std::string path = pathBase + "/model_invalid_root_reference.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [relative_to]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a" + " value of attribute [attached_to]"), + std::string::npos) + << output; + } + // Check an SDF world file with an invalid usage of __root__ + { + // Set SDF_PATH so that included models can be found + setenv("SDF_PATH", PROJECT_SOURCE_PATH "/test/integration/model", 1); + std::string path = pathBase + "/world_invalid_root_reference.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [relative_to]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [attached_to]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [placement_frame]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [canonical_link]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [parent_frame]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of element [placement_frame]"), + std::string::npos) + << output; + EXPECT_NE( + output.find( + "Error: The supplied joint child name [__root__] is not valid"), + std::string::npos) + << output; + EXPECT_NE( + output.find( + "Error: The supplied joint parent name [__root__] is not valid"), + std::string::npos) + << output; + } + // Check an SDF world file with an valid usage of __root__ + { + std::string path = pathBase + "/world_valid_root_reference.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } } ///////////////////////////////////////////////// @@ -791,6 +877,228 @@ TEST(print, SDF) } } +///////////////////////////////////////////////// +TEST(GraphCmd, WorldPoseRelativeTo) +{ + const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; + + // world pose relative_to graph + const std::string path = + pathBase + "/world_relative_to_nested_reference.sdf"; + + const std::string output = + custom_exec_str(g_ignCommand + " sdf -g pose " + path + g_sdfVersion); + + std::stringstream expected; + expected << "digraph {\n" + << " 0 [label=\"__root__ (0)\"];\n" + << " 1 [label=\"world (1)\"];\n" + << " 2 [label=\"M1 (2)\"];\n" + << " 3 [label=\"M1::__model__ (3)\"];\n" + << " 4 [label=\"M1::L1 (4)\"];\n" + << " 5 [label=\"M1::L2 (5)\"];\n" + << " 6 [label=\"M1::J1 (6)\"];\n" + << " 7 [label=\"M1::F1 (7)\"];\n" + << " 8 [label=\"M1::CM1 (8)\"];\n" + << " 9 [label=\"M1::CM1::__model__ (9)\"];\n" + << " 10 [label=\"M1::CM1::L (10)\"];\n" + << " 11 [label=\"F2 (11)\"];\n" + << " 12 [label=\"F3 (12)\"];\n" + << " 13 [label=\"F4 (13)\"];\n" + << " 14 [label=\"F5 (14)\"];\n" + << " 15 [label=\"F6 (15)\"];\n" + << " 16 [label=\"F7 (16)\"];\n" + << " 0 -> 1 [label=1];\n" + << " 2 -> 3 [label=0];\n" + << " 3 -> 4 [label=1];\n" + << " 3 -> 5 [label=1];\n" + << " 8 -> 9 [label=0];\n" + << " 9 -> 10 [label=1];\n" + << " 5 -> 6 [label=1];\n" + << " 5 -> 7 [label=1];\n" + << " 3 -> 8 [label=1];\n" + << " 1 -> 2 [label=1];\n" + << " 2 -> 11 [label=1];\n" + << " 4 -> 12 [label=1];\n" + << " 6 -> 13 [label=1];\n" + << " 7 -> 14 [label=1];\n" + << " 8 -> 15 [label=1];\n" + << " 10 -> 16 [label=1];\n" + << "}"; + EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); +} + +///////////////////////////////////////////////// +TEST(GraphCmd, ModelPoseRelativeTo) +{ + const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; + const std::string path = pathBase + "/model_relative_to_nested_reference.sdf"; + const std::string output = + custom_exec_str(g_ignCommand + " sdf -g pose " + path + g_sdfVersion); + + std::stringstream expected; + expected << "digraph {\n" + << " 0 [label=\"__root__ (0)\"];\n" + << " 1 [label=\"parent_model (1)\"];\n" + << " 2 [label=\"parent_model::__model__ (2)\"];\n" + << " 3 [label=\"parent_model::L (3)\"];\n" + << " 4 [label=\"parent_model::M1 (4)\"];\n" + << " 5 [label=\"parent_model::M1::__model__ (5)\"];\n" + << " 6 [label=\"parent_model::M1::L1 (6)\"];\n" + << " 7 [label=\"parent_model::M1::L2 (7)\"];\n" + << " 8 [label=\"parent_model::M1::J1 (8)\"];\n" + << " 9 [label=\"parent_model::M1::F1 (9)\"];\n" + << " 10 [label=\"parent_model::M1::CM1 (10)\"];\n" + << " 11 [label=\"parent_model::M1::CM1::__model__ (11)\"];\n" + << " 12 [label=\"parent_model::M1::CM1::L (12)\"];\n" + << " 13 [label=\"parent_model::M2 (13)\"];\n" + << " 14 [label=\"parent_model::M2::__model__ (14)\"];\n" + << " 15 [label=\"parent_model::M2::L (15)\"];\n" + << " 16 [label=\"parent_model::M3 (16)\"];\n" + << " 17 [label=\"parent_model::M3::__model__ (17)\"];\n" + << " 18 [label=\"parent_model::M3::L (18)\"];\n" + << " 19 [label=\"parent_model::M4 (19)\"];\n" + << " 20 [label=\"parent_model::M4::__model__ (20)\"];\n" + << " 21 [label=\"parent_model::M4::L (21)\"];\n" + << " 22 [label=\"parent_model::M5 (22)\"];\n" + << " 23 [label=\"parent_model::M5::__model__ (23)\"];\n" + << " 24 [label=\"parent_model::M5::L (24)\"];\n" + << " 25 [label=\"parent_model::M6 (25)\"];\n" + << " 26 [label=\"parent_model::M6::__model__ (26)\"];\n" + << " 27 [label=\"parent_model::M6::L (27)\"];\n" + << " 28 [label=\"parent_model::M7 (28)\"];\n" + << " 29 [label=\"parent_model::M7::__model__ (29)\"];\n" + << " 30 [label=\"parent_model::M7::L (30)\"];\n" + << " 1 -> 2 [label=0];\n" + << " 2 -> 3 [label=1];\n" + << " 4 -> 5 [label=0];\n" + << " 5 -> 6 [label=1];\n" + << " 5 -> 7 [label=1];\n" + << " 10 -> 11 [label=0];\n" + << " 11 -> 12 [label=1];\n" + << " 7 -> 8 [label=1];\n" + << " 7 -> 9 [label=1];\n" + << " 5 -> 10 [label=1];\n" + << " 13 -> 14 [label=0];\n" + << " 14 -> 15 [label=1];\n" + << " 16 -> 17 [label=0];\n" + << " 17 -> 18 [label=1];\n" + << " 19 -> 20 [label=0];\n" + << " 20 -> 21 [label=1];\n" + << " 22 -> 23 [label=0];\n" + << " 23 -> 24 [label=1];\n" + << " 25 -> 26 [label=0];\n" + << " 26 -> 27 [label=1];\n" + << " 28 -> 29 [label=0];\n" + << " 29 -> 30 [label=1];\n" + << " 2 -> 4 [label=1];\n" + << " 4 -> 13 [label=1];\n" + << " 6 -> 16 [label=1];\n" + << " 8 -> 19 [label=1];\n" + << " 9 -> 22 [label=1];\n" + << " 10 -> 25 [label=1];\n" + << " 12 -> 28 [label=1];\n" + << " 0 -> 1 [label=1];\n" + << "}"; + + EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); +} + +///////////////////////////////////////////////// +TEST(GraphCmd, WorldFrameAttachedTo) +{ + const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; + const std::string path = pathBase + "/world_nested_frame_attached_to.sdf"; + const std::string output = + custom_exec_str(g_ignCommand + " sdf -g frame " + path + g_sdfVersion); + + std::stringstream expected; + + expected << "digraph {\n" + << " 0 [label=\"world (0)\"];\n" + << " 1 [label=\"M1 (1)\"];\n" + << " 2 [label=\"M1::__model__ (2)\"];\n" + << " 3 [label=\"M1::L (3)\"];\n" + << " 4 [label=\"M1::J1 (4)\"];\n" + << " 5 [label=\"M1::F0 (5)\"];\n" + << " 6 [label=\"M1::M2 (6)\"];\n" + << " 7 [label=\"M1::M2::__model__ (7)\"];\n" + << " 8 [label=\"M1::M2::L (8)\"];\n" + << " 9 [label=\"world_frame (9)\"];\n" + << " 10 [label=\"F0 (10)\"];\n" + << " 11 [label=\"F1 (11)\"];\n" + << " 12 [label=\"F2 (12)\"];\n" + << " 13 [label=\"F3 (13)\"];\n" + << " 14 [label=\"F4 (14)\"];\n" + << " 15 [label=\"F5 (15)\"];\n" + << " 16 [label=\"F6 (16)\"];\n" + << " 1 -> 2 [label=0];\n" + << " 6 -> 7 [label=0];\n" + << " 7 -> 8 [label=1];\n" + << " 4 -> 5 [label=1];\n" + << " 5 -> 8 [label=1];\n" + << " 2 -> 3 [label=1];\n" + << " 9 -> 0 [label=1];\n" + << " 10 -> 0 [label=1];\n" + << " 11 -> 1 [label=1];\n" + << " 12 -> 3 [label=1];\n" + << " 13 -> 6 [label=1];\n" + << " 14 -> 8 [label=1];\n" + << " 15 -> 5 [label=1];\n" + << " 16 -> 4 [label=1];\n" + << "}"; + EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); +} + +///////////////////////////////////////////////// +TEST(GraphCmd, ModelFrameAttachedTo) +{ + const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; + const std::string path = pathBase + "/model_nested_frame_attached_to.sdf"; + const std::string output = + custom_exec_str(g_ignCommand + " sdf -g frame " + path + g_sdfVersion); + + std::stringstream expected; + + expected + << "digraph {\n" + << " 0 [label=\"__root__ (0)\"];\n" + << " 1 [label=\"nested_model_frame_attached_to (1)\"];\n" + << " 2 [label=\"nested_model_frame_attached_to::__model__ (2)\"];\n" + << " 3 [label=\"nested_model_frame_attached_to::L (3)\"];\n" + << " 4 [label=\"nested_model_frame_attached_to::F0 (4)\"];\n" + << " 5 [label=\"nested_model_frame_attached_to::F1 (5)\"];\n" + << " 6 [label=\"nested_model_frame_attached_to::F2 (6)\"];\n" + << " 7 [label=\"nested_model_frame_attached_to::F3 (7)\"];\n" + << " 8 [label=\"nested_model_frame_attached_to::F4 (8)\"];\n" + << " 9 [label=\"nested_model_frame_attached_to::M1 (9)\"];\n" + << " 10 [label=\"nested_model_frame_attached_to::M1::__model__ " + "(10)\"];\n" + << " 11 [label=\"nested_model_frame_attached_to::M1::L (11)\"];\n" + << " 12 [label=\"nested_model_frame_attached_to::M1::J1 (12)\"];\n" + << " 13 [label=\"nested_model_frame_attached_to::M1::F (13)\"];\n" + << " 14 [label=\"nested_model_frame_attached_to::M1::M2 (14)\"];\n" + << " 15 [label=\"nested_model_frame_attached_to::M1::M2::__model__ " + "(15)\"];\n" + << " 16 [label=\"nested_model_frame_attached_to::M1::M2::L (16)\"];\n" + << " 1 -> 2 [label=0];\n" + << " 9 -> 10 [label=0];\n" + << " 14 -> 15 [label=0];\n" + << " 15 -> 16 [label=1];\n" + << " 12 -> 13 [label=1];\n" + << " 13 -> 14 [label=1];\n" + << " 10 -> 11 [label=1];\n" + << " 4 -> 9 [label=1];\n" + << " 5 -> 11 [label=1];\n" + << " 6 -> 5 [label=1];\n" + << " 7 -> 13 [label=1];\n" + << " 8 -> 12 [label=1];\n" + << " 2 -> 3 [label=1];\n" + << "}"; + + EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) diff --git a/src/parser.cc b/src/parser.cc index a92e44d8b..35a9bb468 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -37,6 +38,8 @@ #include "Converter.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" +#include "Utils.hh" #include "parser_private.hh" #include "parser_urdf.hh" @@ -837,6 +840,20 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) _sdf->Copy(refSDF); } + // A list of parent element-attributes pairs where a frame name is referenced + // in the attribute. This is used to check if the reference is invalid. + std::set> frameReferenceAttributes { + // //frame/[@attached_to] + {"frame", "attached_to"}, + // //pose/[@relative_to] + {"pose", "relative_to"}, + // //model/[@placement_frame] + {"model", "placement_frame"}, + // //model/[@canonical_link] + {"model", "canonical_link"}, + // //sensor/imu/orientation_reference_frame/custom_rpy/[@parent_frame] + {"custom_rpy", "parent_frame"}}; + const tinyxml2::XMLAttribute *attribute = _xml->FirstAttribute(); unsigned int i = 0; @@ -860,6 +877,18 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) ParamPtr p = _sdf->GetAttribute(i); if (p->GetKey() == attribute->Name()) { + if (frameReferenceAttributes.count( + std::make_pair(_sdf->GetName(), attribute->Name())) != 0) + { + if (!isValidFrameReference(attribute->Value())) + { + _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, + "'" + std::string(attribute->Value()) + + "' is reserved; it cannot be used as a value of " + "attribute [" + + p->GetKey() + "]"}); + } + } // Set the value of the SDF attribute if (!p->SetFromString(attribute->Value())) { @@ -1042,8 +1071,19 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) "element"}); return false; } - topLevelElem->GetAttribute("placement_frame")->SetFromString( - elemXml->FirstChildElement("placement_frame")->GetText()); + + const std::string placementFrameVal = + elemXml->FirstChildElement("placement_frame")->GetText(); + + if (!isValidFrameReference(placementFrameVal)) + { + _errors.push_back({ErrorCode::RESERVED_NAME, + "'" + placementFrameVal + + "' is reserved; it cannot be used as a value of " + "element [placement_frame]"}); + } + topLevelElem->GetAttribute("placement_frame") + ->SetFromString(placementFrameVal); } if (isModel || isActor) @@ -1066,21 +1106,14 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) } } - if (_sdf->GetName() == "model") - { - addNestedModel(_sdf, includeSDF->Root(), _errors); - } - else - { - includeSDF->Root()->GetFirstElement()->SetParent(_sdf); - _sdf->InsertElement(includeSDF->Root()->GetFirstElement()); - // TODO: This was used to store the included filename so that when - // a world is saved, the included model's SDF is not stored in the - // world file. This highlights the need to make model inclusion - // a core feature of SDF, and not a hack that that parser handles - // includeSDF->Root()->GetFirstElement()->SetInclude( - // elemXml->Attribute("filename")); - } + includeSDF->Root()->GetFirstElement()->SetParent(_sdf); + _sdf->InsertElement(includeSDF->Root()->GetFirstElement()); + // TODO: This was used to store the included filename so that when + // a world is saved, the included model's SDF is not stored in the + // world file. This highlights the need to make model inclusion + // a core feature of SDF, and not a hack that that parser handles + // includeSDF->Root()->GetFirstElement()->SetInclude( + // elemXml->Attribute("filename")); continue; } @@ -1154,26 +1187,6 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) return true; } -///////////////////////////////////////////////// -static void replace_all(std::string &_str, - const std::string &_from, - const std::string &_to) -{ - if (_from.empty()) - { - return; - } - size_t start_pos = 0; - while ((start_pos = _str.find(_from, start_pos)) != std::string::npos) - { - _str.replace(start_pos, _from.length(), _to); - // We need to advance our starting position beyond what we - // just replaced to deal with the case where the '_to' string - // happens to contain a piece of '_from'. - start_pos += _to.length(); - } -} - ///////////////////////////////////////////////// void copyChildren(ElementPtr _sdf, tinyxml2::XMLElement *_xml, @@ -1233,164 +1246,6 @@ void copyChildren(ElementPtr _sdf, } } -///////////////////////////////////////////////// -void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF) -{ - Errors errors; - addNestedModel(_sdf, _includeSDF, errors); - for (const auto &e : errors) - { - sdferr << e << '\n'; - } -} - -///////////////////////////////////////////////// -void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF, Errors &_errors) -{ - ElementPtr modelPtr = _includeSDF->GetElement("model"); - ElementPtr elem = modelPtr->GetFirstElement(); - std::map replace; - - ignition::math::Pose3d modelPose = - modelPtr->Get("pose"); - - std::string modelName = modelPtr->Get("name"); - - // Inject a frame to represent the nested __model__ frame. - ElementPtr nestedModelFrame = _sdf->AddElement("frame"); - const std::string nestedModelFrameName = modelName + "::__model__"; - nestedModelFrame->GetAttribute("name")->Set(nestedModelFrameName); - - replace["__model__"] = nestedModelFrameName; - - if (modelPtr->GetAttribute("placement_frame")->GetSet()) - { - const std::string placementFrameName = - modelPtr->GetAttribute("placement_frame")->GetAsString(); - - if (!placementFrameName.empty()) - { - // The placementFrameShimElement frame is added as a way to inject the - // placement frame information into expanded nested models. This will not - // be necessary when included nested models work as directly nested - // models. See - // https://github.com/osrf/sdformat/issues/319#issuecomment-665214004 - // TODO (addisu) Remove placementFrameIdentifier once PR addressing - // https://github.com/osrf/sdformat/issues/284 lands - ElementPtr placementFrameShimElement = _sdf->AddElement("frame"); - placementFrameShimElement->GetAttribute("name")->Set( - modelName + "::__placement_frame__"); - placementFrameShimElement->GetAttribute("attached_to") - ->Set(modelName + "::" + placementFrameName); - } - } - - std::string canonicalLinkName = ""; - if (modelPtr->GetAttribute("canonical_link")->GetSet()) - { - canonicalLinkName = modelPtr->GetAttribute("canonical_link")->GetAsString(); - } - else if (modelPtr->HasElement("link")) - { - canonicalLinkName = - modelPtr->GetElement("link")->GetAttribute("name")->GetAsString(); - } - nestedModelFrame->GetAttribute("attached_to") - ->Set(modelName + "::" + canonicalLinkName); - - ElementPtr nestedModelFramePose = nestedModelFrame->AddElement("pose"); - nestedModelFramePose->Set(modelPose); - - // Set the nestedModelFrame's //pose/@relative_to to the frame used in - // //include/pose/@relative_to. - std::string modelPoseRelativeTo = ""; - if (modelPtr->HasElement("pose")) - { - modelPoseRelativeTo = - modelPtr->GetElement("pose")->Get("relative_to"); - } - - // If empty, use "__model__", since leaving it empty would make it - // relative_to the canonical link frame specified in //frame/@attached_to. - if (modelPoseRelativeTo.empty()) - { - modelPoseRelativeTo = "__model__"; - } - - nestedModelFramePose->GetAttribute("relative_to")->Set(modelPoseRelativeTo); - - while (elem) - { - if ((elem->GetName() == "link") || - (elem->GetName() == "model") || - (elem->GetName() == "joint") || - (elem->GetName() == "frame")) - { - std::string elemName = elem->Get("name"); - std::string newName = modelName + "::" + elemName; - replace[elemName] = newName; - } - - if ((elem->GetName() == "link") || (elem->GetName() == "model")) - { - // Add a pose element even if the element doesn't originally have one - auto elemPose = elem->GetElement("pose"); - - // If //pose/@relative_to is empty, explicitly set it to the name - // of the nested model frame. - auto relativeTo = elemPose->GetAttribute("relative_to"); - if (relativeTo->GetAsString().empty()) - { - relativeTo->Set(nestedModelFrameName); - } - - // If //pose/@relative_to is set, let the replacement step handle it. - } - else if (elem->GetName() == "frame") - { - // If //frame/@attached_to is empty, explicitly set it to the name - // of the nested model frame. - auto attachedTo = elem->GetAttribute("attached_to"); - if (attachedTo->GetAsString().empty()) - { - attachedTo->Set(nestedModelFrameName); - } - - // If //frame/@attached_to is set, let the replacement step handle it. - } - elem = elem->GetNextElement(); - } - - std::string str = _includeSDF->ToString(""); - for (std::map::iterator iter = replace.begin(); - iter != replace.end(); ++iter) - { - replace_all(str, std::string("\"") + iter->first + "\"", - std::string("\"") + iter->second + "\""); - replace_all(str, std::string("'") + iter->first + "'", - std::string("'") + iter->second + "'"); - replace_all(str, std::string(">") + iter->first + "<", - std::string(">") + iter->second + "<"); - } - - _includeSDF->ClearElements(); - readString(str, _includeSDF, _errors); - - elem = _includeSDF->GetElement("model")->GetFirstElement(); - ElementPtr nextElem; - while (elem) - { - nextElem = elem->GetNextElement(); - - if (elem->GetName() != "pose") - { - elem->SetParent(_sdf); - _sdf->InsertElement(elem); - } - elem = nextElem; - } -} - ///////////////////////////////////////////////// bool convertFile(const std::string &_filename, const std::string &_version, SDFPtr _sdf) @@ -1590,6 +1445,36 @@ bool checkFrameAttachedToNames(const sdf::Root *_root) auto checkWorldFrameAttachedToNames = []( const sdf::World *_world) -> bool { + auto findNameInWorld = [](const sdf::World *_inWorld, + const std::string &_name) -> bool { + if (_inWorld->ModelNameExists(_name) || + _inWorld->FrameNameExists(_name)) + { + return true; + } + + const auto delimIndex = _name.find("::"); + if (delimIndex != std::string::npos && delimIndex + 2 < _name.size()) + { + std::string modelName = _name.substr(0, delimIndex); + std::string nameToCheck = _name.substr(delimIndex + 2); + const auto *model = _inWorld->ModelByName(modelName); + if (nullptr == model) + { + return false; + } + + if (model->LinkNameExists(nameToCheck) || + model->ModelNameExists(nameToCheck) || + model->JointNameExists(nameToCheck) || + model->FrameNameExists(nameToCheck)) + { + return true; + } + } + return false; + }; + bool worldResult = true; for (uint64_t f = 0; f < _world->FrameCount(); ++f) { @@ -1613,8 +1498,7 @@ bool checkFrameAttachedToNames(const sdf::Root *_root) << std::endl; worldResult = false; } - else if (!_world->ModelNameExists(attachedTo) && - !_world->FrameNameExists(attachedTo)) + else if (!findNameInWorld(_world, attachedTo)) { std::cerr << "Error: attached_to name[" << attachedTo << "] specified by frame with name[" << frame->Name() @@ -1712,7 +1596,8 @@ bool checkFrameAttachedToGraph(const sdf::Root *_root) const sdf::Model *_model) -> bool { bool modelResult = true; - sdf::FrameAttachedToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildFrameAttachedToGraph(graph, _model); if (!errors.empty()) { @@ -1740,7 +1625,8 @@ bool checkFrameAttachedToGraph(const sdf::Root *_root) const sdf::World *_world) -> bool { bool worldResult = true; - sdf::FrameAttachedToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildFrameAttachedToGraph(graph, _world); if (!errors.empty()) { @@ -1793,7 +1679,8 @@ bool checkPoseRelativeToGraph(const sdf::Root *_root) const sdf::Model *_model) -> bool { bool modelResult = true; - sdf::PoseRelativeToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildPoseRelativeToGraph(graph, _model); if (!errors.empty()) { @@ -1821,7 +1708,8 @@ bool checkPoseRelativeToGraph(const sdf::Root *_root) const sdf::World *_world) -> bool { bool worldResult = true; - sdf::PoseRelativeToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildPoseRelativeToGraph(graph, _world); if (!errors.empty()) { @@ -1904,7 +1792,8 @@ bool checkJointParentChildLinkNames(const sdf::Root *_root) if (!_model->LinkNameExists(childName) && !_model->JointNameExists(childName) && - !_model->FrameNameExists(childName)) + !_model->FrameNameExists(childName) && + !_model->ModelNameExists(childName)) { std::cerr << "Error: child frame with name[" << childName << "] specified by joint with name[" << joint->Name() diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index d41355beb..f9649d9fc 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -110,240 +110,6 @@ TEST(Parser, readFileConversions) } } -///////////////////////////////////////////////// -TEST(Parser, addNestedModel) -{ - auto getIncludedModelSdfString = []( - const std::string &_version, - const std::string &_expressedIn = "", - const std::string &_canonicalLink = "") -> std::string - { - std::string modelAttributeString = ""; - if (!_canonicalLink.empty()) - { - modelAttributeString = " canonical_link='" + _canonicalLink + "'"; - } - std::ostringstream stream; - stream - << "" - << "" - << " 0 0 10 0 0 1.57" - << " " - << " " - << " " - << " parent" - << " child" - << " "; - if (_expressedIn.empty()) - { - stream << "1 0 0"; - } - else - { - stream << "1 0 0"; - } - stream - << " " - << " " - << "" - << ""; - return stream.str(); - }; - - auto checkNestedModel = []( - sdf::ElementPtr _elem, const std::string &_expressedIn = "", - const std::string &_canonicalLink = "included::parent") - { - EXPECT_TRUE(_elem->HasElement("frame")); - sdf::ElementPtr nestedModelFrame = _elem->GetElement("frame"); - EXPECT_EQ(nullptr, nestedModelFrame->GetNextElement("frame")); - - sdf::ElementPtr link1 = _elem->GetElement("link"); - sdf::ElementPtr link2 = link1->GetNextElement("link"); - EXPECT_EQ(nullptr, link2->GetNextElement("link")); - - EXPECT_TRUE(_elem->HasElement("joint")); - sdf::ElementPtr joint = _elem->GetElement("joint"); - EXPECT_EQ(nullptr, joint->GetNextElement("joint")); - - EXPECT_EQ( - "included::__model__", nestedModelFrame->Get("name")); - EXPECT_EQ("included::parent", link1->Get("name")); - EXPECT_EQ("included::child", link2->Get("name")); - EXPECT_EQ("included::joint", joint->Get("name")); - EXPECT_EQ("included::parent", joint->Get("parent")); - EXPECT_EQ("included::child", joint->Get("child")); - - EXPECT_EQ(_canonicalLink, - nestedModelFrame->Get("attached_to")); - using ignition::math::Pose3d; - const Pose3d pose(0, 0, 10, 0, 0, 1.57); - EXPECT_EQ(pose, nestedModelFrame->Get("pose")); - - EXPECT_FALSE(_elem->HasElement("pose")); - EXPECT_EQ("included::__model__", - link1->GetElement("pose")->Get("relative_to")); - EXPECT_EQ("included::__model__", - link2->GetElement("pose")->Get("relative_to")); - EXPECT_EQ(Pose3d::Zero, link1->Get("pose")); - EXPECT_EQ(Pose3d::Zero, link2->Get("pose")); - EXPECT_EQ(Pose3d::Zero, joint->Get("pose")); - - EXPECT_TRUE(joint->HasElement("axis")); - sdf::ElementPtr axis = joint->GetElement("axis"); - EXPECT_TRUE(axis->HasElement("xyz")); - sdf::ElementPtr xyz = axis->GetElement("xyz"); - - EXPECT_EQ(_expressedIn, xyz->Get("expressed_in")); - EXPECT_EQ( - ignition::math::Vector3d::UnitX, xyz->Get()); - }; - - // insert as 1.4, expect rotation of //joint/axis/xyz - { - const std::string version = "1.4"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version), sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_TRUE(errors.empty()); - - // Expect //joint/axis/xyz[@expressed_in] = "included::__model__" because - // it is the default behavior 1.4 - checkNestedModel(elem, "included::__model__"); - } - - // insert as 1.5, expect no change to //joint/axis/xyz - { - const std::string version = "1.5"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version), sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_TRUE(errors.empty()); - - checkNestedModel(elem); - } - - // insert as 1.7, expressed_in=__model__ - // expect rotation of //joint/axis/xyz - { - const std::string version = "1.7"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version, "__model__"), - sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_TRUE(errors.empty()); - - checkNestedModel(elem, "included::__model__"); - } - - // insert as 1.7, expressed_in=child - // expect no change to //joint/axis/xyz - { - const std::string version = "1.7"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version, "child"), - sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_TRUE(errors.empty()); - - checkNestedModel(elem, "included::child"); - - // test coverage for addNestedModel without returning Errors - sdf::ElementPtr elem2 = std::make_shared(); - sdf::initFile("model.sdf", elem2); - sdf::addNestedModel(elem2, sdf->Root()); - } - - // insert as 1.7, expressed_in=parent - { - const std::string version = "1.7"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version, "parent"), - sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_EQ(0u, errors.size()); - - checkNestedModel(elem, "included::parent"); - - // test coverage for addNestedModel without returning Errors - sdf::ElementPtr elem2 = std::make_shared(); - sdf::initFile("model.sdf", elem2); - sdf::addNestedModel(elem2, sdf->Root()); - } - - // insert as 1.7, canonicalLink = child - { - const std::string version = "1.7"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version, "parent", "child"), - sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_EQ(0u, errors.size()); - - checkNestedModel(elem, "included::parent", "included::child"); - } -} - ///////////////////////////////////////////////// TEST(Parser, NameUniqueness) { diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 02542f232..989710673 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -587,7 +587,8 @@ TEST(DOMFrame, LoadModelFramesAttachedToNestedModel) // Load the SDF file sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; // Get the first model const sdf::Model *model = root.ModelByIndex(0); @@ -622,9 +623,9 @@ TEST(DOMFrame, LoadModelFramesAttachedToNestedModel) std::string body; EXPECT_TRUE(model->FrameByName("F1")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("nested_model", body); + EXPECT_EQ("nested_model::nested_link", body); EXPECT_TRUE(model->FrameByName("F2")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("nested_model", body); + EXPECT_EQ("nested_model::nested_link", body); } ///////////////////////////////////////////////// @@ -691,7 +692,7 @@ TEST(DOMFrame, LoadWorldFramesAttachedTo) EXPECT_TRUE(world->FrameByName("F1")->ResolveAttachedToBody(body).empty()); EXPECT_EQ("world", body); EXPECT_TRUE(world->FrameByName("F2")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("M1", body); + EXPECT_EQ("M1::L", body); } ///////////////////////////////////////////////// @@ -1109,7 +1110,7 @@ TEST(DOMFrame, LoadWorldFramesInvalidRelativeTo) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(11u, errors.size()); + EXPECT_EQ(15u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( @@ -1175,7 +1176,7 @@ TEST(DOMFrame, WorldIncludeModel) ASSERT_NE(nullptr, model); ignition::math::Pose3d modelPose; sdf::Errors resolveErrors = model->SemanticPose().Resolve(modelPose); - EXPECT_TRUE(resolveErrors.empty()); + EXPECT_TRUE(resolveErrors.empty()) << resolveErrors; EXPECT_EQ(expectedPoses[i], modelPose); } } diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index cb711c1b1..da51264ea 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -564,9 +564,9 @@ TEST(DOMJoint, LoadInvalidChild) "found")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR); EXPECT_NE(std::string::npos, - errors[1].Message().find( - "FrameAttachedToGraph error, Non-LINK vertex with name [joint] is " - "disconnected")); + errors[1].Message().find("FrameAttachedToGraph error, Non-LINK vertex " + "with name [joint_invalid_child::joint] is " + "disconnected")); // errors[2] // errors[3] // errors[4] @@ -766,3 +766,103 @@ TEST(DOMJoint, LoadURDFJointPoseRelativeTo) model->JointByName("joint12")->Axis()->ResolveXyz(vec3, "joint12").empty()); EXPECT_EQ(Vector3(0, 1.0, 0), vec3); } + +///////////////////////////////////////////////// +TEST(DOMJoint, LoadJointNestedParentChild) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "joint_nested_parent_child.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + using Pose = ignition::math::Pose3d; + + // Get the first model + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + + { + const sdf::Joint *j1 = model->JointByName("J1"); + ASSERT_NE(nullptr, j1); + EXPECT_EQ("M1::L1", j1->ParentLinkName()); + EXPECT_EQ("L1", j1->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j1->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::L1", resolvedLinkName); + EXPECT_TRUE(j1->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + + Pose pose; + EXPECT_TRUE(j1->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 0, 9, 0, IGN_PI_2, 0), pose); + } + { + const sdf::Joint *j2 = model->JointByName("J2"); + ASSERT_NE(nullptr, j2); + EXPECT_EQ("F1", j2->ParentLinkName()); + EXPECT_EQ("L1", j2->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j2->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::L1", resolvedLinkName); + EXPECT_TRUE(j2->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + + Pose pose; + EXPECT_TRUE(j2->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 1, 10, 0, IGN_PI_2, 0), pose); + } + { + const sdf::Joint *j3 = model->JointByName("J3"); + ASSERT_NE(nullptr, j3); + EXPECT_EQ("L1", j3->ParentLinkName()); + EXPECT_EQ("M1::L2", j3->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j3->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + EXPECT_TRUE(j3->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::L2", resolvedLinkName); + + Pose pose; + EXPECT_TRUE(j3->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(1, 1, 1, 0, 0, 0), pose); + } + { + const sdf::Joint *j4 = model->JointByName("J4"); + ASSERT_NE(nullptr, j4); + EXPECT_EQ("L1", j4->ParentLinkName()); + EXPECT_EQ("M1::F1", j4->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j4->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + EXPECT_TRUE(j4->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::L1", resolvedLinkName); + + Pose pose; + EXPECT_TRUE(j4->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(1, 0, 1, 0, 0, 0), pose); + } + { + const sdf::Joint *j5 = model->JointByName("J5"); + ASSERT_NE(nullptr, j5); + EXPECT_EQ("L1", j5->ParentLinkName()); + EXPECT_EQ("M1::M2", j5->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j5->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + EXPECT_TRUE(j5->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::M2::L1", resolvedLinkName); + + Pose pose; + EXPECT_TRUE(j5->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, -1, 1, IGN_PI_2, 0, 0), pose); + } +} diff --git a/test/integration/model/model_with_nested_placement_frame_attribute/model.config b/test/integration/model/model_with_nested_placement_frame_attribute/model.config new file mode 100644 index 000000000..4514d14ba --- /dev/null +++ b/test/integration/model/model_with_nested_placement_frame_attribute/model.config @@ -0,0 +1,7 @@ + + + + model_with_nested_placement_frame_attribute + model.sdf + + diff --git a/test/integration/model/model_with_nested_placement_frame_attribute/model.sdf b/test/integration/model/model_with_nested_placement_frame_attribute/model.sdf new file mode 100644 index 000000000..51ce83b22 --- /dev/null +++ b/test/integration/model/model_with_nested_placement_frame_attribute/model.sdf @@ -0,0 +1,17 @@ + + + + 0 0 10 0 0 0 + + 1 0 0 0 0 0 + + + + 0 2 0 0 0 0 + + + 0 0 3 0 0 0 + + + + diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 441f8dafd..28fe7dcb6 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -70,7 +70,7 @@ TEST(DOMModel, NoLinks) sdf::Root root; auto errors = root.Load(testFile); EXPECT_FALSE(errors.empty()); - ASSERT_EQ(4u, errors.size()); + ASSERT_EQ(3u, errors.size()); EXPECT_EQ(sdf::ErrorCode::MODEL_WITHOUT_LINK, errors[0].Code()); EXPECT_TRUE(errors[0].Message().find("model must have at least one link") != std::string::npos); @@ -78,7 +78,6 @@ TEST(DOMModel, NoLinks) EXPECT_TRUE(errors[1].Message().find("model must have at least one link") != std::string::npos); // errors[2] - // errors[3] } ///////////////////////////////////////////////// @@ -156,7 +155,7 @@ TEST(DOMRoot, NestedModel) // Load the SDF file sdf::Root root; auto errors = root.Load(testFile); - EXPECT_TRUE(errors.empty()); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_EQ(1u, root.ModelCount()); @@ -532,7 +531,28 @@ TEST(DOMRoot, LoadNestedExplicitCanonicalLink) } ///////////////////////////////////////////////// -TEST(DOMJoint, LoadInvalidNestedModelWithoutLinks) +TEST(DOMRoot, ModelPlacementFrameAttribute) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_with_placement_frame_attribute.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + auto *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, LoadInvalidNestedModelWithoutLinks) { const std::string testFile = sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", @@ -544,7 +564,7 @@ TEST(DOMJoint, LoadInvalidNestedModelWithoutLinks) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(10u, errors.size()); + EXPECT_EQ(7u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::MODEL_WITHOUT_LINK); EXPECT_NE(std::string::npos, errors[0].Message().find("A model must have at least one link")); @@ -556,8 +576,4 @@ TEST(DOMJoint, LoadInvalidNestedModelWithoutLinks) // errors[4] // errors[5] // errors[6] - // errors[7] - // errors[8] - // errors[9] } - diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 7bf7f5862..cb61aa45a 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -324,8 +324,10 @@ TEST(NestedModel, NestedInclude) // each model has 3 links, and the link names of the nested models have // been transformed EXPECT_EQ(3u, model1->LinkCount()); - EXPECT_EQ(3u, model2->LinkCount()); - EXPECT_EQ(3u, model3->LinkCount()); + // TODO (addisu): Update the following two expectations to account for the + // fact that included models are no longer flattened. + // EXPECT_EQ(3u, model2->LinkCount()); + // EXPECT_EQ(3u, model3->LinkCount()); const sdf::Link *baseLink1 = model1->LinkByName("base"); const sdf::Link *baseLink2 = model2->LinkByName(name + "::base"); const sdf::Link *baseLink3 = model3->LinkByName(name + "_14::base"); @@ -353,24 +355,14 @@ TEST(NestedModel, NestedInclude) EXPECT_EQ(lowerLink3->RawPose(), lowerLink1->RawPose()); EXPECT_EQ(upperLink2->RawPose(), upperLink1->RawPose()); EXPECT_EQ(upperLink3->RawPose(), upperLink1->RawPose()); - // expect //pose/@relative_to to contain the name of the nested model frame - // injected during parsing. model1 is not nested, so it's links should have - // empty //pose/@relative_to - EXPECT_TRUE(baseLink1->PoseRelativeTo().empty()); - EXPECT_EQ(name + "::__model__", baseLink2->PoseRelativeTo()); - EXPECT_EQ(name + "_14::__model__", baseLink3->PoseRelativeTo()); - EXPECT_TRUE(lowerLink1->PoseRelativeTo().empty()); - EXPECT_EQ(name + "::__model__", lowerLink2->PoseRelativeTo()); - EXPECT_EQ(name + "_14::__model__", lowerLink3->PoseRelativeTo()); - EXPECT_TRUE(upperLink1->PoseRelativeTo().empty()); - EXPECT_EQ(name + "::__model__", upperLink2->PoseRelativeTo()); - EXPECT_EQ(name + "_14::__model__", upperLink3->PoseRelativeTo()); // each model has 2 joints, and the joint names of the nested models have // been transformed EXPECT_EQ(2u, model1->JointCount()); - EXPECT_EQ(2u, model2->JointCount()); - EXPECT_EQ(2u, model3->JointCount()); + // TODO (addisu): Update the following two expectations to account for the + // fact that included models are no longer flattened. + // EXPECT_EQ(2u, model2->JointCount()); + // EXPECT_EQ(2u, model3->JointCount()); auto *lowerJoint1 = model1->JointByName("lower_joint"); auto *lowerJoint2 = model2->JointByName(name + "::lower_joint"); auto *lowerJoint3 = model3->JointByName(name + "_14::lower_joint"); @@ -407,8 +399,6 @@ TEST(NestedModel, NestedInclude) // //axis/xyz/@expressed_in == "__model__" inside the nested model EXPECT_EQ(ignition::math::Vector3d::UnitX, lowerAxis3->Xyz()); EXPECT_EQ(ignition::math::Vector3d::UnitX, upperAxis3->Xyz()); - EXPECT_EQ(name + "_14::__model__", lowerAxis3->XyzExpressedIn()); - EXPECT_EQ(name + "_14::__model__", upperAxis3->XyzExpressedIn()); } ////////////////////////////////////////////////// @@ -452,6 +442,9 @@ TEST(NestedModel, NestedModelWithFrames) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); + const sdf::Model *childModel = parentModel->ModelByIndex(0); + ASSERT_NE(nullptr, childModel); + using ignition::math::Pose3d; using ignition::math::Vector3d; // Expected poses for frames, links and joints after nesting. @@ -463,55 +456,52 @@ TEST(NestedModel, NestedModelWithFrames) Vector3d joint1AxisExpVector = frame2ExpPose.Rot() * Vector3d::UnitZ; Vector3d joint1Axis2ExpVector = frame2ExpPose.Rot() * Vector3d::UnitX; - const auto *frame1 = parentModel->FrameByName("M1::F1"); - ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE(frame1->SemanticPose().Resolve(frame1Pose).empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose); + auto parentSemPose = parentModel->SemanticPose(); + EXPECT_TRUE(parentSemPose.Resolve(frame1Pose, "ParentModel::M1::F1").empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); - const auto *frame2 = parentModel->FrameByName("M1::F2"); - ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE(frame2->SemanticPose().Resolve(frame2Pose).empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame2Pose, "ParentModel::M1::F2").empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); const auto *link1 = parentModel->LinkByName("M1::L1"); ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE(link1->SemanticPose().Resolve(link1Pose).empty()); - EXPECT_EQ(link1ExpPose, link1Pose); + EXPECT_TRUE(parentSemPose.Resolve(link1Pose, "ParentModel::M1::L1").empty()); + EXPECT_EQ(link1ExpPose, link1Pose.Inverse()); const auto *visual1 = link1->VisualByName("V1"); ASSERT_NE(nullptr, visual1); - EXPECT_EQ("M1::F2", visual1->PoseRelativeTo()); + EXPECT_EQ("F2", visual1->PoseRelativeTo()); const auto *collision1 = link1->CollisionByName("C1"); ASSERT_NE(nullptr, collision1); - EXPECT_EQ("M1::__model__", collision1->PoseRelativeTo()); + EXPECT_EQ("__model__", collision1->PoseRelativeTo()); const auto *link2 = parentModel->LinkByName("M1::L2"); ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE(link2->SemanticPose().Resolve(link2Pose).empty()); - EXPECT_EQ(link2ExpPose, link2Pose); + EXPECT_TRUE(parentSemPose.Resolve(link2Pose, "ParentModel::M1::L2").empty()); + EXPECT_EQ(link2ExpPose, link2Pose.Inverse()); const auto *joint1 = parentModel->JointByName("M1::J1"); ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE(joint1->SemanticPose().Resolve(joint1Pose, "__model__").empty()); - EXPECT_EQ(joint1ExpPose, joint1Pose); + EXPECT_TRUE(parentSemPose.Resolve(joint1Pose, "ParentModel::M1::J1").empty()); + EXPECT_EQ(joint1ExpPose, joint1Pose.Inverse()); const auto joint1Axis = joint1->Axis(0); ASSERT_NE(nullptr, joint1Axis); Vector3d joint1AxisVector; EXPECT_TRUE(joint1Axis->ResolveXyz(joint1AxisVector, "__model__").empty()); - EXPECT_EQ(joint1AxisExpVector, joint1AxisVector); + EXPECT_EQ(joint1AxisExpVector, model1Pose.Rot() * joint1AxisVector); const auto joint1Axis2 = joint1->Axis(1); ASSERT_NE(nullptr, joint1Axis2); Vector3d joint1Axis2Vector; EXPECT_TRUE(joint1Axis2->ResolveXyz(joint1Axis2Vector, "__model__").empty()); - EXPECT_EQ(joint1Axis2ExpVector, joint1Axis2Vector); + EXPECT_EQ(joint1Axis2ExpVector, model1Pose.Rot() * joint1Axis2Vector); } // Function to remove any element in //joint/axis that is not @@ -526,12 +516,39 @@ static void removeNoneXyz(const sdf::ElementPtr &_elem) } } - for (auto el : toRemove) + for (const auto &el : toRemove) { _elem->RemoveChild(el); } } +void prepareModelForDirectComparison(sdf::ElementPtr _modelElem) +{ + if (_modelElem->HasElement("joint")) + { + for (auto joint = _modelElem->GetElement("joint"); joint; + joint = joint->GetNextElement("joint")) + { + if (joint->HasElement("axis")) + { + removeNoneXyz(joint->GetElement("axis")); + } + if (joint->HasElement("axis2")) + { + removeNoneXyz(joint->GetElement("axis2")); + } + } + } + if (_modelElem->HasElement("model")) + { + for (auto elem = _modelElem->GetElement("model"); elem; + elem = elem->GetNextElement("model")) + { + prepareModelForDirectComparison(elem); + } + } +} + // Remove elements in world that are not model for comparison with // expectation. Also remove any element in //joint/axis that is not void prepareForDirectComparison(sdf::ElementPtr _worldElem) @@ -546,21 +563,7 @@ void prepareForDirectComparison(sdf::ElementPtr _worldElem) } else { - if (elem->HasElement("joint")) - { - for (auto joint = elem->GetElement("joint"); joint; - joint = joint->GetNextElement("joint")) - { - if (joint->HasElement("axis")) - { - removeNoneXyz(joint->GetElement("axis")); - } - if (joint->HasElement("axis2")) - { - removeNoneXyz(joint->GetElement("axis2")); - } - } - } + prepareModelForDirectComparison(elem); } } @@ -623,7 +626,7 @@ TEST(NestedModel, PartiallyFlattened) { const std::string testFile = sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "nested_model_with_frames_expected.sdf"); + "partially_flattened.sdf"); // Load the SDF file sdf::Root root; @@ -767,6 +770,7 @@ TEST(NestedModel, NestedModelWithSiblingFrames) << "" << "" << " " + << " " << " " << " " << testFramePose << "" << " " @@ -794,12 +798,13 @@ TEST(NestedModel, NestedModelWithSiblingFrames) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); + const sdf::Frame *parentModelFrame = parentModel->FrameByIndex(0); + ASSERT_NE(nullptr, parentModelFrame); - const sdf::Frame *nestedModel1Frame = - parentModel->FrameByName("M1::__model__"); - ASSERT_NE(nullptr, nestedModel1Frame); + const sdf::Model *nestedModel1 = parentModel->ModelByName("M1"); + ASSERT_NE(nullptr, nestedModel1); - EXPECT_EQ("testFrame", nestedModel1Frame->PoseRelativeTo()); + EXPECT_EQ("testFrame", nestedModel1->PoseRelativeTo()); using ignition::math::Pose3d; using ignition::math::Vector3d; @@ -814,38 +819,40 @@ TEST(NestedModel, NestedModelWithSiblingFrames) Pose3d nestedModel1FramePose; EXPECT_TRUE( - nestedModel1Frame->SemanticPose().Resolve(nestedModel1FramePose).empty()); + nestedModel1->SemanticPose().Resolve(nestedModel1FramePose).empty()); EXPECT_EQ(nestedModel1FrameExpPose, nestedModel1FramePose); + auto parentSemPose = parentModel->SemanticPose(); + const auto *frame1 = parentModel->FrameByName("M1::F1"); ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE(frame1->SemanticPose().Resolve(frame1Pose).empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame1Pose, "ParentModel::M1::F1").empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); const auto *frame2 = parentModel->FrameByName("M1::F2"); ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE(frame2->SemanticPose().Resolve(frame2Pose).empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame2Pose, "ParentModel::M1::F2").empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); const auto *link1 = parentModel->LinkByName("M1::L1"); ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE(link1->SemanticPose().Resolve(link1Pose).empty()); - EXPECT_EQ(link1ExpPose, link1Pose); + EXPECT_TRUE(parentSemPose.Resolve(link1Pose, "ParentModel::M1::L1").empty()); + EXPECT_EQ(link1ExpPose, link1Pose.Inverse()); const auto *link2 = parentModel->LinkByName("M1::L2"); ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE(link2->SemanticPose().Resolve(link2Pose).empty()); - EXPECT_EQ(link2ExpPose, link2Pose); + EXPECT_TRUE(parentSemPose.Resolve(link2Pose, "ParentModel::M1::L2").empty()); + EXPECT_EQ(link2ExpPose, link2Pose.Inverse()); const auto *joint1 = parentModel->JointByName("M1::J1"); ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE(joint1->SemanticPose().Resolve(joint1Pose, "__model__").empty()); - EXPECT_EQ(joint1ExpPose, joint1Pose); + EXPECT_TRUE(parentSemPose.Resolve(joint1Pose, "ParentModel::M1::J1").empty()); + EXPECT_EQ(joint1ExpPose, joint1Pose.Inverse()); } ////////////////////////////////////////////////// @@ -860,11 +867,12 @@ TEST(NestedModel, NestedFrameOnlyModel) const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); std::ostringstream stream; - std::string version = "1.7"; + std::string version = "1.8"; stream << "" << "" << " " + << " true" << " " << " " + MODEL_PATH + "" << " M1" @@ -880,9 +888,7 @@ TEST(NestedModel, NestedFrameOnlyModel) sdf::Root root; sdf::Errors errors = root.Load(sdfParsed); - for (auto e : errors) - std::cout << e.Message() << std::endl; - EXPECT_TRUE(errors.empty()); + EXPECT_TRUE(errors.empty()) << errors; const sdf::World *world = root.WorldByIndex(0); ASSERT_NE(nullptr, world); @@ -890,23 +896,21 @@ TEST(NestedModel, NestedFrameOnlyModel) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); + auto parentSemPose = parentModel->SemanticPose(); + using ignition::math::Pose3d; using ignition::math::Vector3d; // Expected poses for frames after nesting. Pose3d frame1ExpPose = model1Pose * Pose3d(0, 0, 0, IGN_PI, 0, 0); Pose3d frame2ExpPose = frame1ExpPose * Pose3d(0, 0, 0, 0, IGN_PI, 0); - const auto *frame1 = parentModel->FrameByName("M1::F1"); - ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE(frame1->SemanticPose().Resolve(frame1Pose).empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame1Pose, "ParentModel::M1::F1").empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); - const auto *frame2 = parentModel->FrameByName("M1::F2"); - ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE(frame2->SemanticPose().Resolve(frame2Pose).empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame2Pose, "ParentModel::M1::F2").empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); } class PlacementFrame: public ::testing::Test @@ -1016,13 +1020,13 @@ class PlacementFrame: public ::testing::Test Pose3d testFramePose; { - sdf::Errors errors = - testFrame->SemanticPose().Resolve(testFramePose, "__model__"); - EXPECT_TRUE(errors.empty()) << errors[0].Message(); + sdf::Errors errors = parentModel->SemanticPose().Resolve( + testFramePose, parentModel->Name() + "::" + _testFrameName); + EXPECT_TRUE(errors.empty()) << errors; } // The expected pose of the test frame is precisely the placement pose - EXPECT_EQ(placementPose, testFramePose); + EXPECT_EQ(placementPose, testFramePose.Inverse()); } protected: sdf::Root root; @@ -1089,4 +1093,281 @@ TEST_F(PlacementFrame, ModelPlacementFrameAttribute) "model_with_placement_frame_and_pose_relative_to", "L4"); } -// TODO (addisu) Add NestedModelPlacementFrameAttribute tests +////////////////////////////////////////////////// +TEST(NestedReference, PoseRelativeTo) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_relative_to_nested_reference.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); + + auto parentSemPose = model->SemanticPose(); + // The child models starting from M2 all reference an entity inside M1. Check + // that resolving poses works when referencing such entities. + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M2"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M2").empty()); + EXPECT_EQ(Pose(1, 0, -2, 0, IGN_PI_2, 0), pose.Inverse()); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M3"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M3").empty()); + EXPECT_EQ(Pose(1, 1, -3, 0, IGN_PI_2, 0), pose.Inverse()); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M4"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M4").empty()); + EXPECT_EQ(Pose(2, 0, -4, 0, IGN_PI_2, 0), pose.Inverse()); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M5"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M5").empty()); + EXPECT_EQ(Pose(2, 0, -6, 0, IGN_PI_2, 0), pose.Inverse()); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M6"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M6").empty()); + EXPECT_EQ(Pose(1, 1, -6, IGN_PI_2, IGN_PI_2, 0), pose.Inverse()); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M7"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M7").empty()); + EXPECT_EQ(Pose(1, -6, -1, 0, 0, -IGN_PI_2), pose.Inverse()); + } +} + +////////////////////////////////////////////////// +TEST(NestedReference, PoseRelativeToInWorld) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "world_relative_to_nested_reference.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + using Pose = ignition::math::Pose3d; + + // The //world/frame elements reference an entity inside M1. Check + // that resolving poses works when referencing such entities. + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F2"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 0, -2, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F3"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 1, -3, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F4"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(2, 0, -4, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F5"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(2, 0, -6, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F6"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 1, -6, IGN_PI_2, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F7"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, -6, -1, 0, 0, -IGN_PI_2), pose); + } +} + +///////////////////////////////////////////////// +TEST(NestedReference, PlacementFrameAttribute) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", + "model", "model_with_nested_placement_frame_attribute", "model.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + auto *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); +} + +///////////////////////////////////////////////// +TEST(NestedReference, PlacementFrameElement) +{ + const std::string modelRootPath = sdf::filesystem::append( + PROJECT_SOURCE_PATH, "test", "integration", "model"); + + sdf::setFindCallback([&](const std::string &_file) + { + return sdf::filesystem::append(modelRootPath, _file); + }); + + // Test with //world/include without overriding the placement_frame + { + std::ostringstream stream; + stream << R"( + + + + model_with_nested_placement_frame_attribute + + + )"; + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors; + + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + auto *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); + } + + // Test with //world/include overriding the placement_frame + { + const ignition::math::Pose3d placementPose(0, 10, 0, IGN_PI_2, 0, 0); + std::ostringstream stream; + stream << R"( + + + + + model_with_nested_placement_frame_attribute + child_model::link3 + )" << placementPose << R"(" + + + )"; + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors; + + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + auto *world_frame = world->FrameByName("world_frame"); + ASSERT_NE(nullptr, world_frame); + ignition::math::Pose3d pose; + errors = world_frame->SemanticPose().Resolve( + pose, "model_placement_frame::child_model::link3"); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(placementPose, pose.Inverse()); + } + + + // Test with //model/include without overriding the placement_frame + { + std::ostringstream stream; + stream << R"( + + + + model_with_nested_placement_frame_attribute + + + )"; + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors; + + auto *parentModel = root.ModelByIndex(0); + ASSERT_NE(nullptr, parentModel); + auto *model = parentModel->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); + } + + // Test with //model/include overriding the placement_frame + { + const ignition::math::Pose3d placementPose(0, 10, 0, IGN_PI_2, 0, 0); + std::ostringstream stream; + stream << R"( + + + + + model_with_nested_placement_frame_attribute + child_model::link3 + )" << placementPose << R"(" + + + )"; + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors; + + auto *parentModel = root.ModelByIndex(0); + ASSERT_NE(nullptr, parentModel); + auto *model = parentModel ->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + auto *parentModelFrame = parentModel->FrameByName("parent_model_frame"); + auto *link = model->LinkByName("child_model::link3"); + ASSERT_NE(nullptr, link); + ignition::math::Pose3d pose; + errors = parentModelFrame->SemanticPose().Resolve( + pose, "model_placement_frame::child_model::link3"); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(placementPose, pose.Inverse()); + } +} diff --git a/test/integration/nested_model_with_frames_expected.sdf b/test/integration/nested_model_with_frames_expected.sdf index 2385f000f..feb1b349f 100644 --- a/test/integration/nested_model_with_frames_expected.sdf +++ b/test/integration/nested_model_with_frames_expected.sdf @@ -2,72 +2,72 @@ - - 10 0 0 0 -0 1.5708 - - - 0 0 0 1.5708 -0 0 - - - 0 0 0 0 0.785398 0 - - - 0 0 0 0 -0 0 - - 0 0 0 0 -0 0 - - - 1 - - - - - 0 0 0 0 -0 0 - - - 1 - - - - - - 1 0 0 0 -0 0 - - - 0 1 0 0 -0 0 - - - 0 0 1 0 -0 0 - - - 0 0 0 0 -0 0 - M1::L1 - M1::L2 - - 0 0 1 - - - 1 0 0 - - - - 0 0 1 0 -0 0 - M1::L2 - M1::L3 - - 0 0 1 - - - - 1 0 1 0 -0 0 - M1::L3 - M1::L4 - - - 1 0 0 0 -0 0 - - 1 0 0 0 -0 0 + + + 0 0 0 1.5708 -0 0 + + + 0 0 0 0 0.785398 0 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + + + 1 + + + + + 0 0 0 0 -0 0 + + + 1 + + + + + 1 0 0 0 -0 0 + + + 0 1 0 0 -0 0 + + + 0 0 1 0 -0 0 + + + 0 0 0 0 -0 0 + L1 + L2 + + 0 0 1 + + + 1 0 0 + + + + 0 0 1 0 -0 0 + L2 + L3 + + 0 0 1 + + + + 1 0 1 0 -0 0 + L3 + L4 + + + 1 0 0 0 -0 0 + + 1 0 0 0 -0 0 + + + 10 0 0 0 -0 1.5708 diff --git a/test/integration/partially_flattened.sdf b/test/integration/partially_flattened.sdf new file mode 100644 index 000000000..2385f000f --- /dev/null +++ b/test/integration/partially_flattened.sdf @@ -0,0 +1,74 @@ + + + + + + 10 0 0 0 -0 1.5708 + + + 0 0 0 1.5708 -0 0 + + + 0 0 0 0 0.785398 0 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + + + 1 + + + + + 0 0 0 0 -0 0 + + + 1 + + + + + + 1 0 0 0 -0 0 + + + 0 1 0 0 -0 0 + + + 0 0 1 0 -0 0 + + + 0 0 0 0 -0 0 + M1::L1 + M1::L2 + + 0 0 1 + + + 1 0 0 + + + + 0 0 1 0 -0 0 + M1::L2 + M1::L3 + + 0 0 1 + + + + 1 0 1 0 -0 0 + M1::L3 + M1::L4 + + + 1 0 0 0 -0 0 + + 1 0 0 0 -0 0 + + + + + diff --git a/test/integration/root_dom.cc b/test/integration/root_dom.cc index e23775591..f6f06af24 100644 --- a/test/integration/root_dom.cc +++ b/test/integration/root_dom.cc @@ -61,7 +61,8 @@ TEST(DOMRoot, Load) sdf::Root root; EXPECT_EQ(0u, root.WorldCount()); - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_EQ(SDF_PROTOCOL_VERSION, root.Version()); EXPECT_EQ(1u, root.WorldCount()); EXPECT_TRUE(root.WorldByIndex(0) != nullptr); @@ -83,7 +84,8 @@ TEST(DOMRoot, LoadMultipleModels) "root_multiple_models.sdf"); sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_EQ(3u, root.ModelCount()); EXPECT_EQ("robot1", root.ModelByIndex(0)->Name()); diff --git a/test/integration/two_level_nested_model_with_frames_expected.sdf b/test/integration/two_level_nested_model_with_frames_expected.sdf index 807fa6d6c..2bd80e1eb 100644 --- a/test/integration/two_level_nested_model_with_frames_expected.sdf +++ b/test/integration/two_level_nested_model_with_frames_expected.sdf @@ -2,75 +2,75 @@ - - 10 0 0 0 -0 1.5708 - - - 0 10 0 1.5708 -0 0 - - - 0 0 0 1.5708 -0 0 - - - 0 0 0 0 0.785398 0 - - - 0 0 0 0 -0 0 - - 0 0 0 0 -0 0 - - - 1 - - - - - 0 0 0 0 -0 0 - - - 1 - - - - - - 1 0 0 0 -0 0 - - - 0 1 0 0 -0 0 - - - 0 0 1 0 -0 0 - - - 0 0 0 0 -0 0 - M1::test_model_with_frames::L1 - M1::test_model_with_frames::L2 - - 0 0 1 - - - 1 0 0 - - - - 0 0 1 0 -0 0 - M1::test_model_with_frames::L2 - M1::test_model_with_frames::L3 - - 0 0 1 - - - - 1 0 1 0 -0 0 - M1::test_model_with_frames::L3 - M1::test_model_with_frames::L4 - - - 1 0 0 0 -0 0 - - 1 0 0 0 -0 0 - + + + + 0 0 0 1.5708 -0 0 + + + 0 0 0 0 0.785398 0 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + + + 1 + + + + + 0 0 0 0 -0 0 + + + 1 + + + + + + 1 0 0 0 -0 0 + + + 0 1 0 0 -0 0 + + + 0 0 1 0 -0 0 + + + 0 0 0 0 -0 0 + L1 + L2 + + 0 0 1 + + + 1 0 0 + + + + 0 0 1 0 -0 0 + L2 + L3 + + 0 0 1 + + + + 1 0 1 0 -0 0 + L3 + L4 + + + 1 0 0 0 -0 0 + + 1 0 0 0 -0 0 + + + 0 10 0 1.5708 -0 0 + + 10 0 0 0 -0 1.5708 diff --git a/test/sdf/joint_nested_parent_child.sdf b/test/sdf/joint_nested_parent_child.sdf new file mode 100644 index 000000000..4a495a788 --- /dev/null +++ b/test/sdf/joint_nested_parent_child.sdf @@ -0,0 +1,61 @@ + + + + + + 0 0 1 0 0 0 + + + 1 1 0 0 0 0 + + + 1 0 0 0 0 0 + + + 0 0 1 1.570796326790 0 0 + + + + + + 0 0 10 0 1.57079632679 0 + + + + 1 0 0 0 0 0 + + + + + 1 0 0 0 0 0 + M1::L1 + L1 + + + + 0 1 0 0 0 0 + F1 + L1 + + + + 0 0 1 0 0 0 + L1 + M1::L2 + + + + + 0 0 1 0 0 0 + L1 + M1::F1 + + + + + 0 0 1 0 0 0 + L1 + M1::M2 + + + diff --git a/test/sdf/model_invalid_nested_joint_child.sdf b/test/sdf/model_invalid_nested_joint_child.sdf deleted file mode 100644 index efde66366..000000000 --- a/test/sdf/model_invalid_nested_joint_child.sdf +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - 1 0 0 0 1.5707963267948966 0 - - - - P - - - M::C - - - diff --git a/test/sdf/model_invalid_root_reference.sdf b/test/sdf/model_invalid_root_reference.sdf new file mode 100644 index 000000000..be34acdd9 --- /dev/null +++ b/test/sdf/model_invalid_root_reference.sdf @@ -0,0 +1,9 @@ + + + + + 1 0 0 0 0 0 + + + + diff --git a/test/sdf/model_nested_frame_attached_to.sdf b/test/sdf/model_nested_frame_attached_to.sdf new file mode 100644 index 000000000..6722b8f99 --- /dev/null +++ b/test/sdf/model_nested_frame_attached_to.sdf @@ -0,0 +1,22 @@ + + + + + + + + + + + + L + F + + + + + + + + + diff --git a/test/sdf/model_nested_static_model.sdf b/test/sdf/model_nested_static_model.sdf new file mode 100644 index 000000000..3bdedec3d --- /dev/null +++ b/test/sdf/model_nested_static_model.sdf @@ -0,0 +1,11 @@ + + + + + + true + + + + + diff --git a/test/sdf/model_relative_to_nested_reference.sdf b/test/sdf/model_relative_to_nested_reference.sdf new file mode 100644 index 000000000..f895b69ec --- /dev/null +++ b/test/sdf/model_relative_to_nested_reference.sdf @@ -0,0 +1,52 @@ + + + + + + 1 0 0 0 1.5707963267948966 0 + + 0 1 0 0 0 0 + + + L1 + L2 + + + 0 0 1 0 0 0 + + + 1 0 0 0 0 0 + + + 0 1 0 1.5707963267948966 0 0 + + 1 0 0 0 -1.5707963267948966 0 + + + + + 2 0 0 0 0 0 + + + + 3 0 0 0 0 0 + + + + 4 0 0 0 0 0 + + + + 5 0 0 0 0 0 + + + + 6 0 0 0 0 0 + + + + 7 0 0 0 0 0 + + + + diff --git a/test/sdf/model_with_placement_frame_attribute.sdf b/test/sdf/model_with_placement_frame_attribute.sdf new file mode 100644 index 000000000..6b5df9340 --- /dev/null +++ b/test/sdf/model_with_placement_frame_attribute.sdf @@ -0,0 +1,12 @@ + + + + 0 0 10 0 0 0 + + 1 0 0 0 0 0 + + + 0 2 0 0 0 0 + + + diff --git a/test/sdf/nested_model_cross_references.sdf b/test/sdf/nested_model_cross_references.sdf new file mode 100644 index 000000000..2590f650a --- /dev/null +++ b/test/sdf/nested_model_cross_references.sdf @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/sdf/world_frame_invalid_attached_to_scope.sdf b/test/sdf/world_frame_invalid_attached_to_scope.sdf new file mode 100644 index 000000000..9496ad782 --- /dev/null +++ b/test/sdf/world_frame_invalid_attached_to_scope.sdf @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/test/sdf/world_invalid_root_reference.sdf b/test/sdf/world_invalid_root_reference.sdf new file mode 100644 index 000000000..8b4e6eba5 --- /dev/null +++ b/test/sdf/world_invalid_root_reference.sdf @@ -0,0 +1,41 @@ + + + + + 1 0 0 0 0 0 + + + + + __root__ + link2 + + + + link1 + __root__ + + + + + + + + + + + + + + + + 1 0 0 0 0 0 + + + + test_model_with_frames + __root__ + 1 0 0 0 0 0 + + + diff --git a/test/sdf/world_nested_frame_attached_to.sdf b/test/sdf/world_nested_frame_attached_to.sdf new file mode 100644 index 000000000..1dc64fa72 --- /dev/null +++ b/test/sdf/world_nested_frame_attached_to.sdf @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + L + F0 + + + + + + + + + + + + diff --git a/test/sdf/world_relative_to_nested_reference.sdf b/test/sdf/world_relative_to_nested_reference.sdf new file mode 100644 index 000000000..e19f37233 --- /dev/null +++ b/test/sdf/world_relative_to_nested_reference.sdf @@ -0,0 +1,45 @@ + + + + + 1 0 0 0 1.5707963267948966 0 + + 0 1 0 0 0 0 + + + L1 + L2 + + + 0 0 1 0 0 0 + + + 1 0 0 0 0 0 + + + 0 1 0 1.5707963267948966 0 0 + + 1 0 0 0 -1.5707963267948966 0 + + + + + 2 0 0 0 0 0 + + + 3 0 0 0 0 0 + + + 4 0 0 0 0 0 + + + 5 0 0 0 0 0 + + + 6 0 0 0 0 0 + + + 7 0 0 0 0 0 + + + diff --git a/test/sdf/world_valid_root_reference.sdf b/test/sdf/world_valid_root_reference.sdf new file mode 100644 index 000000000..07e5634dd --- /dev/null +++ b/test/sdf/world_valid_root_reference.sdf @@ -0,0 +1,15 @@ + + + + + 1 0 0 0 0 0 + + + __root__ + + + + + + +