New pose proposal: support rotation in degrees (#252) (#589)

* Added rotation type attribute to pose, handled parsing in param Signed-off-by: Aaron Chong <[email protected]> * Using degrees boolean attribute in pose instead, added tests and test sdf Signed-off-by: Aaron Chong <[email protected]> * Fix code check Signed-off-by: Aaron Chong <[email protected]> * Changed attribute to @rotation_type, modified tests, added delimiters Signed-off-by: Aaron Chong <[email protected]> * Passing attributes into param value parsing, reverted Utils loadPose function Signed-off-by: Aaron Chong <[email protected]> * Reverted location of parsing nested sdf Signed-off-by: Aaron Chong <[email protected]> * Added parent element for param, more tests Signed-off-by: Aaron Chong <[email protected]> * Const ref for GetAttributes and it's relevant calls Signed-off-by: Aaron Chong <[email protected]> * Reverting clone and copy behavior without reallocating memory, using make_shared Signed-off-by: Aaron Chong <[email protected]> * Compared parent element pointers instead of just the names Signed-off-by: Aaron Chong <[email protected]> * Removed redundant reset, checked for null parent before printing error Signed-off-by: Aaron Chong <[email protected]> * Moved tests from integration/param_parent_element to Param_TEST Signed-off-by: Aaron Chong <[email protected]> * Reverting urdf.sdf, adding new pose_without_frames.sdf, using that in actor and inertial Signed-off-by: Aaron Chong <[email protected]> * Fixed typo, added explicit named default variable, made rotation type errors clearer Signed-off-by: Aaron Chong <[email protected]> * Added comment in pose_1_9.sdf that each pose is independent Signed-off-by: Aaron Chong <[email protected]> * Removed use of parsing by string, modified tests to reflect changed behavior Signed-off-by: Aaron Chong <[email protected]> * Reverted changes made to schema for poses that do not handle relative_to frames Signed-off-by: Aaron Chong <[email protected]> * ComCompare degrees more explicitly in tests instead of using IGN_DTOR Signed-off-by: Aaron Chong <[email protected]> * Using consistent notation for reporting rotation_type attribute, modifying test Signed-off-by: Aaron Chong <[email protected]> * Using instead of typedef for forward declaration, error strings include full value Signed-off-by: Aaron Chong <[email protected]> * Using RedirectConsoleStream and ScopeExit, using Root and Root::LoadSdfString in tests Signed-off-by: Aaron Chong <[email protected]> * Added Reparse function for Param, relevant tests regarding rotation_type manipulation, cleaned up Signed-off-by: Aaron Chong <[email protected]> * Reducing changes to just the degrees attribute, modifying tests because of behavior changes in Set Signed-off-by: Aaron Chong <[email protected]> * Fixed stale brief and comment, breaking for loops early Signed-off-by: Aaron Chong <[email protected]> * Changed 'required' of @relative_to and @Degrees to 0, and modified description of pose to be more explicit Signed-off-by: Aaron Chong <[email protected]> * Modified reparsing tests in Param_TEST, added another test using Pose Signed-off-by: Aaron Chong <[email protected]> * Using alias Pose instead of full type name, added check for parent element pointer, fixed typo Signed-off-by: Aaron Chong <[email protected]> * Support printing pose elements in degrees, added tests Signed-off-by: Aaron Chong <[email protected]> * Removed const for SetParentElement Signed-off-by: Aaron Chong <[email protected]> * Adding additional documentation for the behavior between Set<T> and Reparse Signed-off-by: Aaron Chong <[email protected]> * Storing parsed tokens in array instead of vector Signed-off-by: Aaron Chong <[email protected]> * Used explicit Get<boo> for degrees attribute Signed-off-by: Aaron Chong <[email protected]> * Fixed missing array include for windows build Signed-off-by: Aaron Chong <[email protected]> * Check for valueIndex, instead of array size Signed-off-by: Aaron Chong <[email protected]>
gazebosim · Sep 2, 2021 · 96ac74e · 96ac74e
1 parent c919d23
commit 96ac74e
Show file tree

Hide file tree

Showing 16 changed files with 1,237 additions and 139 deletions.
diff --git a/include/sdf/Element.hh b/include/sdf/Element.hh
@@ -243,6 +243,10 @@ namespace sdf
     /// \return The number of attributes.
     public: size_t GetAttributeCount() const;
 
+    /// \brief Get all the attribute params.
+    /// \return The vector of parameter attributes.
+    public: const Param_V &GetAttributes() const;
+
     /// \brief Get an attribute using an index.
     /// \param[in] _index the index of the attribute to get.
     /// \return A Param pointer to the attribute.

diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh
@@ -53,6 +53,10 @@ namespace sdf
   inline namespace SDF_VERSION_NAMESPACE {
   //
 
+  class SDFORMAT_VISIBLE Element;
+  using ElementPtr = std::shared_ptr<Element>;
+  using ElementWeakPtr = std::weak_ptr<Element>;
+
   class SDFORMAT_VISIBLE Param;
 
   /// \def ParamPtr
@@ -180,13 +184,44 @@ namespace sdf
     /// of the parameter. nullopt otherwise.
     public: std::optional<std::string> GetMaxValueAsString() const;
 
+    /// \brief Set the parameter value from a string.
+    /// \param[in] _value New value for the parameter in string form.
+    /// \param[in] _ignoreParentAttributes Whether to ignore parent element
+    /// attributes when parsing value from string as well as subsequent
+    /// reparses.
+    public: bool SetFromString(const std::string &_value,
+                               bool _ignoreParentAttributes);
+
     /// \brief Set the parameter value from a string.
     /// \param[in] _value New value for the parameter in string form.
     public: bool SetFromString(const std::string &_value);
 
+    /// \brief Get the parent Element of this Param.
+    /// \return Pointer to this Param's parent Element, nullptr if there is no
+    /// parent Element.
+    public: ElementPtr GetParentElement() const;
+
+    /// \brief Set the parent Element of this Param.
+    /// \param[in] _parentElement Pointer to new parent Element. A nullptr can
+    /// provided to remove the current parent Element.
+    public: void SetParentElement(ElementPtr _parentElement);
+
     /// \brief Reset the parameter to the default value.
     public: void Reset();
 
+    /// \brief Reparse the parameter value. This should be called after the
+    /// parent element's attributes have been modified, in the event that the
+    /// value was set using SetFromString or posesses a default value, and that
+    /// the final parsed value is dependent on the attributes of the parent
+    /// element. For example, the rotation component of a pose element can
+    /// be parsed as degrees or radians, depending on the attribute @degrees
+    /// of the parent element. If however the value was explicitly set using the
+    /// Set<T> function, reparsing would not change the value.
+    /// \return True if the parameter value has been reparsed successfully.
+    /// \sa bool SetFromString(const std::string &_value)
+    /// \sa bool Set(const T &_value)
+    public: bool Reparse();
+
     /// \brief Get the key value.
     /// \return The key.
     public: const std::string &GetKey() const;
@@ -209,6 +244,12 @@ namespace sdf
     /// \return True if the parameter has been set.
     public: bool GetSet() const;
 
+    /// \brief Return true if the parameter ignores the parent element's
+    /// attributes, or if the parameter has no parent element.
+    /// \return True if the parameter ignores the parent element's attributes,
+    /// or if the parameter has no parent element.
+    public: bool IgnoresParentElementAttribute() const;
+
     /// \brief Clone the parameter.
     /// \return A new parameter that is the clone of this.
     public: ParamPtr Clone() const;
@@ -301,6 +342,9 @@ namespace sdf
     /// \brief Description of the parameter.
     public: std::string description;
 
+    /// \brief Parent element.
+    public: ElementWeakPtr parentElement;
+
     /// \brief Update function pointer.
     public: std::function<std::any ()> updateFunc;
 
@@ -319,6 +363,14 @@ namespace sdf
     /// \brief This parameter's value
     public: ParamVariant value;
 
+    /// \brief True if the value has been parsed while ignoring its parent
+    /// element's attributes, and will continue to ignore them for subsequent
+    /// reparses.
+    public: bool ignoreParentAttributes;
+
+    /// \brief This parameter's value that was provided as a string
+    public: std::optional<std::string> strValue;
+
     /// \brief This parameter's default value
     public: ParamVariant defaultValue;
 
@@ -399,7 +451,7 @@ namespace sdf
     {
       std::stringstream ss;
       ss << _value;
-      return this->SetFromString(ss.str());
+      return this->SetFromString(ss.str(), true);
     }
     catch(...)
     {

diff --git a/sdf/1.9/pose.sdf b/sdf/1.9/pose.sdf
@@ -1,11 +1,19 @@
 <!-- Pose -->
-<element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
-  <description>A position(x,y,z) and orientation(roll, pitch yaw) with respect to the frame named in the relative_to attribute.</description>
+<element name="pose" type="pose" default="0 0 0   0 0 0" required="0">
+  <description>
+    A pose (x, y, z, r, p, y) expressed in the frame named by @relative_to. The first three components (x, y, z) represent the position of the element's origin (in the @relative_to frame) and the last three components (roll, pitch, yaw) represent the orientation of the element as a sequence of intrinsic Euler rotations.
+  </description>
 
-  <attribute name="relative_to" type="string" default="" required="*">
+  <attribute name="relative_to" type="string" default="" required="0">
     <description>
       Name of frame relative to which the pose is applied.
     </description>
   </attribute>
 
+  <attribute name="degrees" type="bool" default="false" required="0">
+    <description>
+      Whether or not the euler angles are in degrees, otherwise they will be interpreted as radians by default.
+    </description>
+  </attribute>
+
 </element> <!-- End Pose -->
diff --git a/src/Element.cc b/src/Element.cc
@@ -152,6 +152,7 @@ void Element::AddValue(const std::string &_type,
   this->dataPtr->value =
       std::make_shared<Param>(this->dataPtr->name, _type, _defaultValue,
                               _required, _minValue, _maxValue, _description);
+  this->dataPtr->value->SetParentElement(shared_from_this());
 }
 
 /////////////////////////////////////////////////
@@ -161,8 +162,10 @@ ParamPtr Element::CreateParam(const std::string &_key,
                               bool _required,
                               const std::string &_description)
 {
-  return ParamPtr(
-      new Param(_key, _type, _defaultValue, _required, _description));
+  ParamPtr param = std::make_shared<Param>(
+      _key, _type, _defaultValue, _required, _description);
+  param->SetParentElement(shared_from_this());
+  return param;
 }
 
 /////////////////////////////////////////////////
@@ -195,7 +198,9 @@ ElementPtr Element::Clone() const
   for (aiter = this->dataPtr->attributes.begin();
        aiter != this->dataPtr->attributes.end(); ++aiter)
   {
-    clone->dataPtr->attributes.push_back((*aiter)->Clone());
+    auto clonedAttribute = (*aiter)->Clone();
+    clonedAttribute->SetParentElement(clone);
+    clone->dataPtr->attributes.push_back(clonedAttribute);
   }
 
   ElementPtr_V::const_iterator eiter;
@@ -215,6 +220,7 @@ ElementPtr Element::Clone() const
   if (this->dataPtr->value)
   {
     clone->dataPtr->value = this->dataPtr->value->Clone();
+    clone->dataPtr->value->SetParentElement(clone);
   }
 
   if (this->dataPtr->includeElement)
@@ -248,6 +254,7 @@ void Element::Copy(const ElementPtr _elem)
     }
     ParamPtr param = this->GetAttribute((*iter)->GetKey());
     (*param) = (**iter);
+    param->SetParentElement(shared_from_this());
   }
 
   if (_elem->GetValue())
@@ -260,6 +267,7 @@ void Element::Copy(const ElementPtr _elem)
     {
       *(this->dataPtr->value) = *(_elem->GetValue());
     }
+    this->dataPtr->value->SetParentElement(shared_from_this());
   }
 
   this->dataPtr->elementDescriptions.clear();
@@ -657,6 +665,12 @@ size_t Element::GetAttributeCount() const
   return this->dataPtr->attributes.size();
 }
 
+/////////////////////////////////////////////////
+const Param_V &Element::GetAttributes() const
+{
+  return this->dataPtr->attributes;
+}
+
 /////////////////////////////////////////////////
 ParamPtr Element::GetAttribute(unsigned int _index) const
 {