Support printing sdf poses in degrees and allow snapping to commonly …

…used angles (#689) * Ruby option to print in_degrees or snap_to_degrees Signed-off-by: Aaron Chong <[email protected]> * Basic PrintConfig added Signed-off-by: Aaron Chong <[email protected]> * PrintConfig gets passed into printing implementations of Element and Param Signed-off-by: Aaron Chong <[email protected]> * Adding basic test for print options Signed-off-by: Aaron Chong <[email protected]> * Reverting to PrintConfig with basic API Signed-off-by: Aaron Chong <[email protected]> * Moved creation of PrintConfig into ign functions Signed-off-by: Aaron Chong <[email protected]> * Param value GetPoseAsString and tests Signed-off-by: Aaron Chong <[email protected]> * Moved attribute painting to its own function, fixed test strings Signed-off-by: Aaron Chong <[email protected]> * Added basic tests for pose rotation input as quaternions Signed-off-by: Aaron Chong <[email protected]> * Using different flags for ign sdf -p, allow snapping to different values Signed-off-by: Aaron Chong <[email protected]> * Disabling test on windows, fixing comment Signed-off-by: Aaron Chong <[email protected]> * Remove stale function, fixed linting Signed-off-by: Aaron Chong <[email protected]> * Adding tolerance as a argument, added tests Signed-off-by: Aaron Chong <[email protected]> * Use 3 spaces when changing rotation formats or snapping to degrees Signed-off-by: Aaron Chong <[email protected]> * Added check for tolerance larger than snapping interval Signed-off-by: Aaron Chong <[email protected]> * Moving PrintAttributes to ElementPrivate to remain ABI stability Signed-off-by: Aaron Chong <[email protected]> * Using true/false instead of 1/0 Signed-off-by: Aaron Chong <[email protected]> * Remove use of SDF_ASSERT in GetAsString Signed-off-by: Aaron Chong <[email protected]> * Added tests for //include/pose Signed-off-by: Aaron Chong <[email protected]> * Adding parsing passing test for empty quat_xyzw pose Signed-off-by: Aaron Chong <[email protected]> * Added check for default string values to be modified when rotation_format is defined Signed-off-by: Aaron Chong <[email protected]> * Added tests Signed-off-by: Aaron Chong <[email protected]> * Reparsing translates default value into string to be used if values have not been assigned to param Signed-off-by: Aaron Chong <[email protected]> * Using StringFromValueImpl for getting strings from all ParamVariants Signed-off-by: Aaron Chong <[email protected]> * Refactor pose string from value into its own function Signed-off-by: Aaron Chong <[email protected]> * Fixing casting erroerror, added documentation and tests for tolerance < interval Signed-off-by: Aaron Chong <[email protected]> * Correcting stale comments Signed-off-by: Aaron Chong <[email protected]> * Fixing snapToInterval math, added more tests Signed-off-by: Aaron Chong <[email protected]> * Removed unneeded visibility macro Signed-off-by: Aaron Chong <[email protected]> * Adding return documentation and using const reference to variant instead of pointer Signed-off-by: Aaron Chong <[email protected]> * Returning string directly, removing stale _config, reverting strValue to nullopt Signed-off-by: Aaron Chong <[email protected]> * Remove use of assertions Signed-off-by: Aaron Chong <[email protected]> * Suggested changes to #729 (#748) Signed-off-by: Addisu Z. Taddese <[email protected]> * Using three space delimiter between position and rotation if attributes are set Signed-off-by: Aaron Chong <[email protected]> * Added comment regarding use of default PrintConfig in Reparse Signed-off-by: Aaron Chong <[email protected]> * Adding equality comparison for PrintConfig Signed-off-by: Aaron Chong <[email protected]> * Removed stale include Signed-off-by: Aaron Chong <[email protected]> * Uniied string and value parsing behavior, and modified necessary tests Signed-off-by: Aaron Chong <[email protected]> * Overloaded function to maintain ABI stability Signed-off-by: Aaron Chong <[email protected]> * Fixing missing space in test for exec command Signed-off-by: Aaron Chong <[email protected]> * Adding comment regarding attributeExceptions Signed-off-by: Aaron Chong <[email protected]> * Indenting help message, adding test for shuffling command flags Signed-off-by: Aaron Chong <[email protected]> * Modifying cmd flag shuffling test to handling flags with arguments too Signed-off-by: Aaron Chong <[email protected]> * Removed Get from PrintConfig getter functions Signed-off-by: Aaron Chong <[email protected]> * Using std optional's converting constructor Signed-off-by: Aaron Chong <[email protected]> * Modified snapToInterval implementation, added test Signed-off-by: Aaron Chong <[email protected]> * Added bool type specific value parser, values are parsed using ParamStreamer by default Signed-off-by: Aaron Chong <[email protected]> * Reverting all unnecessary changes made in sdf12 to old tests Signed-off-by: Aaron Chong <[email protected]> * Added comparison for PreserveIncludes Signed-off-by: Aaron Chong <[email protected]> * Check for 'type' attribute in unknown elements as well, in order to parse booleans into true/false, instead of 1/0 Signed-off-by: Aaron Chong <[email protected]> * Only checking for pose related PrintConfig options for returning string instead of default PrintConfig Signed-off-by: Aaron Chong <[email protected]> * Added comment regarding sanitizing -0 in test outputs Signed-off-by: Aaron Chong <[email protected]> Co-authored-by: Addisu Z. Taddese <[email protected]>
gazebosim · Dec 21, 2021 · 5e91938 · 5e91938
1 parent de356cd
commit 5e91938
Show file tree

Hide file tree

Showing 27 changed files with 1,262 additions and 138 deletions.
diff --git a/include/sdf/Element.hh b/include/sdf/Element.hh
@@ -199,7 +199,7 @@ namespace sdf
     /// \param[in] _prefix String value to prefix to the output.
     /// \param[in] _includeDefaultElements flag to include default elements.
     /// \param[in] _includeDefaultAttributes flag to include default attributes.
-    /// \param[in] _config Configuration for printing the values.
+    /// \param[in] _config Configuration for converting to string.
     /// \return The string representation.
     public: std::string ToString(
         const std::string &_prefix,
@@ -589,7 +589,7 @@ namespace sdf
     /// \param[in] _includeDefaultElements flag to include default elements.
     /// \param[in] _includeDefaultAttributes flag to include default attributes.
     /// \param[in] _config Configuration for printing values.
-    /// \param[out] _out the std::ostreamstream to write output to.
+    /// \param[out] _out the std::ostringstream to write output to.
     private: void PrintValuesImpl(const std::string &_prefix,
                                   bool _includeDefaultElements,
                                   bool _includeDefaultAttributes,
@@ -685,6 +685,14 @@ namespace sdf
 
     /// \brief XML path of this element.
     public: std::string xmlPath;
+
+    /// \brief Generate the string (XML) for the attributes.
+    /// \param[in] _includeDefaultAttributes flag to include default attributes.
+    /// \param[in] _config Configuration for printing attributes.
+    /// \param[out] _out the std::ostringstream to write output to.
+    public: void PrintAttributes(bool _includeDefaultAttributes,
+                                 const PrintConfig &_config,
+                                 std::ostringstream &_out) const;
   };
 
   ///////////////////////////////////////////////

diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh
@@ -401,17 +401,34 @@ namespace sdf
                                     const std::string &_valueStr,
                                     ParamVariant &_valueToSet) const;
 
-    /// \brief Method used to get the string representation from a ParamVariant
+    /// \brief Method used to get the string representation from a ParamVariant,
+    /// or the string that was used to set it.
     /// \param[in] _config Print configuration for the string output
     /// \param[in] _typeName The data type of the value
     /// \param[in] _value The value
-    /// \param[in] _valueStr The string representation of the value
-    /// \return True if the string was successfully retrieved from the value,
-    /// false otherwise.
-    public: bool StringFromValueImpl(const PrintConfig &_config,
-                                     const std::string &_typeName,
-                                     const ParamVariant &_value,
-                                     std::string &_valueStr) const;
+    /// \param[out] _valueStr The output string.
+    /// \return True if the string was successfully retrieved, false otherwise.
+    public: bool StringFromValueImpl(
+                const PrintConfig &_config,
+                const std::string &_typeName,
+                const ParamVariant &_value,
+                std::string &_valueStr) const;
+
+    /// \brief Method used to get the string representation from a ParamVariant,
+    /// or the string that was used to set it.
+    /// \param[in] _config Print configuration for the string output
+    /// \param[in] _typeName The data type of the value
+    /// \param[in] _value The value
+    /// \param[in] _orignalStr The original string that was used to set the
+    /// value. A nullopt can be passed in if it is not available.
+    /// \param[out] _valueStr The output string.
+    /// \return True if the string was successfully retrieved, false otherwise.
+    public: bool StringFromValueImpl(
+                const PrintConfig &_config,
+                const std::string &_typeName,
+                const ParamVariant &_value,
+                const std::optional<std::string> &_originalStr,
+                std::string &_valueStr) const;
 
     /// \brief Data type to string mapping
     /// \return The type as a string, empty string if unknown type

diff --git a/include/sdf/PrintConfig.hh b/include/sdf/PrintConfig.hh
@@ -17,6 +17,7 @@
 #ifndef SDF_PRINTCONFIG_HH_
 #define SDF_PRINTCONFIG_HH_
 
+#include <optional>
 #include <ignition/utils/ImplPtr.hh>
 
 #include "sdf/sdf_config.h"
@@ -29,9 +30,41 @@ namespace sdf
   /// This class contains configuration options for printing elements.
   class SDFORMAT_VISIBLE PrintConfig
   {
-    /// \brief Default constructor.
+    /// \brief Default constructor. All options are set to false by default.
     public: PrintConfig();
 
+    /// \brief Sets the option for printing pose rotations in degrees if true,
+    /// otherwise they will be printed as radians by default.
+    /// \param[in] _value Whether to print pose rotations in degrees.
+    public: void SetRotationInDegrees(bool _value);
+
+    /// \brief Returns whether or not pose rotations should be printed in
+    /// degrees.
+    /// \return True if pose rotations are printed in degrees, false otherwise.
+    public: bool RotationInDegrees() const;
+
+    /// \brief Sets the option for printing pose rotation in degrees as well as
+    /// snapping the rotation to the desired interval, with the provided
+    /// tolerance.
+    /// \param[in] _interval Degrees interval to snap to, this value must be
+    /// larger than 0, and less than or equal to 360.
+    /// \param[in] _tolerance Tolerance which snapping occurs, this value must
+    /// be larger than 0, less than 360, and less than the provided interval.
+    /// \return True, unless any of the provided values are not valid.
+    public: bool SetRotationSnapToDegrees(unsigned int _interval,
+                                          double _tolerance);
+
+    /// \brief Returns the current degree value that pose rotations will snap to
+    /// when printed.
+    /// \return The assigned degrees interval value to snap to. If it has not
+    /// been assigned, a nullopt will be returned.
+    public: std::optional<unsigned int> RotationSnapToDegrees() const;
+
+    /// \brief Returns the tolerance for snapping degree values when printed.
+    /// \return The assigned tolerance value which allows snapping to happen. If
+    /// it has not been assigned, a nullopt will be returned.
+    public: std::optional<double> RotationSnapTolerance() const;
+
     /// \brief Set print config to preserve <include> tags.
     /// \param[in] _preserve True to preserve <include> tags.
     /// False to expand included model.
@@ -42,6 +75,11 @@ namespace sdf
     /// False if they are to be expanded.
     public: bool PreserveIncludes() const;
 
+    /// \brief Return true if both PrintConfig objects contain the same values.
+    /// \param[in] _config PrintConfig to compare.
+    /// \return True if 'this' == _config.
+    public: bool operator==(const PrintConfig &_config) const;
+
     /// \brief Private data pointer.
     IGN_UTILS_IMPL_PTR(dataPtr)
   };

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
@@ -136,8 +136,8 @@ if (BUILD_SDF_TEST)
     Pbr_TEST.cc
     Physics_TEST.cc
     Plugin_TEST.cc
-    PrintConfig_TEST.cc
     Plane_TEST.cc
+    PrintConfig_TEST.cc
     Root_TEST.cc
     Scene_TEST.cc
     SemanticPose_TEST.cc

diff --git a/src/Element.cc b/src/Element.cc
@@ -499,6 +499,7 @@ void Element::PrintDocLeftPane(std::string &_html, int _spacing,
   _html += "</div>\n";
 }
 
+/////////////////////////////////////////////////
 void Element::PrintValuesImpl(const std::string &_prefix,
                               bool _includeDefaultElements,
                               bool _includeDefaultAttributes,
@@ -513,22 +514,7 @@ void Element::PrintValuesImpl(const std::string &_prefix,
   {
     _out << _prefix << "<" << this->dataPtr->name;
 
-    Param_V::const_iterator aiter;
-    for (aiter = this->dataPtr->attributes.begin();
-         aiter != this->dataPtr->attributes.end(); ++aiter)
-    {
-      // Only print attribute values if they were set
-      // TODO(anyone): GetRequired is added here to support up-conversions where
-      // a new required attribute with a default value is added. We would have
-      // better separation of concerns if the conversion process set the
-      // required attributes with their default values.
-      if ((*aiter)->GetSet() || (*aiter)->GetRequired() ||
-          _includeDefaultAttributes)
-      {
-        _out << " " << (*aiter)->GetKey() << "='"
-             << (*aiter)->GetAsString(_config) << "'";
-      }
-    }
+    this->dataPtr->PrintAttributes(_includeDefaultAttributes, _config, _out);
 
     if (this->dataPtr->elements.size() > 0)
     {
@@ -560,6 +546,50 @@ void Element::PrintValuesImpl(const std::string &_prefix,
   }
 }
 
+/////////////////////////////////////////////////
+void ElementPrivate::PrintAttributes(bool _includeDefaultAttributes,
+                                     const PrintConfig &_config,
+                                     std::ostringstream &_out) const
+{
+  // Attribute exceptions are used in the event of a non-default PrintConfig
+  // which modifies the Attributes of this Element that are printed out. The
+  // modifications to an Attribute by a PrintConfig will overwrite the original
+  // existing Attribute when this Element is printed.
+  std::set<std::string> attributeExceptions;
+  if (this->name == "pose")
+  {
+    if (_config.RotationInDegrees() || _config.RotationSnapToDegrees())
+    {
+      attributeExceptions.insert("degrees");
+      _out << " " << "degrees='true'";
+
+      attributeExceptions.insert("rotation_format");
+      _out << " " << "rotation_format='euler_rpy'";
+    }
+  }
+
+  Param_V::const_iterator aiter;
+  for (aiter = this->attributes.begin();
+       aiter != this->attributes.end(); ++aiter)
+  {
+    // Only print attribute values if they were set
+    // TODO(anyone): GetRequired is added here to support up-conversions where
+    // a new required attribute with a default value is added. We would have
+    // better separation of concerns if the conversion process set the
+    // required attributes with their default values.
+    if ((*aiter)->GetSet() || (*aiter)->GetRequired() ||
+        _includeDefaultAttributes)
+    {
+      const std::string key = (*aiter)->GetKey();
+      const auto it = attributeExceptions.find(key);
+      if (it == attributeExceptions.end())
+      {
+        _out << " " << key << "='" << (*aiter)->GetAsString(_config) << "'";
+      }
+    }
+  }
+}
+
 /////////////////////////////////////////////////
 void Element::PrintValues(std::string _prefix, const PrintConfig &_config) const
 {