diff --git a/include/sdf/Element.hh b/include/sdf/Element.hh index d9fa9eb92..dddfad993 100644 --- 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 index c59fb683d..eca2a8573 100644 --- 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; + using ElementWeakPtr = std::weak_ptr; + class SDFORMAT_VISIBLE Param; /// \def ParamPtr @@ -180,13 +184,44 @@ namespace sdf /// of the parameter. nullopt otherwise. public: std::optional 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 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 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 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 index 523f0894b..c98958ffe 100644 --- a/sdf/1.9/pose.sdf +++ b/sdf/1.9/pose.sdf @@ -1,11 +1,19 @@ - - A position(x,y,z) and orientation(roll, pitch yaw) with respect to the frame named in the relative_to attribute. + + + 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. + - + Name of frame relative to which the pose is applied. + + + Whether or not the euler angles are in degrees, otherwise they will be interpreted as radians by default. + + + diff --git a/src/Element.cc b/src/Element.cc index bb6d748f4..0be5fd657 100644 --- 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(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( + _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 { diff --git a/src/Element_TEST.cc b/src/Element_TEST.cc index 8de937577..de58a067f 100644 --- a/src/Element_TEST.cc +++ b/src/Element_TEST.cc @@ -191,40 +191,42 @@ TEST(Element, ReferenceSDF) ///////////////////////////////////////////////// TEST(Element, AddValue) { - sdf::Element elem; + sdf::ElementPtr elem = std::make_shared(); - elem.SetName("test"); - elem.AddValue("string", "foo", false, "foo description"); + elem->SetName("test"); + elem->AddValue("string", "foo", false, "foo description"); - sdf::ParamPtr param = elem.GetValue(); + sdf::ParamPtr param = elem->GetValue(); ASSERT_TRUE(param->IsType()); ASSERT_EQ(param->GetKey(), "test"); ASSERT_EQ(param->GetTypeName(), "string"); ASSERT_EQ(param->GetDefaultAsString(), "foo"); ASSERT_EQ(param->GetDescription(), "foo description"); + ASSERT_NE(param->GetParentElement(), nullptr); + EXPECT_EQ(param->GetParentElement(), elem); } ///////////////////////////////////////////////// TEST(Element, AddAttribute) { - sdf::Element elem; + sdf::ElementPtr elem = std::make_shared(); - ASSERT_EQ(elem.GetAttributeCount(), 0UL); + ASSERT_EQ(elem->GetAttributeCount(), 0UL); - elem.AddAttribute("test", "string", "foo", false, "foo description"); - ASSERT_EQ(elem.GetAttributeCount(), 1UL); + elem->AddAttribute("test", "string", "foo", false, "foo description"); + ASSERT_EQ(elem->GetAttributeCount(), 1UL); - elem.AddAttribute("attr", "float", "0.0", false, "float description"); - ASSERT_EQ(elem.GetAttributeCount(), 2UL); + elem->AddAttribute("attr", "float", "0.0", false, "float description"); + ASSERT_EQ(elem->GetAttributeCount(), 2UL); - sdf::ParamPtr param = elem.GetAttribute("test"); + sdf::ParamPtr param = elem->GetAttribute("test"); ASSERT_TRUE(param->IsType()); ASSERT_EQ(param->GetKey(), "test"); ASSERT_EQ(param->GetTypeName(), "string"); ASSERT_EQ(param->GetDefaultAsString(), "foo"); ASSERT_EQ(param->GetDescription(), "foo description"); - param = elem.GetAttribute("attr"); + param = elem->GetAttribute("attr"); ASSERT_TRUE(param->IsType()); ASSERT_EQ(param->GetKey(), "attr"); ASSERT_EQ(param->GetTypeName(), "float"); @@ -235,48 +237,48 @@ TEST(Element, AddAttribute) ///////////////////////////////////////////////// TEST(Element, GetAttributeSet) { - sdf::Element elem; - ASSERT_EQ(elem.GetAttributeCount(), 0UL); - elem.AddAttribute("test", "string", "foo", false, "foo description"); - ASSERT_EQ(elem.GetAttributeCount(), 1UL); + sdf::ElementPtr elem = std::make_shared(); + ASSERT_EQ(elem->GetAttributeCount(), 0UL); + elem->AddAttribute("test", "string", "foo", false, "foo description"); + ASSERT_EQ(elem->GetAttributeCount(), 1UL); - EXPECT_FALSE(elem.GetAttributeSet("test")); - elem.GetAttribute("test")->Set("asdf"); - EXPECT_TRUE(elem.GetAttributeSet("test")); + EXPECT_FALSE(elem->GetAttributeSet("test")); + elem->GetAttribute("test")->Set("asdf"); + EXPECT_TRUE(elem->GetAttributeSet("test")); } ///////////////////////////////////////////////// TEST(Element, RemoveAttribute) { - sdf::Element elem; - ASSERT_EQ(elem.GetAttributeCount(), 0UL); + sdf::ElementPtr elem = std::make_shared(); + ASSERT_EQ(elem->GetAttributeCount(), 0UL); - elem.AddAttribute("test", "string", "foo", false, "foo description"); - elem.AddAttribute("attr", "float", "0.0", false, "float description"); - ASSERT_EQ(elem.GetAttributeCount(), 2UL); + elem->AddAttribute("test", "string", "foo", false, "foo description"); + elem->AddAttribute("attr", "float", "0.0", false, "float description"); + ASSERT_EQ(elem->GetAttributeCount(), 2UL); - elem.RemoveAttribute("test"); - EXPECT_EQ(elem.GetAttributeCount(), 1UL); - EXPECT_EQ(elem.GetAttribute("test"), nullptr); - EXPECT_NE(elem.GetAttribute("attr"), nullptr); + elem->RemoveAttribute("test"); + EXPECT_EQ(elem->GetAttributeCount(), 1UL); + EXPECT_EQ(elem->GetAttribute("test"), nullptr); + EXPECT_NE(elem->GetAttribute("attr"), nullptr); } ///////////////////////////////////////////////// TEST(Element, RemoveAllAttributes) { - sdf::Element elem; - ASSERT_EQ(elem.GetAttributeCount(), 0UL); - - elem.AddAttribute("test", "string", "foo", false, "foo description"); - elem.AddAttribute("test2", "string", "foo", false, "foo description"); - elem.AddAttribute("attr", "float", "0.0", false, "float description"); - ASSERT_EQ(elem.GetAttributeCount(), 3UL); - - elem.RemoveAllAttributes(); - EXPECT_EQ(elem.GetAttributeCount(), 0UL); - EXPECT_EQ(elem.GetAttribute("test"), nullptr); - EXPECT_EQ(elem.GetAttribute("test2"), nullptr); - EXPECT_EQ(elem.GetAttribute("attr"), nullptr); + sdf::ElementPtr elem = std::make_shared(); + ASSERT_EQ(elem->GetAttributeCount(), 0UL); + + elem->AddAttribute("test", "string", "foo", false, "foo description"); + elem->AddAttribute("test2", "string", "foo", false, "foo description"); + elem->AddAttribute("attr", "float", "0.0", false, "float description"); + ASSERT_EQ(elem->GetAttributeCount(), 3UL); + + elem->RemoveAllAttributes(); + EXPECT_EQ(elem->GetAttributeCount(), 0UL); + EXPECT_EQ(elem->GetAttribute("test"), nullptr); + EXPECT_EQ(elem->GetAttribute("test2"), nullptr); + EXPECT_EQ(elem->GetAttribute("attr"), nullptr); } ///////////////////////////////////////////////// @@ -397,6 +399,9 @@ TEST(Element, Clone) sdf::ElementPtr child = std::make_shared(); sdf::ElementPtr desc = std::make_shared(); + parent->SetName("parent"); + child->SetName("child"); + parent->InsertElement(child); ASSERT_NE(parent->GetFirstElement(), nullptr); @@ -430,6 +435,15 @@ TEST(Element, Clone) ASSERT_NE(newelem->GetIncludeElement(), nullptr); EXPECT_EQ("include", newelem->GetIncludeElement()->GetName()); ASSERT_TRUE(newelem->GetExplicitlySetInFile()); + + ASSERT_NE(nullptr, parent->GetValue()->GetParentElement()); + EXPECT_EQ(parent, parent->GetValue()->GetParentElement()); + + ASSERT_NE(nullptr, newelem->GetValue()->GetParentElement()); + EXPECT_EQ(newelem, newelem->GetValue()->GetParentElement()); + + auto clonedAttribs = newelem->GetAttributes(); + EXPECT_EQ(newelem, clonedAttribs[0]->GetParentElement()); } ///////////////////////////////////////////////// @@ -683,16 +697,16 @@ TEST(Element, DocLeftPane) ///////////////////////////////////////////////// TEST(Element, DocRightPane) { - sdf::Element elem; + sdf::ElementPtr elem = std::make_shared(); - elem.SetDescription("Element description"); - elem.AddAttribute("test", "string", "foo", false, "foo description"); - elem.AddValue("string", "val", false, "val description"); - elem.AddElementDescription(std::make_shared()); + elem->SetDescription("Element description"); + elem->AddAttribute("test", "string", "foo", false, "foo description"); + elem->AddValue("string", "val", false, "val description"); + elem->AddElementDescription(std::make_shared()); std::string html; int index = 1; - elem.PrintDocRightPane(html, 0, index); + elem->PrintDocRightPane(html, 0, index); ASSERT_EQ(html, "<>
\n" "
\n" @@ -741,11 +755,11 @@ TEST(Element, SetEmpty) ///////////////////////////////////////////////// TEST(Element, Set) { - sdf::Element elem; + sdf::ElementPtr elem = std::make_shared(); - elem.AddValue("string", "val", false, "val description"); + elem->AddValue("string", "val", false, "val description"); - ASSERT_TRUE(elem.Set("hello")); + ASSERT_TRUE(elem->Set("hello")); } ///////////////////////////////////////////////// @@ -781,6 +795,8 @@ TEST(Element, Copy) ASSERT_EQ(param->GetTypeName(), "string"); ASSERT_EQ(param->GetDefaultAsString(), "val"); ASSERT_EQ(param->GetDescription(), "val description"); + ASSERT_NE(param->GetParentElement(), nullptr); + EXPECT_EQ(param->GetParentElement(), dest); ASSERT_EQ(dest->GetAttributeCount(), 1UL); ASSERT_TRUE(dest->GetExplicitlySetInFile()); @@ -790,6 +806,8 @@ TEST(Element, Copy) ASSERT_EQ(param->GetTypeName(), "string"); ASSERT_EQ(param->GetDefaultAsString(), "foo"); ASSERT_EQ(param->GetDescription(), "foo description"); + ASSERT_NE(param->GetParentElement(), nullptr); + EXPECT_EQ(param->GetParentElement(), dest); ASSERT_NE(dest->GetFirstElement(), nullptr); ASSERT_NE(dest->GetIncludeElement(), nullptr); @@ -815,6 +833,8 @@ TEST(Element, CopyDestValue) ASSERT_EQ(param->GetTypeName(), "string"); ASSERT_EQ(param->GetDefaultAsString(), "val"); ASSERT_EQ(param->GetDescription(), "val description"); + ASSERT_NE(param->GetParentElement(), nullptr); + EXPECT_EQ(param->GetParentElement(), dest); ASSERT_EQ(dest->GetAttributeCount(), 1UL); param = dest->GetAttribute("test"); @@ -823,6 +843,8 @@ TEST(Element, CopyDestValue) ASSERT_EQ(param->GetTypeName(), "string"); ASSERT_EQ(param->GetDefaultAsString(), "foo"); ASSERT_EQ(param->GetDescription(), "foo description"); + ASSERT_NE(param->GetParentElement(), nullptr); + EXPECT_EQ(param->GetParentElement(), dest); ASSERT_NE(dest->GetFirstElement(), nullptr); } diff --git a/src/Param.cc b/src/Param.cc index 5c3811e51..c658c4a1a 100644 --- a/src/Param.cc +++ b/src/Param.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -29,6 +30,7 @@ #include "sdf/Assert.hh" #include "sdf/Param.hh" #include "sdf/Types.hh" +#include "sdf/Element.hh" using namespace sdf; @@ -68,6 +70,7 @@ Param::Param(const std::string &_key, const std::string &_typeName, this->dataPtr->typeName = _typeName; this->dataPtr->description = _description; this->dataPtr->set = false; + this->dataPtr->ignoreParentAttributes = false; SDF_ASSERT(this->ValueFromString(_default), "Invalid parameter"); this->dataPtr->defaultValue = this->dataPtr->value; @@ -102,6 +105,7 @@ Param::Param(const std::string &_key, const std::string &_typeName, this->dataPtr->value = valCopy; } +////////////////////////////////////////////////// Param::Param(const Param &_param) : dataPtr(std::make_unique(*_param.dataPtr)) { @@ -298,7 +302,11 @@ std::string Param::GetAsString() const { StringStreamClassicLocale ss; - ss << ParamStreamer{ this->dataPtr->value }; + if (this->dataPtr->strValue.has_value()) + ss << this->dataPtr->strValue.value(); + else + ss << ParamStreamer{ this->dataPtr->value }; + return ss.str(); } @@ -427,6 +435,106 @@ bool ParseColorUsingStringStream(const std::string &_input, return isValidColor; } +////////////////////////////////////////////////// +/// \brief Helper function for Param::ValueFromString for parsing pose. This +/// checks the pose components specified in _input are xyzrpy +/// (expects 6 values) and whether to parse the rotation values as degrees using +/// the parent element attributes. +/// \param[in] _input Input string. +/// \param[in] _key Key of the parameter, used for error message. +/// \param[in] _attributes Attributes associated to this pose. +/// \param[out] _value This will be set with the parsed value. +/// \return True if parsing pose succeeded. +bool ParsePoseUsingStringStream(const std::string &_input, + const std::string &_key, const Param_V &_attributes, + ParamPrivate::ParamVariant &_value) +{ + StringStreamClassicLocale ss(_input); + std::string token; + std::array values; + std::size_t valueIndex = 0; + double v; + bool isValidPose = true; + while (ss >> token) + { + try + { + v = std::stod(token); + } + // Catch invalid argument exception from std::stod + catch(std::invalid_argument &) + { + sdferr << "Invalid argument. Unable to set value ["<< _input + << "] for key [" << _key << "].\n"; + isValidPose = false; + break; + } + // Catch out of range exception from std::stod + catch(std::out_of_range &) + { + sdferr << "Out of range. Unable to set value [" << token + << "] for key [" << _key << "].\n"; + isValidPose = false; + break; + } + + if (!std::isfinite(v)) + { + sdferr << "Pose values must be finite.\n"; + isValidPose = false; + break; + } + + if (valueIndex >= 6u) + { + sdferr << "Pose values can only accept 6 values.\n"; + isValidPose = false; + break; + } + + values[valueIndex++] = v; + } + + if (!isValidPose) + return false; + + if (valueIndex != 6u) + { + sdferr << "The value for //pose must have 6 values, but " + << valueIndex << " were found instead.\n"; + return false; + } + + const bool defaultParseAsDegrees = false; + bool parseAsDegrees = defaultParseAsDegrees; + + for (const auto &p : _attributes) + { + if (p->GetKey() == "degrees") + { + if (!p->Get(parseAsDegrees)) + { + sdferr << "Invalid boolean value found for attribute " + "//pose[@degrees].\n"; + return false; + } + break; + } + } + + if (parseAsDegrees) + { + _value = ignition::math::Pose3d(values[0], values[1], values[2], + IGN_DTOR(values[3]), IGN_DTOR(values[4]), IGN_DTOR(values[5])); + } + else + { + _value = ignition::math::Pose3d(values[0], values[1], values[2], + values[3], values[4], values[5]); + } + return true; +} + ////////////////////////////////////////////////// bool Param::ValueFromString(const std::string &_value) { @@ -558,8 +666,14 @@ bool ParamPrivate::ValueFromStringImpl(const std::string &_typeName, { if (!tmp.empty()) { - return ParseUsingStringStream( - tmp, this->key, _valueToSet); + const ElementPtr p = this->parentElement.lock(); + if (!this->ignoreParentAttributes && p) + { + return ParsePoseUsingStringStream( + tmp, this->key, p->GetAttributes(), _valueToSet); + } + return ParsePoseUsingStringStream( + tmp, this->key, {}, _valueToSet); } } else if (_typeName == "ignition::math::Quaterniond" || @@ -595,8 +709,11 @@ bool ParamPrivate::ValueFromStringImpl(const std::string &_typeName, } ////////////////////////////////////////////////// -bool Param::SetFromString(const std::string &_value) +bool Param::SetFromString(const std::string &_value, + bool _ignoreParentAttributes) { + this->dataPtr->ignoreParentAttributes = _ignoreParentAttributes; + this->dataPtr->strValue = _value; std::string str = sdf::trim(_value.c_str()); if (str.empty() && this->dataPtr->required) @@ -628,13 +745,60 @@ bool Param::SetFromString(const std::string &_value) return this->dataPtr->set; } +////////////////////////////////////////////////// +bool Param::SetFromString(const std::string &_value) +{ + return this->SetFromString(_value, false); +} + +////////////////////////////////////////////////// +ElementPtr Param::GetParentElement() const +{ + return this->dataPtr->parentElement.lock(); +} + +////////////////////////////////////////////////// +void Param::SetParentElement(ElementPtr _parentElement) +{ + this->dataPtr->parentElement = _parentElement; + this->Reparse(); +} + ////////////////////////////////////////////////// void Param::Reset() { this->dataPtr->value = this->dataPtr->defaultValue; + this->dataPtr->strValue = std::nullopt; this->dataPtr->set = false; } +////////////////////////////////////////////////// +bool Param::Reparse() +{ + if (!this->dataPtr->strValue.has_value()) + return false; + + const std::string strVal = this->dataPtr->strValue.value(); + if (!this->SetFromString(strVal, this->dataPtr->ignoreParentAttributes)) + { + if (const auto parentElement = this->dataPtr->parentElement.lock()) + { + sdferr << "Failed to set value '" << strVal << "' to key [" + << this->GetKey() << "] for new parent element of name '" + << parentElement->GetName() << "', reverting to previous value '" + << this->GetAsString() << "'.\n"; + } + else + { + sdferr << "Failed to set value '" << strVal << "' to key [" + << this->GetKey() << "] without a parent element, " + << "reverting to previous value '" << this->GetAsString() << "'.\n"; + } + return false; + } + return true; +} + ////////////////////////////////////////////////// ParamPtr Param::Clone() const { @@ -677,6 +841,13 @@ bool Param::GetSet() const return this->dataPtr->set; } +///////////////////////////////////////////////// +bool Param::IgnoresParentElementAttribute() const +{ + const auto parentElement = this->dataPtr->parentElement.lock(); + return !parentElement || this->dataPtr->ignoreParentAttributes; +} + ///////////////////////////////////////////////// bool Param::ValidateValue() const { diff --git a/src/Param_TEST.cc b/src/Param_TEST.cc index ece457299..5835b40c6 100644 --- a/src/Param_TEST.cc +++ b/src/Param_TEST.cc @@ -26,6 +26,7 @@ #include #include "sdf/Exception.hh" +#include "sdf/Element.hh" #include "sdf/Param.hh" bool check_double(const std::string &num) @@ -490,6 +491,270 @@ TEST(Param, MinMaxViolation) } } +////////////////////////////////////////////////// +TEST(Param, SettingParentElement) +{ + sdf::ElementPtr parentElement = std::make_shared(); + + sdf::Param doubleParam("key", "double", "1.0", false, "description"); + doubleParam.SetParentElement(parentElement); + + ASSERT_NE(nullptr, doubleParam.GetParentElement()); + EXPECT_EQ(parentElement, doubleParam.GetParentElement()); + + // Set a new parent Element + sdf::ElementPtr newParentElement = std::make_shared(); + + doubleParam.SetParentElement(newParentElement); + ASSERT_NE(nullptr, doubleParam.GetParentElement()); + EXPECT_EQ(newParentElement, doubleParam.GetParentElement()); + + // Remove the parent Element + doubleParam.SetParentElement(nullptr); + EXPECT_EQ(nullptr, doubleParam.GetParentElement()); +} + +////////////////////////////////////////////////// +TEST(Param, CopyConstructor) +{ + sdf::ElementPtr parentElement = std::make_shared(); + + sdf::Param doubleParam("key", "double", "1.0", false, "description"); + doubleParam.SetParentElement(parentElement); + + ASSERT_NE(nullptr, doubleParam.GetParentElement()); + EXPECT_EQ(parentElement, doubleParam.GetParentElement()); + + sdf::Param newParam(doubleParam); + ASSERT_NE(nullptr, newParam.GetParentElement()); + EXPECT_EQ(parentElement, newParam.GetParentElement()); +} + +////////////////////////////////////////////////// +TEST(Param, EqualOperator) +{ + sdf::ElementPtr parentElement = std::make_shared(); + + sdf::Param doubleParam("key", "double", "1.0", false, "description"); + doubleParam.SetParentElement(parentElement); + + ASSERT_NE(nullptr, doubleParam.GetParentElement()); + EXPECT_EQ(parentElement, doubleParam.GetParentElement()); + + sdf::Param newParam = doubleParam; + ASSERT_NE(nullptr, newParam.GetParentElement()); + EXPECT_EQ(parentElement, newParam.GetParentElement()); +} + +////////////////////////////////////////////////// +TEST(Param, DestroyParentElementAfterConstruct) +{ + sdf::ParamPtr param = nullptr; + + { + sdf::ElementPtr parentElement = std::make_shared(); + + param = std::make_shared( + "key", "double", "1.0", false, "description"); + param->SetParentElement(parentElement); + } + + ASSERT_NE(nullptr, param); + EXPECT_EQ(nullptr, param->GetParentElement()); +} + +////////////////////////////////////////////////// +TEST(Param, ReparsingAfterSetDouble) +{ + sdf::Param doubleParam("key", "double", "1.0", false, "description"); + + // Reparsing without setting values will fail. Value will still be the + // default. + EXPECT_FALSE(doubleParam.Reparse()); + double value; + EXPECT_TRUE(doubleParam.Get(value)); + EXPECT_DOUBLE_EQ(value, 1.0); + + // Reparsing after setting a value will pass. + ASSERT_TRUE(doubleParam.Set(5.)); + EXPECT_TRUE(doubleParam.Get(value)); + EXPECT_DOUBLE_EQ(value, 5.0); + EXPECT_TRUE(doubleParam.Reparse()); + + // Value will be as expected, as the value was set using the explcit Set + // function, and no parent element attributes are meant to change the value + // for type double. + EXPECT_TRUE(doubleParam.Get(value)); + EXPECT_DOUBLE_EQ(value, 5.0); +} + +////////////////////////////////////////////////// +TEST(Param, ReparsingAfterSetFromStringDouble) +{ + sdf::Param doubleParam("key", "double", "1.0", false, "description"); + + // Reparsing without setting values will fail. Value will still be the + // default. + EXPECT_FALSE(doubleParam.Reparse()); + double value; + EXPECT_TRUE(doubleParam.Get(value)); + EXPECT_DOUBLE_EQ(value, 1.0); + + // Reparsing after setting a value from string will pass. + ASSERT_TRUE(doubleParam.SetFromString("5.0")); + EXPECT_TRUE(doubleParam.Get(value)); + EXPECT_DOUBLE_EQ(value, 5.0); + EXPECT_TRUE(doubleParam.Reparse()); + + // Value will be as expected, as no parent element attributes are meant to + // change the value for type double. + EXPECT_TRUE(doubleParam.Get(value)); + EXPECT_DOUBLE_EQ(value, 5.0); +} + +///////////////////////////////////////////////// +TEST(Param, ReparsingAfterSetPose) +{ + using Pose = ignition::math::Pose3d; + + sdf::Param poseParam("", "pose", "1 2 3 0.4 0.5 0.6", false, "description"); + Pose value; + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), value); + + // Reparsing without setting values will fail. + EXPECT_FALSE(poseParam.Reparse()); + + // Reparsing after setting values will pass. Note that the value was + // explicitly set using the Set function, reparsing will always yield the same + // value moving forwards. + EXPECT_TRUE(poseParam.Set(Pose(2, 3, 4, 0.5, 0.6, 0.7))); + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_EQ(Pose(2, 3, 4, 0.5, 0.6, 0.7), value); + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + sdf::ParamPtr degreesAttrib = poseElem->GetAttribute("degrees"); + ASSERT_NE(nullptr, degreesAttrib); + + // Setting parent with @degrees as false, value will remain the same. + poseParam.SetParentElement(poseElem); + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_EQ(Pose(2, 3, 4, 0.5, 0.6, 0.7), value); + + // Changing attribute @degrees to true, value remains the same as it was + // explicitly Set. + ASSERT_TRUE(degreesAttrib->Set(true)); + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_EQ(Pose(2, 3, 4, 0.5, 0.6, 0.7), value); + + // After reparse, value will still remain the same, as the explicit Set + // function was used. + EXPECT_TRUE(poseParam.Reparse()); + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_EQ(Pose(2, 3, 4, 0.5, 0.6, 0.7), value); +} + +///////////////////////////////////////////////// +TEST(Param, ReparsingAfterSetFromStringPose) +{ + using Pose = ignition::math::Pose3d; + + sdf::Param poseParam("", "pose", "1 2 3 0.4 0.5 0.6", false, "description"); + Pose value; + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), value); + + // Reparsing without setting values will fail. + EXPECT_FALSE(poseParam.Reparse()); + + // Reparsing after setting values will pass. Note that the value was + // set using SetFromString. + EXPECT_TRUE(poseParam.SetFromString("2 3 4 0.5 0.6 0.7")); + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_EQ(Pose(2, 3, 4, 0.5, 0.6, 0.7), value); + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + sdf::ParamPtr degreesAttrib = poseElem->GetAttribute("degrees"); + ASSERT_NE(nullptr, degreesAttrib); + + // Setting parent with @degrees as false, value will remain the same. + poseParam.SetParentElement(poseElem); + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_EQ(Pose(2, 3, 4, 0.5, 0.6, 0.7), value); + + // Changing attribute @degrees to true, value remains the same before + // reparsing. + ASSERT_TRUE(degreesAttrib->Set(true)); + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_EQ(Pose(2, 3, 4, 0.5, 0.6, 0.7), value); + + // After reparse, rotation values will be parsed as degrees. + EXPECT_TRUE(poseParam.Reparse()); + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_EQ(Pose(2, 3, 4, IGN_DTOR(0.5), IGN_DTOR(0.6), IGN_DTOR(0.7)), value); +} + +///////////////////////////////////////////////// +TEST(Param, IgnoresParentElementAttribute) +{ + { + // Without parent + sdf::Param doubleParam("key", "double", "1.0", false, "description"); + EXPECT_TRUE(doubleParam.IgnoresParentElementAttribute()); + } + + { + // With parent + sdf::Param doubleParam("key", "double", "1.0", false, "description"); + sdf::ElementPtr parentElement = std::make_shared(); + doubleParam.SetParentElement(parentElement); + EXPECT_FALSE(doubleParam.IgnoresParentElementAttribute()); + } + + { + // Param from parent element + sdf::ElementPtr elem(new sdf::Element); + elem->SetName("double"); + elem->AddValue("double", "0", true); + + sdf::ParamPtr valParam = elem->GetValue(); + ASSERT_NE(nullptr, valParam); + EXPECT_FALSE(valParam->IgnoresParentElementAttribute()); + } + + { + // With parent using Set and SetFromString + sdf::Param doubleParam("key", "double", "1.0", false, "description"); + sdf::ElementPtr parentElement = std::make_shared(); + doubleParam.SetParentElement(parentElement); + ASSERT_TRUE(doubleParam.Set(23.4)); + EXPECT_TRUE(doubleParam.IgnoresParentElementAttribute()); + + ASSERT_TRUE(doubleParam.SetFromString("34.5")); + EXPECT_FALSE(doubleParam.IgnoresParentElementAttribute()); + + ASSERT_TRUE(doubleParam.SetFromString("45.6", true)); + EXPECT_TRUE(doubleParam.IgnoresParentElementAttribute()); + + ASSERT_TRUE(doubleParam.SetFromString("56.7", false)); + EXPECT_FALSE(doubleParam.IgnoresParentElementAttribute()); + + ASSERT_TRUE(doubleParam.Set(67.8)); + EXPECT_TRUE(doubleParam.IgnoresParentElementAttribute()); + } +} + + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) diff --git a/src/parser.cc b/src/parser.cc index 90bfbb0d0..26993bb11 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1049,12 +1049,6 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, } } - if (_xml->GetText() != nullptr && _sdf->GetValue()) - { - if (!_sdf->GetValue()->SetFromString(_xml->GetText())) - return false; - } - // check for nested sdf std::string refSDFStr = _sdf->ReferenceSDF(); if (!refSDFStr.empty()) @@ -1186,6 +1180,12 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, } } + if (_xml->GetText() != nullptr && _sdf->GetValue()) + { + if (!_sdf->GetValue()->SetFromString(_xml->GetText())) + return false; + } + if (_sdf->GetCopyChildren()) { copyChildren(_sdf, _xml, false); diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index ce5f9289c..fd557fa4b 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -302,7 +302,7 @@ TEST(Parser, SyntaxErrorInValues) sdf::readFile(path, sdf); EXPECT_PRED2(contains, buffer.str(), - "Unable to set value [bad 0 0 0 0 0 ] for key[pose]"); + "Unable to set value [bad 0 0 0 0 0] for key [pose]"); EXPECT_PRED2(contains, buffer.str(), "bad_syntax_pose.sdf:L5"); EXPECT_PRED2(contains, buffer.str(), "/sdf/world[@name=\"default\"]/model[@name=\"robot1\"]/pose:"); diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index dcfffd58a..ddfb34936 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -39,6 +39,7 @@ set(tests plugin_attribute.cc plugin_bool.cc plugin_include.cc + pose_1_9_sdf.cc provide_feedback.cc root_dom.cc scene_dom.cc diff --git a/test/integration/include_custom_model_expected_output.sdf b/test/integration/include_custom_model_expected_output.sdf index df2dd0d2a..d50a2e11e 100644 --- a/test/integration/include_custom_model_expected_output.sdf +++ b/test/integration/include_custom_model_expected_output.sdf @@ -24,9 +24,9 @@ - 0.5 0.5 1 1 - 0.5 0.5 1 1 - 0 0 1 1 + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 @@ -72,12 +72,12 @@ 0.01 - 1 + true 1 --> - -1.06 0 0 0 -0 3.14 + -1.06 0 0 0 0 3.14 1.047 @@ -92,7 +92,7 @@ + true --> 30 chassis/camera @@ -100,7 +100,7 @@ - -1.06 0 0 0 -0 3.14 + -1.06 0 0 0 0 3.14 0.1 0.1 0.1 @@ -110,7 +110,7 @@ - 0.6 0 0.7 0 -0 0 + 0.6 0 0.7 0 0 0 @@ -170,7 +170,7 @@ --> - -0.2 0 0.3 0 -0 3.14 + -0.2 0 0.3 0 0 3.14 @@ -205,13 +205,13 @@ - 0.1 0.2 0.3 0 -0 0 + 0.1 0.2 0.3 0 0 0 - 1.047 + 1.0469999999999999 1280 @@ -232,7 +232,7 @@ 20 - 1 + true top/cam @@ -332,7 +332,7 @@ - 0 0 0 0 0 0 + 0 0 0 0 -0 0 @@ -408,7 +408,7 @@ - 1 1 1 0 -0 0 + 1 1 1 0 0 0 @@ -429,7 +429,7 @@ - 0 0 0 0 -0 0 + 0 0 0 0 0 0 1.047 @@ -444,7 +444,7 @@ 1 30 - 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 08eea7116..b8dfc81d7 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.41651899999999997 + 0.416519 0 0.481014 @@ -23,9 +23,9 @@ - 0.5 0.5 1 1 - 0.5 0.5 1 1 - 0 0 1 1 + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 @@ -73,32 +73,32 @@ 0.01 - 1 + true 1 --> - -1.06 0 0 0 -0 3.14 + -1.06 0 0 0 0 3.14 - 1.0469999999999999 + 1.047 320 240 - 0.10000000000000001 + 0.1 100 1 30 - 1 + true chassis/camera - -1.06 0 0 0 -0 3.14 + -1.06 0 0 0 0 3.14 0.1 0.1 0.1 @@ -108,7 +108,7 @@ - 0.6 0 0.7 0 -0 0 + 0.6 0 0.7 0 0 0 @@ -132,7 +132,7 @@ - -0.2 0 0.3 0 -0 3.14 + -0.2 0 0.3 0 0 3.14 0.05 @@ -154,17 +154,17 @@ - 0 0 0 0 -0 3.14 + 0 0 0 0 0 3.14 - 1.0469999999999999 + 1.047 320 240 - 0.10000000000000001 + 0.1 100 @@ -172,21 +172,21 @@ 60 - 1 + true top/camera - -0.2 0 0.3 0 -0 3.14 + -0.2 0 0.3 0 0 3.14 - 0.050000000000000003 + 0.05 - 0 0 0 0 -0 0 + 0 0 0 0 0 0 top @@ -199,14 +199,14 @@ - 0 0 0 0 -0 0 + 0 0 0 0 0 0 2 - 0.14583299999999999 + 0.145833 0 0 - 0.14583299999999999 + 0.145833 0 0.125 @@ -214,7 +214,7 @@ - 0.29999999999999999 + 0.3 @@ -226,7 +226,7 @@ - 0.29999999999999999 + 0.3 @@ -235,14 +235,14 @@ - 0 0 0 0 -0 0 + 0 0 0 0 0 0 2 - 0.14583299999999999 + 0.145833 0 0 - 0.14583299999999999 + 0.145833 0 0.125 @@ -250,7 +250,7 @@ - 0.29999999999999999 + 0.3 @@ -262,7 +262,7 @@ - 0.29999999999999999 + 0.3 @@ -271,14 +271,14 @@ - 0 0 0 0 -0 0 + 0 0 0 0 0 0 2 - 0.14583299999999999 + 0.145833 0 0 - 0.14583299999999999 + 0.145833 0 0.125 @@ -286,7 +286,7 @@ - 0.29999999999999999 + 0.3 @@ -298,7 +298,7 @@ - 0.29999999999999999 + 0.3 @@ -311,8 +311,8 @@ 0 0 1 - -1.7976900000000001e+308 - 1.7976900000000001e+308 + -1.79769e+308 + 1.79769e+308 @@ -322,8 +322,8 @@ 0 0 1 - -1.7976900000000001e+308 - 1.7976900000000001e+308 + -1.79769e+308 + 1.79769e+308 diff --git a/test/integration/nested_model_with_frames_expected.sdf b/test/integration/nested_model_with_frames_expected.sdf index 8358e2519..3b4e7a8d3 100644 --- a/test/integration/nested_model_with_frames_expected.sdf +++ b/test/integration/nested_model_with_frames_expected.sdf @@ -4,7 +4,7 @@ - 0 0 0 1.5708 -0 0 + 0 0 0 1.570796 0 0 0 0 0 0 0.785398 0 @@ -29,13 +29,13 @@ - 1 0 0 0 -0 0 + 1 0 0 0 0 0 - 0 1 0 0 -0 0 + 0 1 0 0 0 0 - 0 0 1 0 -0 0 + 0 0 1 0 0 0 0 0 0 0 -0 0 @@ -49,7 +49,7 @@ - 0 0 1 0 -0 0 + 0 0 1 0 0 0 L2 L3 @@ -57,14 +57,14 @@ - 1 0 1 0 -0 0 + 1 0 1 0 0 0 L3 L4 - 1 0 0 0 -0 0 + 1 0 0 0 0 0 - 1 0 0 0 -0 0 + 1 0 0 0 0 0 10 0 0 0 -0 1.5708 diff --git a/test/integration/pose_1_9_sdf.cc b/test/integration/pose_1_9_sdf.cc new file mode 100644 index 000000000..73bf14e25 --- /dev/null +++ b/test/integration/pose_1_9_sdf.cc @@ -0,0 +1,498 @@ +/* + * Copyright 2021 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. + * + */ + +#include +#include +#include +#include + +#include +#include +#include +#include "sdf/Element.hh" +#include "sdf/Error.hh" +#include "sdf/Model.hh" +#include "sdf/Root.hh" +#include "sdf/World.hh" +#include "sdf/parser.hh" +#include "sdf/Filesystem.hh" +#include "test_config.h" +#include "test_utils.hh" + +////////////////////////////////////////////////// +TEST(Pose1_9, ModelPose) +{ + using Pose = ignition::math::Pose3d; + + const std::string testFile = sdf::testing::TestFile( + "sdf", "pose_1_9.sdf"); + const double pi = 3.14159265358979323846; + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + ASSERT_TRUE(errors.empty()) << errors; + EXPECT_EQ(SDF_PROTOCOL_VERSION, root.Version()); + + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + const sdf::Model *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_empty_pose", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + model = world->ModelByIndex(1); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_empty_pose_with_degrees_false", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + model = world->ModelByIndex(2); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_empty_pose_with_degrees_true", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + model = world->ModelByIndex(3); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_pose_no_attribute", model->Name()); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), model->RawPose()); + + model = world->ModelByIndex(4); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_pose_with_degrees_false", model->Name()); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), model->RawPose()); + + model = world->ModelByIndex(5); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_pose_with_degrees_true", model->Name()); + EXPECT_EQ(Pose(1, 2, 3, pi / 2, pi, pi * 3 / 2), model->RawPose()); + + model = world->ModelByIndex(6); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_single_space_delimiter", model->Name()); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), model->RawPose()); + + model = world->ModelByIndex(7); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_newline_delimiter", model->Name()); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), model->RawPose()); + + model = world->ModelByIndex(8); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_messy_delimiters", model->Name()); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), model->RawPose()); +} + +////////////////////////////////////////////////// +static bool contains(const std::string &_a, const std::string &_b) +{ + return _a.find(_b) != std::string::npos; +} + +////////////////////////////////////////////////// +TEST(Pose1_9, PoseSet7ValuesFail) +{ + // Redirect sdferr output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + buffer.str(""); + using Pose = ignition::math::Pose3d; + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + sdf::ParamPtr poseValueParam = poseElem->GetValue(); + ASSERT_NE(nullptr, poseValueParam); + EXPECT_FALSE(poseValueParam->SetFromString( + "1 2 3 0.7071068 0.7071068 0 0")); + EXPECT_PRED2(contains, buffer.str(), "can only accept 6 values"); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, PoseElementSetAndGet) +{ + using Pose = ignition::math::Pose3d; + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + poseElem->Set(Pose(1, 2, 3, 0.4, 0.5, 0.6)); + + Pose elemVal; + ASSERT_TRUE(poseElem->Get("", elemVal, Pose())); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), elemVal); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, PoseElementSetAndParamGet) +{ + using Pose = ignition::math::Pose3d; + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + poseElem->Set(Pose(1, 2, 3, 0.4, 0.5, 0.6)); + + sdf::ParamPtr poseValueParam = poseElem->GetValue(); + ASSERT_NE(nullptr, poseValueParam); + + Pose paramVal; + ASSERT_TRUE(poseValueParam->Get(paramVal)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), paramVal); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, PoseParamSetAndGet) +{ + using Pose = ignition::math::Pose3d; + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + sdf::ParamPtr poseValueParam = poseElem->GetValue(); + ASSERT_NE(nullptr, poseValueParam); + + ASSERT_TRUE(poseValueParam->Set(Pose(1, 2, 3, 0.4, 0.5, 0.6))); + + Pose paramVal; + ASSERT_TRUE(poseValueParam->Get(paramVal)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), paramVal); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, PoseParamSetFromStringAndGet) +{ + using Pose = ignition::math::Pose3d; + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + sdf::ParamPtr poseValueParam = poseElem->GetValue(); + ASSERT_NE(nullptr, poseValueParam); + + ASSERT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); + + Pose paramVal; + ASSERT_TRUE(poseValueParam->Get(paramVal)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), paramVal); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, PoseParamSetAndElemGet) +{ + using Pose = ignition::math::Pose3d; + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + sdf::ParamPtr poseValueParam = poseElem->GetValue(); + ASSERT_NE(nullptr, poseValueParam); + + ASSERT_TRUE(poseValueParam->Set(Pose(1, 2, 3, 0.4, 0.5, 0.6))); + + Pose elemVal; + ASSERT_TRUE(poseElem->Get("", elemVal, Pose())); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), elemVal); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, PoseParamSetAndParentElemGet) +{ + using Pose = ignition::math::Pose3d; + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + sdf::ParamPtr poseValueParam = poseElem->GetValue(); + ASSERT_NE(nullptr, poseValueParam); + + ASSERT_TRUE(poseValueParam->Set(Pose(1, 2, 3, 0.4, 0.5, 0.6))); + + sdf::ElementPtr parentElem = poseValueParam->GetParentElement(); + ASSERT_NE(nullptr, parentElem); + EXPECT_EQ(poseElem, parentElem); + Pose parentElemVal; + ASSERT_TRUE(parentElem->Get("", parentElemVal, Pose())); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), parentElemVal); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, ChangingParentPoseElementAfterSet) +{ + // Since the values are explicitly set using the Set function, + // reparsing should not change their values, even when parent elements have + // been changed. + + using Pose = ignition::math::Pose3d; + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + ASSERT_TRUE(poseElem->Set(Pose(1, 2, 3, 0.4, 0.5, 0.6))); + + sdf::ElementPtr degreesPoseElem(new sdf::Element); + degreesPoseElem->SetName("pose"); + degreesPoseElem->AddValue("pose", "0 0 0 0 0 0", true); + degreesPoseElem->AddAttribute("relative_to", "string", "", false); + degreesPoseElem->AddAttribute("degrees", "bool", "true", false); + + sdf::ElementPtr radiansPoseElem(new sdf::Element); + radiansPoseElem->SetName("pose"); + radiansPoseElem->AddValue("pose", "0 0 0 0 0 0", true); + radiansPoseElem->AddAttribute("relative_to", "string", "", false); + radiansPoseElem->AddAttribute("degrees", "bool", "false", false); + + // Param from original default attibute + sdf::ParamPtr valParam = poseElem->GetValue(); + ASSERT_NE(nullptr, valParam); + + Pose val; + ASSERT_TRUE(valParam->Get(val)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), val); + + // Set parent to Element with degrees attribute true. + valParam->SetParentElement(degreesPoseElem); + ASSERT_TRUE(valParam->Get(val)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), val); + + // Set parent to Element with degrees attribute false. + valParam->SetParentElement(radiansPoseElem); + ASSERT_TRUE(valParam->Get(val)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), val); + + // Remove parent + valParam->SetParentElement(nullptr); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), val); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, ChangingParentPoseElementAfterParamSetFromString) +{ + // Since the values are set using the SetFromString function, reparsing + // should change their values, when parent elements have been changed. + + const double pi = 3.14159265358979323846; + using Pose = ignition::math::Pose3d; + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + sdf::ElementPtr degreesPoseElem(new sdf::Element); + degreesPoseElem->SetName("pose"); + degreesPoseElem->AddValue("pose", "0 0 0 0 0 0", true); + degreesPoseElem->AddAttribute("relative_to", "string", "", false); + degreesPoseElem->AddAttribute("degrees", "bool", "true", false); + + sdf::ElementPtr radiansPoseElem(new sdf::Element); + radiansPoseElem->SetName("pose"); + radiansPoseElem->AddValue("pose", "0 0 0 0 0 0", true); + radiansPoseElem->AddAttribute("relative_to", "string", "", false); + radiansPoseElem->AddAttribute("degrees", "bool", "false", false); + + // Param from original default attibute + sdf::ParamPtr valParam = poseElem->GetValue(); + ASSERT_NE(nullptr, valParam); + ASSERT_TRUE(valParam->SetFromString("1, 2, 3, 0.4, 0.5, 0.6")); + + Pose val; + ASSERT_TRUE(valParam->Get(val)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), val); + + // Set parent to Element with degrees attribute true. + valParam->SetParentElement(degreesPoseElem); + ASSERT_TRUE(valParam->Get(val)); + EXPECT_EQ(Pose(1, 2, 3, 0.4 * pi / 180, 0.5 * pi / 180, 0.6 * pi / 180), val); + + // Set parent to Element with degrees attribute false. + valParam->SetParentElement(radiansPoseElem); + ASSERT_TRUE(valParam->Get(val)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), val); + + // Remove parent + valParam->SetParentElement(nullptr); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), val); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, ChangingAttributeOfParentElement) +{ + const double pi = 3.14159265358979323846; + using Pose = ignition::math::Pose3d; + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + // Param value in radians + sdf::ParamPtr valParam = poseElem->GetValue(); + ASSERT_NE(nullptr, valParam); + ASSERT_TRUE(valParam->SetFromString("1, 2, 3, 0.4, 0.5, 0.6")); + + Pose val; + ASSERT_TRUE(valParam->Get(val)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), val); + + // Changing to degrees without reparsing, value will remain the same + sdf::ParamPtr degreesAttrib = poseElem->GetAttribute("degrees"); + ASSERT_NE(nullptr, degreesAttrib); + ASSERT_TRUE(degreesAttrib->Set(true)); + ASSERT_TRUE(valParam->Get(val)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), val); + + // Values will change to be degrees after reparsing + ASSERT_TRUE(valParam->Reparse()); + ASSERT_TRUE(valParam->Get(val)); + EXPECT_EQ(Pose(1, 2, 3, 0.4 * pi / 180, 0.5 * pi / 180, 0.6 * pi / 180), val); + + // Changing back to radians + ASSERT_TRUE(degreesAttrib->Set(false)); + ASSERT_TRUE(valParam->Reparse()); + ASSERT_TRUE(valParam->Get(val)); + EXPECT_EQ(Pose(1, 2, 3, 0.4, 0.5, 0.6), val); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, ToStringWithoutAttrib) +{ + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + sdf::ParamPtr poseValueParam = poseElem->GetValue(); + ASSERT_NE(nullptr, poseValueParam); + EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); + + std::string elemStr = poseElem->ToString(""); + EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, ToStringWithDegreesFalse) +{ + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + sdf::ParamPtr degreesAttrib = poseElem->GetAttribute("degrees"); + ASSERT_NE(nullptr, degreesAttrib); + ASSERT_TRUE(degreesAttrib->Set(false)); + + sdf::ParamPtr poseValueParam = poseElem->GetValue(); + ASSERT_NE(nullptr, poseValueParam); + 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, "0.4 0.5 0.6"); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, ToStringWithDegreesTrue) +{ + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + sdf::ParamPtr degreesAttrib = poseElem->GetAttribute("degrees"); + ASSERT_NE(nullptr, degreesAttrib); + ASSERT_TRUE(degreesAttrib->Set(true)); + + sdf::ParamPtr poseValueParam = poseElem->GetValue(); + ASSERT_NE(nullptr, poseValueParam); + 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, "0.4 0.5 0.6"); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, ToStringAfterChangingDegreeAttribute) +{ + using Pose = ignition::math::Pose3d; + + sdf::ElementPtr poseElem(new sdf::Element); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "0 0 0 0 0 0", true); + poseElem->AddAttribute("relative_to", "string", "", false); + poseElem->AddAttribute("degrees", "bool", "false", false); + + // Param value in radians + sdf::ParamPtr valParam = poseElem->GetValue(); + ASSERT_NE(nullptr, valParam); + ASSERT_TRUE(valParam->SetFromString("1 2 3 0.4 0.5 0.6")); + + std::string elemStr = poseElem->ToString(""); + EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); + + // Changing to degrees + sdf::ParamPtr degreesAttrib = poseElem->GetAttribute("degrees"); + ASSERT_NE(nullptr, degreesAttrib); + ASSERT_TRUE(degreesAttrib->Set(true)); + elemStr = poseElem->ToString(""); + EXPECT_PRED2(contains, elemStr, "degrees='1'"); + 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, "0.4 0.5 0.6"); +} 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 a73e206d2..d907410ac 100644 --- a/test/integration/two_level_nested_model_with_frames_expected.sdf +++ b/test/integration/two_level_nested_model_with_frames_expected.sdf @@ -5,7 +5,7 @@ - 0 0 0 1.5708 -0 0 + 0 0 0 1.570796 0 0 0 0 0 0 0.785398 0 @@ -30,13 +30,13 @@ - 1 0 0 0 -0 0 + 1 0 0 0 0 0 - 0 1 0 0 -0 0 + 0 1 0 0 0 0 - 0 0 1 0 -0 0 + 0 0 1 0 0 0 0 0 0 0 -0 0 @@ -50,7 +50,7 @@ - 0 0 1 0 -0 0 + 0 0 1 0 0 0 L2 L3 @@ -58,17 +58,17 @@ - 1 0 1 0 -0 0 + 1 0 1 0 0 0 L3 L4 - 1 0 0 0 -0 0 + 1 0 0 0 0 0 - 1 0 0 0 -0 0 + 1 0 0 0 0 0 - 0 10 0 1.5708 -0 0 + 0 10 0 1.570796326794895 0 0 10 0 0 0 -0 1.5708 diff --git a/test/sdf/pose_1_9.sdf b/test/sdf/pose_1_9.sdf new file mode 100644 index 000000000..dd8187324 --- /dev/null +++ b/test/sdf/pose_1_9.sdf @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + 1 2 3 0.4 0.5 0.6 + + + + + 1 2 3 0.4 0.5 0.6 + + + + + 1 2 3 90 180 270 + + + + + 1 2 3 0.4 0.5 0.6 + + + + + + 1 2 3 + 0.4 0.5 0.6 + + + + + + + 1 2 3 + 0.4 + 0.5 0.6 + + + + + +