diff --git a/include/sdf/Element.hh b/include/sdf/Element.hh index 99cc471c8..52ce5f7cb 100644 --- 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 index d46204be3..827ba3f06 100644 --- 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 &_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 index 2e3b700de..8696339c3 100644 --- a/include/sdf/PrintConfig.hh +++ b/include/sdf/PrintConfig.hh @@ -17,6 +17,7 @@ #ifndef SDF_PRINTCONFIG_HH_ #define SDF_PRINTCONFIG_HH_ +#include #include #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 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 RotationSnapTolerance() const; + /// \brief Set print config to preserve tags. /// \param[in] _preserve True to preserve 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 index 597c6f5d8..fb25f7f4b 100644 --- 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 index 168403b8c..68f65e1c5 100644 --- a/src/Element.cc +++ b/src/Element.cc @@ -499,6 +499,7 @@ void Element::PrintDocLeftPane(std::string &_html, int _spacing, _html += "\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 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 { diff --git a/src/Param.cc b/src/Param.cc index 4c6ee2db1..0d370bd3c 100644 --- a/src/Param.cc +++ b/src/Param.cc @@ -308,13 +308,15 @@ void Param::Update() ////////////////////////////////////////////////// std::string Param::GetAsString(const PrintConfig &_config) const { - if (this->dataPtr->strValue.has_value() && !this->dataPtr->strValue->empty()) - { - return this->dataPtr->strValue.value(); - } - else if(!this->dataPtr->strValue.has_value()) + std::string valueStr; + if (this->GetSet() && + this->dataPtr->StringFromValueImpl(_config, + this->dataPtr->typeName, + this->dataPtr->value, + this->dataPtr->strValue, + valueStr)) { - return this->dataPtr->defaultStrValue; + return valueStr; } return this->GetDefaultAsString(_config); @@ -324,10 +326,12 @@ std::string Param::GetAsString(const PrintConfig &_config) const std::string Param::GetDefaultAsString(const PrintConfig &_config) const { std::string defaultStr; - if (this->dataPtr->StringFromValueImpl(_config, - this->dataPtr->typeName, - this->dataPtr->defaultValue, - defaultStr)) + if (this->dataPtr->StringFromValueImpl( + _config, + this->dataPtr->typeName, + this->dataPtr->defaultValue, + this->dataPtr->defaultStrValue, + defaultStr)) { return defaultStr; } @@ -785,13 +789,22 @@ bool ParamPrivate::ValueFromStringImpl(const std::string &_typeName, return true; } +////////////////////////////////////////////////// +/// \brief Helper function for StringFromValueImpl for pose. +/// \param[in] _config Printing configuration for the output string. +/// \param[in] _parentAttributes Parent Element Attributes. +/// \param[in] _value The variant value of this pose. +/// \param[in] _originalStr The original string used to set this pose value. +/// \param[out] _valueStr The pose as a string. +/// \return True if the string was successfully retrieved from the pose, false +/// otherwise. ///////////////////////////////////////////////// bool PoseStringFromValue(const PrintConfig &_config, const Param_V &_parentAttributes, const ParamPrivate::ParamVariant &_value, + const std::optional &_originalStr, std::string &_valueStr) { - (void)_config; StringStreamClassicLocale ss; const ignition::math::Pose3d *pose = @@ -802,20 +815,6 @@ bool PoseStringFromValue(const PrintConfig &_config, return false; } - auto sanitizeZero = [](double _number) - { - StringStreamClassicLocale stream; - if (std::fpclassify(_number) == FP_ZERO) - { - stream << 0; - } - else - { - stream << _number; - } - return stream.str(); - }; - const bool defaultInDegrees = false; bool inDegrees = defaultInDegrees; @@ -828,6 +827,10 @@ bool PoseStringFromValue(const PrintConfig &_config, const std::string threeSpacedDelimiter = " "; std::string posRotDelimiter = defaultPosRotDelimiter; + const bool defaultSnapDegreesToInterval = false; + bool snapDegreesToInterval = defaultSnapDegreesToInterval; + + // Checking parent Element Attributes for desired pose representations. for (const auto &p : _parentAttributes) { const std::string key = p->GetKey(); @@ -854,6 +857,39 @@ bool PoseStringFromValue(const PrintConfig &_config, } } + // Checking PrintConfig for desired pose representations. This overrides + // any parent Element Attributes. + if (_config.RotationInDegrees()) + { + inDegrees = true; + rotationFormat = "euler_rpy"; + posRotDelimiter = threeSpacedDelimiter; + } + if (_config.RotationSnapToDegrees().has_value() && + _config.RotationSnapTolerance().has_value()) + { + inDegrees = true; + rotationFormat = "euler_rpy"; + snapDegreesToInterval = true; + posRotDelimiter = threeSpacedDelimiter; + } + + // Helper function that sanitizes zero values like '-0' + auto sanitizeZero = [](double _number) + { + StringStreamClassicLocale stream; + if (std::fpclassify(_number) == FP_ZERO) + { + stream << 0; + } + else + { + stream << _number; + } + return stream.str(); + }; + + // Returning pose string representations based on desired configurations. if (rotationFormat == "quat_xyzw" && inDegrees) { sdferr << "Invalid pose with //pose[@degrees='true'] and " @@ -870,6 +906,36 @@ bool PoseStringFromValue(const PrintConfig &_config, _valueStr = ss.str(); return true; } + else if (rotationFormat == "euler_rpy" && inDegrees && snapDegreesToInterval) + { + // Helper function that returns a snapped value if it is within the + // tolerance of multiples of interval, otherwise the orginal value is + // returned. + auto snapToInterval = + [](double _val, unsigned int _interval, double _tolerance) + { + double closestQuotient = std::round(_val / _interval); + double distance = std::abs(_val - closestQuotient * _interval); + if (distance < _tolerance) + { + return _interval * closestQuotient; + } + return _val; + }; + + const unsigned int interval = _config.RotationSnapToDegrees().value(); + const double tolerance = _config.RotationSnapTolerance().value(); + + ss << pose->Pos() << posRotDelimiter + << sanitizeZero(snapToInterval( + IGN_RTOD(pose->Rot().Roll()), interval, tolerance)) << " " + << sanitizeZero(snapToInterval( + IGN_RTOD(pose->Rot().Pitch()), interval, tolerance)) << " " + << sanitizeZero(snapToInterval( + IGN_RTOD(pose->Rot().Yaw()), interval, tolerance)); + _valueStr = ss.str(); + return true; + } else if (rotationFormat == "euler_rpy" && inDegrees) { ss << pose->Pos() << posRotDelimiter @@ -880,6 +946,17 @@ bool PoseStringFromValue(const PrintConfig &_config, return true; } + // If no modification to the value is needed, the original string is returned. + if (!_config.RotationInDegrees() && + !_config.RotationSnapToDegrees().has_value() && + !_config.RotationSnapTolerance().has_value() && + _originalStr.has_value() && + !_originalStr->empty()) + { + _valueStr = _originalStr.value(); + return true; + } + ss << pose->Pos() << posRotDelimiter << sanitizeZero(pose->Rot().Roll()) << " " << sanitizeZero(pose->Rot().Pitch()) << " " @@ -889,12 +966,42 @@ bool PoseStringFromValue(const PrintConfig &_config, } ///////////////////////////////////////////////// -bool ParamPrivate::StringFromValueImpl(const PrintConfig &_config, - const std::string &_typeName, - const ParamVariant &_value, - std::string &_valueStr) const +bool ParamPrivate::StringFromValueImpl( + const PrintConfig &_config, + const std::string &_typeName, + const ParamVariant &_value, + std::string &_valueStr) const +{ + return this->StringFromValueImpl( + _config, + _typeName, + _value, + std::nullopt, + _valueStr); +} + +///////////////////////////////////////////////// +bool ParamPrivate::StringFromValueImpl( + const PrintConfig &_config, + const std::string &_typeName, + const ParamVariant &_value, + const std::optional &_originalStr, + std::string &_valueStr) const { - if (_typeName == "ignition::math::Pose3d" || + // This will be handled in a type specific manner + if (_typeName == "bool") + { + const bool *val = std::get_if(&_value); + if (!val) + { + sdferr << "Unable to get bool value from variant.\n"; + return false; + } + + _valueStr = *val ? "true" : "false"; + return true; + } + else if (_typeName == "ignition::math::Pose3d" || _typeName == "pose" || _typeName == "Pose") { @@ -902,9 +1009,9 @@ bool ParamPrivate::StringFromValueImpl(const PrintConfig &_config, if (!this->ignoreParentAttributes && p) { return PoseStringFromValue( - _config, p->GetAttributes(), _value, _valueStr); + _config, p->GetAttributes(), _value, _originalStr, _valueStr); } - return PoseStringFromValue(_config, {}, _value, _valueStr); + return PoseStringFromValue(_config, {}, _value, _originalStr, _valueStr); } StringStreamClassicLocale ss; diff --git a/src/Plugin_TEST.cc b/src/Plugin_TEST.cc index 00d5b6245..35f6aee0d 100644 --- a/src/Plugin_TEST.cc +++ b/src/Plugin_TEST.cc @@ -201,7 +201,7 @@ TEST(DOMPlugin, LoadWithChildren) pluginStr + ""; sdf::ElementPtr elem(new sdf::Element); sdf::initFile("plugin.sdf", elem); - sdf::readString(pluginStrWithSdf, elem); + ASSERT_TRUE(sdf::readString(pluginStrWithSdf, elem)); sdf::Plugin plugin; sdf::Errors errors; diff --git a/src/PrintConfig.cc b/src/PrintConfig.cc index 3770f5007..a36c441b5 100644 --- a/src/PrintConfig.cc +++ b/src/PrintConfig.cc @@ -16,14 +16,24 @@ */ #include "sdf/PrintConfig.hh" +#include "sdf/Console.hh" + +using namespace sdf; -namespace sdf -{ -inline namespace SDF_VERSION_NAMESPACE -{ ///////////////////////////////////////////////// class PrintConfig::Implementation { + /// \brief True if rotation in poses are to be printed in degrees. + public: bool rotationInDegrees = false; + + /// \brief The interval in degrees, which rotation in poses shall snap to, + /// if they are within the tolerance value of rotationSnapTolerance. + public: std::optional rotationSnapToDegrees = std::nullopt; + + /// \brief The tolerance which is used to determine whether snapping of + /// rotation in poses happen. + public: std::optional rotationSnapTolerance = std::nullopt; + /// \brief True to preserve tags, false to expand. public: bool preserveIncludes = false; }; @@ -34,6 +44,18 @@ PrintConfig::PrintConfig() { } +///////////////////////////////////////////////// +void PrintConfig::SetRotationInDegrees(bool _value) +{ + this->dataPtr->rotationInDegrees = _value; +} + +///////////////////////////////////////////////// +bool PrintConfig::RotationInDegrees() const +{ + return this->dataPtr->rotationInDegrees; +} + ///////////////////////////////////////////////// void PrintConfig::SetPreserveIncludes(bool _preserve) { @@ -46,5 +68,51 @@ bool PrintConfig::PreserveIncludes() const return this->dataPtr->preserveIncludes; } +///////////////////////////////////////////////// +bool PrintConfig::SetRotationSnapToDegrees(unsigned int _interval, + double _tolerance) +{ + if (_interval == 0 || _interval > 360) + { + sdferr << "Interval value to snap to must be larger than 0, and less than " + << "or equal to 360.\n"; + return false; + } + + if (_tolerance <= 0 || _tolerance > 360 || + _tolerance >= static_cast(_interval)) + { + sdferr << "Tolerance must be larger than 0, less than or equal to " + << "360, and less than the provided interval.\n"; + return false; + } + + this->dataPtr->rotationSnapToDegrees = _interval; + this->dataPtr->rotationSnapTolerance = _tolerance; + return true; +} + +///////////////////////////////////////////////// +std::optional PrintConfig::RotationSnapToDegrees() const +{ + return this->dataPtr->rotationSnapToDegrees; } + +///////////////////////////////////////////////// +std::optional PrintConfig::RotationSnapTolerance() const +{ + return this->dataPtr->rotationSnapTolerance; +} + +///////////////////////////////////////////////// +bool PrintConfig::operator==(const PrintConfig &_config) const +{ + if (this->RotationInDegrees() == _config.RotationInDegrees() && + this->RotationSnapToDegrees() == _config.RotationSnapToDegrees() && + this->RotationSnapTolerance() == _config.RotationSnapTolerance() && + this->PreserveIncludes() == _config.PreserveIncludes()) + { + return true; + } + return false; } diff --git a/src/PrintConfig_TEST.cc b/src/PrintConfig_TEST.cc index ff473bf5d..9ac523c9e 100644 --- a/src/PrintConfig_TEST.cc +++ b/src/PrintConfig_TEST.cc @@ -20,6 +20,81 @@ #include "sdf/PrintConfig.hh" #include "test_config.h" +///////////////////////////////////////////////// +TEST(PrintConfig, Construction) +{ + sdf::PrintConfig config; + EXPECT_FALSE(config.RotationInDegrees()); + EXPECT_FALSE(config.RotationSnapToDegrees()); + EXPECT_FALSE(config.PreserveIncludes()); +} + +///////////////////////////////////////////////// +TEST(PrintConfig, RotationInDegrees) +{ + sdf::PrintConfig config; + EXPECT_FALSE(config.RotationInDegrees()); + + config.SetRotationInDegrees(true); + EXPECT_TRUE(config.RotationInDegrees()); + + config.SetRotationInDegrees(false); + EXPECT_FALSE(config.RotationInDegrees()); +} + +///////////////////////////////////////////////// +TEST(PrintConfig, RotationSnapToDegrees) +{ + sdf::PrintConfig config; + EXPECT_FALSE(config.RotationSnapToDegrees().has_value()); + EXPECT_FALSE(config.RotationSnapTolerance().has_value()); + + EXPECT_TRUE(config.SetRotationSnapToDegrees(5, 0.01)); + ASSERT_TRUE(config.RotationSnapToDegrees().has_value()); + EXPECT_EQ(5u, config.RotationSnapToDegrees().value()); + ASSERT_TRUE(config.RotationSnapTolerance().has_value()); + EXPECT_DOUBLE_EQ(0.01, config.RotationSnapTolerance().value()); + + EXPECT_FALSE(config.SetRotationSnapToDegrees(0, 0.01)); + EXPECT_FALSE(config.SetRotationSnapToDegrees(360 + 1, 0.01)); + EXPECT_FALSE(config.SetRotationSnapToDegrees(5, -1e6)); + EXPECT_FALSE(config.SetRotationSnapToDegrees(5, 360 + 1e-6)); + + EXPECT_FALSE(config.SetRotationSnapToDegrees(5, 5 + 1e-6)); + EXPECT_TRUE(config.SetRotationSnapToDegrees(5, 5 - 1e-6)); + ASSERT_TRUE(config.RotationSnapToDegrees().has_value()); + EXPECT_EQ(5u, config.RotationSnapToDegrees().value()); + ASSERT_TRUE(config.RotationSnapTolerance().has_value()); + EXPECT_DOUBLE_EQ(5 - 1e-6, config.RotationSnapTolerance().value()); +} + +///////////////////////////////////////////////// +TEST(PrintConfig, Compare) +{ + sdf::PrintConfig first; + sdf::PrintConfig second; + EXPECT_TRUE(first == second); + EXPECT_TRUE(second == first); + + first.SetRotationInDegrees(true); + EXPECT_TRUE(first.RotationInDegrees()); + EXPECT_FALSE(second.RotationInDegrees()); + EXPECT_FALSE(first == second); + EXPECT_FALSE(second == first); + + second.SetRotationInDegrees(true); + EXPECT_TRUE(first == second); + EXPECT_TRUE(second == first); + + EXPECT_TRUE(first.SetRotationSnapToDegrees(5, 0.01)); + EXPECT_FALSE(first == second); + EXPECT_FALSE(second == first); + + EXPECT_TRUE(second.SetRotationSnapToDegrees(5, 0.01)); + EXPECT_TRUE(first == second); + EXPECT_TRUE(second == first); +} + ///////////////////////////////////////////////// TEST(PrintConfig, PreserveIncludes) { diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 9d4d757c4..5de7a65b3 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -31,9 +31,9 @@ require 'optparse' LIBRARY_NAME = '@library_location@' LIBRARY_VERSION = '@SDF_VERSION_FULL@' COMMON_OPTIONS = - " -h [ --help ] Print this help message.\n"\ - " --force-version Use a specific library version.\n"\ - ' --versions Show the available versions.' + " -h [ --help ] Print this help message.\n"\ + " --force-version Use a specific library version.\n"\ + ' --versions Show the available versions.' COMMANDS = { 'sdf' => "Utilities for SDF files.\n\n"\ " ign sdf [options]\n\n"\ @@ -44,6 +44,12 @@ COMMANDS = { 'sdf' => " use only and the output may change without any promise of stability)\n" + " -p [ --print ] arg Print converted arg.\n" + " -i [ --preserve-includes ] Preserve included tags when printing converted arg (does not preserve merge-includes).\n" + + " --degrees Pose rotation angles are printed in degrees.\n" + + " --snap-to-degrees arg Snap pose rotation angles to this specified interval in degrees. This value must be\n" + + " larger than 0, less than or equal to 360, and larger than the defined snap tolerance.\n" + + " --snap-tolerance arg Used in conjunction with --snap-to-degrees, specifies the tolerance at which snapping\n" + + " occurs. This value must be larger than 0, less than 360, and less than the defined\n" + + " degrees value to snap to. If unspecified, its default value is 0.01.\n" + COMMON_OPTIONS } @@ -57,6 +63,8 @@ class Cmd # def parse(args) options = {} + options['degrees'] = 0 + options['snap_tolerance'] = 0.01 usage = COMMANDS[args[0]] @@ -82,6 +90,25 @@ class Cmd opts.on('-i', '--preserve-includes', 'Preserve included tags when printing converted arg (does not preserve merge-includes)') do options['preserve_includes'] = 1 end + opts.on('--degrees', 'Printed pose rotations are will be in degrees') do |degrees| + options['degrees'] = 1 + end + opts.on('--snap-to-degrees arg', Integer, + 'Printed rotations are snapped to specified degree intervals') do |arg| + if arg == 0 || arg > 360 + puts "Degree interval to snap to must be more than 0, and less than or equal to 360." + exit(-1) + end + options['snap_to_degrees'] = arg + end + opts.on('--snap-tolerance arg', Float, + 'Printed rotations are snapped if they are within this specified tolerance') do |arg| + if arg < 0 || arg > 360 + puts "Rotation snapping tolerance must be more than 0, and less than 360." + exit(-1) + end + options['snap_tolerance'] = arg + end opts.on('-g arg', '--graph type', String, 'Print PoseRelativeTo or FrameAttachedTo graph') do |graph_type| options['graph'] = {:type => graph_type} @@ -178,13 +205,22 @@ class Cmd Importer.extern 'int cmdDescribe(const char *)' exit(Importer.cmdDescribe(options['describe'])) elsif options.key?('print') + snap_to_degrees = 0 if options['preserve_includes'] Importer.extern 'int cmdPrintPreserveIncludes(const char *)' exit(Importer.cmdPrintPreserveIncludes(File.expand_path(options['print']))) - else - Importer.extern 'int cmdPrint(const char *)' - exit(Importer.cmdPrint(File.expand_path(options['print']))) + elsif options.key?('snap_to_degrees') + if options['snap_to_degrees'] < options['snap_tolerance'] + puts "Rotation snapping tolerance must be larger than the snapping tolerance." + exit(-1) + end + snap_to_degrees = options['snap_to_degrees'] end + Importer.extern 'int cmdPrint(const char *, int in_degrees, int snap_to_degrees, float snap_tolerance)' + exit(Importer.cmdPrint(File.expand_path(options['print']), + options['degrees'], + snap_to_degrees, + options['snap_tolerance'])) elsif options.key?('graph') Importer.extern 'int cmdGraph(const char *, const char *)' exit(Importer.cmdGraph(options['graph'][:type], File.expand_path(ARGV[1]))) diff --git a/src/ign.cc b/src/ign.cc index 16cdb0d24..23f9625e8 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -18,12 +18,14 @@ #include #include #include +#include #include #include "sdf/sdf_config.h" #include "sdf/Filesystem.hh" #include "sdf/Root.hh" #include "sdf/parser.hh" +#include "sdf/PrintConfig.hh" #include "sdf/system_util.hh" #include "FrameSemantics.hh" @@ -131,7 +133,8 @@ extern "C" SDFORMAT_VISIBLE int cmdDescribe(const char *_version) } ////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path) +extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path, + int inDegrees, int snapToDegrees, float snapTolerance) { if (!sdf::filesystem::exists(_path)) { @@ -153,8 +156,18 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path) return -1; } - sdf->PrintValues(); + sdf::PrintConfig config; + if (inDegrees!= 0) + { + config.SetRotationInDegrees(true); + } + if (snapToDegrees > 0) + { + config.SetRotationSnapToDegrees(static_cast(snapToDegrees), + static_cast(snapTolerance)); + } + sdf->PrintValues(config); return 0; } diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 249cecd3b..d93172742 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -948,6 +948,631 @@ TEST(print, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } } +////////////////////////////////////////////////// +static bool contains(const std::string &_a, const std::string &_b) +{ + return _a.find(_b) != std::string::npos; +} + +///////////////////////////////////////////////// +TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const std::string path = + sdf::testing::TestFile("sdf", "rotations_in_degrees.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "1 2 3 30.009 44.991 -60.009"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const std::string path = + sdf::testing::TestFile("sdf", "rotations_in_radians.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "1 2 3 0.523756 0.785241 -1.04735"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.0087"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.0087"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const auto path = sdf::testing::TestFile( + "sdf", "rotations_in_quaternions.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 0.391948 0.200425 -0.532046 0.723279"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_includes_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + // Set SDF_PATH so that included models can be found + sdf::testing::setenv( + "SDF_PATH", sdf::testing::SourceFile("test", "integration", "model")); + const std::string path = + sdf::testing::TestFile("sdf", "includes_rotations_in_degrees.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "1 2 3 30.009 44.991 -60.009"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_includes_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + // Set SDF_PATH so that included models can be found + sdf::testing::setenv( + "SDF_PATH", sdf::testing::SourceFile("test", "integration", "model")); + const std::string path = + sdf::testing::TestFile("sdf", "includes_rotations_in_radians.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "1 2 3 0.523756 0.785241 -1.04735"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.0087"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.0087"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_includes_rotations_in_quaternions, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + // Set SDF_PATH so that included models can be found + sdf::testing::setenv( + "SDF_PATH", sdf::testing::SourceFile("test", "integration", "model")); + const auto path = sdf::testing::TestFile( + "sdf", "includes_rotations_in_quaternions.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 0.391948 0.200425 -0.532046 0.723279"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_rotations_in_unnormalized_degrees, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const std::string path = + sdf::testing::TestFile("sdf", "rotations_in_unnormalized_degrees.sdf"); + + // Default printing + // Unnormalized degree values cannot be returned as is, as its string is + // returned by parsing the pose value, whenever a parent Element Attribute, + // or PrintConfig is used. + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "1 2 3 30.009 44.991 -60.009"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_rotations_in_unnormalized_radians, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const std::string path = + sdf::testing::TestFile("sdf", "rotations_in_unnormalized_radians.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "1 2 3 -5.75943 -11.78112 5.23583"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.9915 -60.009"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.9915 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.9915 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.9915 -60.009"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(shuffled_cmd_flags, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const std::string path = + sdf::testing::TestFile("sdf", "rotations_in_unnormalized_radians.sdf"); + + // -p PATH --degrees + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.9915 -60.009"); + + // --degrees -p PATH + output = custom_exec_str( + IgnCommand() + " sdf --degrees -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.9915 -60.009"); + + // -p PATH --snap-to-degrees ARG + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // -p --snap-to-degrees ARG PATH + output = custom_exec_str( + IgnCommand() + " sdf -p --snap-to-degrees 5 " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // --snap-to-degrees ARG -p PATH + output = custom_exec_str( + IgnCommand() + " sdf --snap-to-degrees 5 -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_snap_to_degrees_tolerance_too_high, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const std::string path = sdf::testing::TestFile( + "sdf", + "rotations_in_degrees_high_snap_tolerance.sdf"); + + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + + " --snap-to-degrees 5 " + " --snap-tolerance 4" + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 50 60"); +} + ///////////////////////////////////////////////// TEST(GraphCmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) { diff --git a/src/parser.cc b/src/parser.cc index 0c45332b0..f4dc5758d 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1942,19 +1942,27 @@ void copyChildren(ElementPtr _sdf, ElementPtr element(new Element); element->SetParent(_sdf); element->SetName(elem_name); - if (elemXml->GetText() != nullptr) - { - element->AddValue("string", elemXml->GetText(), "1"); - } - + std::optional typeName = std::nullopt; for (const tinyxml2::XMLAttribute *attribute = elemXml->FirstAttribute(); attribute; attribute = attribute->Next()) { - element->AddAttribute(attribute->Name(), "string", "", 1, ""); - element->GetAttribute(attribute->Name())->SetFromString( + const std::string attributeName(attribute->Name()); + if (attributeName == "type") + typeName = attribute->Value(); + + element->AddAttribute(attributeName, "string", "", 1, ""); + element->GetAttribute(attributeName)->SetFromString( attribute->Value()); } + if (elemXml->GetText() != nullptr) + { + if (typeName.has_value()) + element->AddValue(typeName.value(), elemXml->GetText(), true); + else + element->AddValue("string", elemXml->GetText(), true); + } + copyChildren(element, elemXml, _onlyUnknown); _sdf->InsertElement(element); } diff --git a/test/integration/default_elements.cc b/test/integration/default_elements.cc index fb8434db8..40767e6e2 100644 --- a/test/integration/default_elements.cc +++ b/test/integration/default_elements.cc @@ -187,27 +187,27 @@ TEST(ExplicitlySetInFile, ToString) << "\n" << " \n" << " \n" - << " 1.0\n" + << " 1\n" << " 0 0 0\n" << " \n" << " \n" << " EARTH_WGS84\n" - << " 0.0\n" - << " 0.0\n" - << " 0.0\n" - << " 0.0\n" + << " 0\n" + << " 0\n" + << " 0\n" + << " 0\n" << " \n" << " 0 0 -9.8\n" - << " 5.5645e-6 22.8758e-6 -42.3884e-6\n" + << " 6e-06 2.3e-05 -4.2e-05\n" << " \n" << " \n" << " 0.001\n" - << " 1.0\n" + << " 1\n" << " 1000\n" << " \n" << " \n" - << " 0.4 0.4 0.4 1.0\n" - << " .7 .7 .7 1\n" + << " 0.4 0.4 0.4 1\n" + << " 0.7 0.7 0.7 1\n" << " true\n" << " \n" << " \n" @@ -221,27 +221,27 @@ TEST(ExplicitlySetInFile, ToString) << "\n" << " \n" << " \n" - << " 1.0\n" + << " 1\n" << " 0 0 0\n" << " \n" << " \n" << " EARTH_WGS84\n" - << " 0.0\n" - << " 0.0\n" - << " 0.0\n" - << " 0.0\n" + << " 0\n" + << " 0\n" + << " 0\n" + << " 0\n" << " \n" << " 0 0 -9.8\n" - << " 5.5645e-6 22.8758e-6 -42.3884e-6\n" + << " 6e-06 2.3e-05 -4.2e-05\n" << " \n" << " \n" << " 0.001\n" - << " 1.0\n" + << " 1\n" << " 1000\n" << " \n" << " \n" - << " 0.4 0.4 0.4 1.0\n" - << " .7 .7 .7 1\n" + << " 0.4 0.4 0.4 1\n" + << " 0.7 0.7 0.7 1\n" << " true\n" << " \n" << " \n" diff --git a/test/integration/include_custom_model_expected_output.sdf b/test/integration/include_custom_model_expected_output.sdf index e4584cd98..f554228c8 100644 --- a/test/integration/include_custom_model_expected_output.sdf +++ b/test/integration/include_custom_model_expected_output.sdf @@ -211,7 +211,7 @@ - 1.0469999999999999 + 1.047 1280 @@ -457,16 +457,16 @@ 0 0 -9.8 - 5.5645e-6 22.8758e-6 -42.3884e-6 + 6e-06 2.3e-05 -4.2e-05 0.001 - 1.0 + 1 1000 - 0.4 0.4 0.4 1.0 - .7 .7 .7 1 + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 true diff --git a/test/integration/include_custom_nested_model_expected_output.sdf b/test/integration/include_custom_nested_model_expected_output.sdf index 959a6b2cc..e7b3115f2 100644 --- a/test/integration/include_custom_nested_model_expected_output.sdf +++ b/test/integration/include_custom_nested_model_expected_output.sdf @@ -11,7 +11,7 @@ 0.126164 0 0 - 0.416519 + 0.41651899999999997 0 0.481014 @@ -80,13 +80,13 @@ -1.06 0 0 0 0 3.14 - 1.047 + 1.0469999999999999 320 240 - 0.1 + 0.10000000000000001 100 @@ -158,13 +158,13 @@ - 1.047 + 1.0469999999999999 320 240 - 0.1 + 0.10000000000000001 100 @@ -181,7 +181,7 @@ -0.2 0 0.3 0 0 3.14 - 0.05 + 0.050000000000000003 @@ -203,10 +203,10 @@ 2 - 0.145833 + 0.14583299999999999 0 0 - 0.145833 + 0.14583299999999999 0 0.125 @@ -214,7 +214,7 @@ - 0.3 + 0.29999999999999999 @@ -226,7 +226,7 @@ - 0.3 + 0.29999999999999999 @@ -239,10 +239,10 @@ 2 - 0.145833 + 0.14583299999999999 0 0 - 0.145833 + 0.14583299999999999 0 0.125 @@ -250,7 +250,7 @@ - 0.3 + 0.29999999999999999 @@ -262,7 +262,7 @@ - 0.3 + 0.29999999999999999 @@ -275,10 +275,10 @@ 2 - 0.145833 + 0.14583299999999999 0 0 - 0.145833 + 0.14583299999999999 0 0.125 @@ -286,7 +286,7 @@ - 0.3 + 0.29999999999999999 @@ -298,7 +298,7 @@ - 0.3 + 0.29999999999999999 @@ -311,8 +311,8 @@ 0 0 1 - -1.79769e+308 - 1.79769e+308 + -1.7976900000000001e+308 + 1.7976900000000001e+308 @@ -322,8 +322,8 @@ 0 0 1 - -1.79769e+308 - 1.79769e+308 + -1.7976900000000001e+308 + 1.7976900000000001e+308 @@ -334,16 +334,16 @@ 0 0 -9.8 - 5.5645e-6 22.8758e-6 -42.3884e-6 + 6e-06 2.3e-05 -4.2e-05 0.001 - 1.0 + 1 1000 - 0.4 0.4 0.4 1.0 - .7 .7 .7 1 + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 true diff --git a/test/integration/pose_1_9_sdf.cc b/test/integration/pose_1_9_sdf.cc index c70a98037..492ff2cdc 100644 --- a/test/integration/pose_1_9_sdf.cc +++ b/test/integration/pose_1_9_sdf.cc @@ -788,7 +788,7 @@ TEST(Pose1_9, ToStringWithDegreesFalse) EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='0'"); + EXPECT_PRED2(contains, elemStr, "degrees='false'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } @@ -811,7 +811,7 @@ TEST(Pose1_9, ToStringWithDegreesTrue) EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='1'"); + EXPECT_PRED2(contains, elemStr, "degrees='true'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } @@ -863,7 +863,7 @@ TEST(Pose1_9, ToStringWithEulerRPYDegreesTrue) EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='1'"); + EXPECT_PRED2(contains, elemStr, "degrees='true'"); EXPECT_PRED2(contains, elemStr, "rotation_format='euler_rpy'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } @@ -887,9 +887,11 @@ TEST(Pose1_9, ToStringWithQuatXYZ) ASSERT_NE(nullptr, poseValueParam); EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.7071068 0 0 0.7071068")); + // The string output has changed as it was parsed from the value, instead of + // the original string. std::string elemStr = poseElem->ToString(""); EXPECT_PRED2(contains, elemStr, "rotation_format='quat_xyzw'"); - EXPECT_PRED2(contains, elemStr, "0.7071068 0 0 0.7071068"); + EXPECT_PRED2(contains, elemStr, "0.707107 0 0 0.707107"); } ////////////////////////////////////////////////// @@ -915,10 +917,12 @@ TEST(Pose1_9, ToStringWithQuatXYZWDegreesFalse) ASSERT_NE(nullptr, poseValueParam); EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.7071068 0 0 0.7071068")); + // The string output has changed as it was parsed from the value, instead of + // the original string. std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='0'"); + EXPECT_PRED2(contains, elemStr, "degrees='false'"); EXPECT_PRED2(contains, elemStr, "rotation_format='quat_xyzw'"); - EXPECT_PRED2(contains, elemStr, "0.7071068 0 0 0.7071068"); + EXPECT_PRED2(contains, elemStr, "0.707107 0 0 0.707107"); } ////////////////////////////////////////////////// @@ -941,18 +945,22 @@ TEST(Pose1_9, ToStringAfterChangingDegreeAttribute) std::string elemStr = poseElem->ToString(""); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); - // Changing to degrees + // Changing to attribute to degrees, however this does not modify the + // value of the underlying Param. Reparse needs to be called, which uses + // the input from SetFromString, to get a new value. sdf::ParamPtr degreesAttrib = poseElem->GetAttribute("degrees"); ASSERT_NE(nullptr, degreesAttrib); ASSERT_TRUE(degreesAttrib->Set(true)); + EXPECT_TRUE(valParam->Reparse()); + elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='1'"); + EXPECT_PRED2(contains, elemStr, "degrees='true'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); // Changing back to radians ASSERT_TRUE(degreesAttrib->Set(false)); elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='0'"); + EXPECT_PRED2(contains, elemStr, "degrees='false'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } diff --git a/test/integration/print_config.cc b/test/integration/print_config.cc index a3cc9aba1..ded642ab2 100644 --- a/test/integration/print_config.cc +++ b/test/integration/print_config.cc @@ -187,6 +187,9 @@ R"( auto *includeMergedModel = world->ModelByIndex(0); ASSERT_NE(includeMergedModel, nullptr); + // The expected output pose string here still contains a -0 on the pitch value + // as it was set using ignition::math::Pose3d::operator<<, this test will have + // to be modified when we start using ignitionrobotics/ign-math#206. const std::string expectedIncludeMerge = R"( diff --git a/test/sdf/includes_rotations_in_degrees.sdf b/test/sdf/includes_rotations_in_degrees.sdf new file mode 100644 index 000000000..ae04da1d6 --- /dev/null +++ b/test/sdf/includes_rotations_in_degrees.sdf @@ -0,0 +1,12 @@ + + + + + + test_model + parent_model + 1 2 3 30.009 44.991 -60.009 + + + + diff --git a/test/sdf/includes_rotations_in_quaternions.sdf b/test/sdf/includes_rotations_in_quaternions.sdf new file mode 100644 index 000000000..08795bd22 --- /dev/null +++ b/test/sdf/includes_rotations_in_quaternions.sdf @@ -0,0 +1,14 @@ + + + + + + test_model + parent_model + + 1 2 3 0.391948 0.200425 -0.532046 0.723279 + + + + + diff --git a/test/sdf/includes_rotations_in_radians.sdf b/test/sdf/includes_rotations_in_radians.sdf new file mode 100644 index 000000000..ef91f408a --- /dev/null +++ b/test/sdf/includes_rotations_in_radians.sdf @@ -0,0 +1,12 @@ + + + + + + test_model + parent_model + 1 2 3 0.523756 0.785241 -1.04735 + + + + diff --git a/test/sdf/rotations_in_degrees.sdf b/test/sdf/rotations_in_degrees.sdf new file mode 100644 index 000000000..9dbba3c44 --- /dev/null +++ b/test/sdf/rotations_in_degrees.sdf @@ -0,0 +1,8 @@ + + + + + 1 2 3 30.009 44.991 -60.009 + + + diff --git a/test/sdf/rotations_in_degrees_high_snap_tolerance.sdf b/test/sdf/rotations_in_degrees_high_snap_tolerance.sdf new file mode 100644 index 000000000..273564354 --- /dev/null +++ b/test/sdf/rotations_in_degrees_high_snap_tolerance.sdf @@ -0,0 +1,8 @@ + + + + + 1 2 3 30 48.5 60 + + + diff --git a/test/sdf/rotations_in_quaternions.sdf b/test/sdf/rotations_in_quaternions.sdf new file mode 100644 index 000000000..2a8447f38 --- /dev/null +++ b/test/sdf/rotations_in_quaternions.sdf @@ -0,0 +1,10 @@ + + + + + + 1 2 3 0.391948 0.200425 -0.532046 0.723279 + + + + diff --git a/test/sdf/rotations_in_radians.sdf b/test/sdf/rotations_in_radians.sdf new file mode 100644 index 000000000..36a218bd8 --- /dev/null +++ b/test/sdf/rotations_in_radians.sdf @@ -0,0 +1,8 @@ + + + + + 1 2 3 0.523756 0.785241 -1.04735 + + + diff --git a/test/sdf/rotations_in_unnormalized_degrees.sdf b/test/sdf/rotations_in_unnormalized_degrees.sdf new file mode 100644 index 000000000..ba688c750 --- /dev/null +++ b/test/sdf/rotations_in_unnormalized_degrees.sdf @@ -0,0 +1,8 @@ + + + + + 1 2 3 390.009 764.991 -420.009 + + + diff --git a/test/sdf/rotations_in_unnormalized_radians.sdf b/test/sdf/rotations_in_unnormalized_radians.sdf new file mode 100644 index 000000000..4d9578932 --- /dev/null +++ b/test/sdf/rotations_in_unnormalized_radians.sdf @@ -0,0 +1,8 @@ + + + + + 1 2 3 -5.75943 -11.78112 5.23583 + + +