diff --git a/CMakeLists.txt b/CMakeLists.txt index 368d9252a..1952368af 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -155,7 +155,6 @@ option(BUILD_SHARED_LIBS "Set this to true to generate shared libraries (recomme ##################################### # Handle CFlags unset (CMAKE_C_FLAGS_ALL CACHE) -unset (CMAKE_CXX_FLAGS CACHE) # USE_HOST_CFLAGS (default TRUE) # Will check building host machine for proper cflags diff --git a/Migration.md b/Migration.md index 5ddbc0ad7..600c371df 100644 --- a/Migration.md +++ b/Migration.md @@ -143,6 +143,13 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. + std::optional GetMaxValueAsString() const; + bool ValidateValue() const; +## libsdformat 9.4 to 9.5 + +### Additions + +1. **sdf/Element.hh** + + sdf::ElementPtr FindElement() const + ## libsdformat 9.3 to 9.4 ### Modifications diff --git a/cmake/DefaultCFlags.cmake b/cmake/DefaultCFlags.cmake index 610116d9f..be57d0a3d 100644 --- a/cmake/DefaultCFlags.cmake +++ b/cmake/DefaultCFlags.cmake @@ -40,12 +40,12 @@ endif() ##################################### # Set all the global build flags -set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") -set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") +set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") +set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") set (CMAKE_CXX_EXTENSIONS off) -set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") -set (CMAKE_SHARED_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") -set (CMAKE_MODULE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") +set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") +set (CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} ${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") +set (CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} ${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -67,7 +67,9 @@ if ("${CMAKE_CXX_COMPILER_ID} " MATCHES "GNU ") message(FATAL_ERROR "${PROJECT_NAME} requires g++ 7.0 or greater.") endif () elseif ("${CMAKE_CXX_COMPILER_ID} " MATCHES "Clang ") + if(APPLE) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libc++") + endif() elseif ("${CMAKE_CXX_COMPILER_ID} " STREQUAL "MSVC ") if (MSVC_VERSION LESS 1914) message(FATAL_ERROR "${PROJECT_NAME} requires VS 2017 or greater.") diff --git a/include/sdf/Element.hh b/include/sdf/Element.hh index 135aef8a9..953153e10 100644 --- a/include/sdf/Element.hh +++ b/include/sdf/Element.hh @@ -52,6 +52,10 @@ namespace sdf /// \brief Shared pointer to an SDF Element typedef std::shared_ptr ElementPtr; + /// \def ElementConstPtr + /// \brief Shared pointer to a const SDF Element + typedef std::shared_ptr ElementConstPtr; + /// \def ElementWeakPtr /// \brief Weak pointer to an SDF Element typedef std::weak_ptr ElementWeakPtr; @@ -375,7 +379,8 @@ namespace sdf /// \brief Return a pointer to the child element with the provided name. /// /// A new child element, with the provided name, is added to this element - /// if there is no existing child element. + /// if there is no existing child element. If this is not desired see \ref + /// FindElement /// \remarks If there are multiple elements with the given tag, it returns /// the first one. /// \param[in] _name Name of the child element to retreive. @@ -383,6 +388,28 @@ namespace sdf /// element if an existing child element did not exist. public: ElementPtr GetElement(const std::string &_name); + /// \brief Return a pointer to the child element with the provided name. + /// + /// Unlike \ref GetElement, this does not create a new child element if it + /// fails to find an existing element. + /// \remarks If there are multiple elements with the given tag, it returns + /// the first one. + /// \param[in] _name Name of the child element to retreive. + /// \return Pointer to the existing child element, or nullptr + /// if the child element was not found. + public: ElementPtr FindElement(const std::string &_name); + + /// \brief Return a pointer to the child element with the provided name. + /// + /// Unlike \ref GetElement, this does not create a new child element if it + /// fails to find an existing element. + /// \remarks If there are multiple elements with the given tag, it returns + /// the first one. + /// \param[in] _name Name of the child element to retreive. + /// \return Pointer to the existing child element, or nullptr + /// if the child element was not found. + public: ElementConstPtr FindElement(const std::string &_name) const; + /// \brief Add a named element. /// \param[in] _name the name of the element to add. /// \return A pointer to the newly created Element object. diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh index f523695c2..c59fb683d 100644 --- a/include/sdf/Param.hh +++ b/include/sdf/Param.hh @@ -275,6 +275,7 @@ namespace sdf /// \brief Private method to set the Element from a passed-in string. /// \param[in] _value Value to set the parameter to. + /// \return True if the parameter was successfully set, false otherwise. private: bool ValueFromString(const std::string &_value); /// \brief Private data @@ -326,8 +327,63 @@ namespace sdf /// \brief This parameter's maximum allowed value public: std::optional maxValue; + + /// \brief Method used to set the Param from a passed-in string + /// \param[in] _typeName The data type of the value to set + /// \param[in] _valueStr The value as a string + /// \param[out] _valueToSet The value to set + /// \return True if the value was successfully set, false otherwise + public: bool SDFORMAT_VISIBLE ValueFromStringImpl( + const std::string &_typeName, + const std::string &_valueStr, + ParamVariant &_valueToSet) const; + + /// \brief Data type to string mapping + /// \return The type as a string, empty string if unknown type + public: template + std::string TypeToString() const; }; + /////////////////////////////////////////////// + template + std::string ParamPrivate::TypeToString() const + { + if constexpr (std::is_same_v) + return "bool"; + else if constexpr (std::is_same_v) + return "char"; + else if constexpr (std::is_same_v) + return "string"; + else if constexpr (std::is_same_v) + return "int"; + else if constexpr (std::is_same_v) + return "uint64_t"; + else if constexpr (std::is_same_v) + return "unsigned int"; + else if constexpr (std::is_same_v) + return "double"; + else if constexpr (std::is_same_v) + return "float"; + else if constexpr (std::is_same_v) + return "time"; + else if constexpr (std::is_same_v) + return "angle"; + else if constexpr (std::is_same_v) + return "color"; + else if constexpr (std::is_same_v) + return "vector2i"; + else if constexpr (std::is_same_v) + return "vector2d"; + else if constexpr (std::is_same_v) + return "vector3"; + else if constexpr (std::is_same_v) + return "quaternion"; + else if constexpr (std::is_same_v) + return "pose"; + else + return ""; + } + /////////////////////////////////////////////// template void Param::SetUpdateFunc(T _updateFunc) @@ -359,50 +415,48 @@ namespace sdf template bool Param::Get(T &_value) const { - try + T *value = std::get_if(&this->dataPtr->value); + if (value) { - if (typeid(T) == typeid(bool) && this->dataPtr->typeName == "string") + _value = *value; + } + else + { + std::string typeStr = this->dataPtr->TypeToString(); + if (typeStr.empty()) + { + sdferr << "Unknown parameter type[" << typeid(T).name() << "]\n"; + return false; + } + + std::string valueStr = this->GetAsString(); + ParamPrivate::ParamVariant pv; + bool success = this->dataPtr->ValueFromStringImpl(typeStr, valueStr, pv); + + if (success) + { + _value = std::get(pv); + } + else if (typeStr == "bool" && this->dataPtr->typeName == "string") { - std::string strValue = std::get(this->dataPtr->value); - std::transform(strValue.begin(), strValue.end(), strValue.begin(), - [](unsigned char c) - { - return static_cast(std::tolower(c)); - }); + // this section for handling bool types is to keep backward behavior + // TODO(anyone) remove for Fortress. For more details: + // https://github.com/ignitionrobotics/sdformat/pull/638 + valueStr = lowercase(valueStr); std::stringstream tmp; - if (strValue == "true" || strValue == "1") - { + if (valueStr == "true" || valueStr == "1") tmp << "1"; - } else - { tmp << "0"; - } + tmp >> _value; + return true; } - else - { - T *value = std::get_if(&this->dataPtr->value); - if (value) - _value = *value; - else - { - std::stringstream ss; - ss << ParamStreamer{this->dataPtr->value}; - ss >> _value; - } - } - } - catch(...) - { - sdferr << "Unable to convert parameter[" - << this->dataPtr->key << "] " - << "whose type is[" - << this->dataPtr->typeName << "], to " - << "type[" << typeid(T).name() << "]\n"; - return false; + + return success; } + return true; } diff --git a/sdf/1.6/material.sdf b/sdf/1.6/material.sdf index 7cd011598..6c2696a1a 100644 --- a/sdf/1.6/material.sdf +++ b/sdf/1.6/material.sdf @@ -111,6 +111,10 @@ Material glossiness in the range of [0-1], where 0 represents a rough surface and 1 represents a smooth surface. This is the inverse of a roughness map in a PBR metal workflow. + + Filename of the environment / reflection map, typically in the form of a cubemap + + Filename of the ambient occlusion map. The map defines the amount of ambient lighting on the surface. diff --git a/sdf/1.7/material.sdf b/sdf/1.7/material.sdf index 285b476ee..44e69e19a 100644 --- a/sdf/1.7/material.sdf +++ b/sdf/1.7/material.sdf @@ -128,6 +128,10 @@ Material glossiness in the range of [0-1], where 0 represents a rough surface and 1 represents a smooth surface. This is the inverse of a roughness map in a PBR metal workflow. + + Filename of the environment / reflection map, typically in the form of a cubemap + + Filename of the ambient occlusion map. The map defines the amount of ambient lighting on the surface. diff --git a/sdf/1.8/material.sdf b/sdf/1.8/material.sdf index 285b476ee..44e69e19a 100644 --- a/sdf/1.8/material.sdf +++ b/sdf/1.8/material.sdf @@ -128,6 +128,10 @@ Material glossiness in the range of [0-1], where 0 represents a rough surface and 1 represents a smooth surface. This is the inverse of a roughness map in a PBR metal workflow. + + Filename of the environment / reflection map, typically in the form of a cubemap + + Filename of the ambient occlusion map. The map defines the amount of ambient lighting on the surface. diff --git a/src/Element.cc b/src/Element.cc index f4d9cddcb..6d780cecf 100644 --- a/src/Element.cc +++ b/src/Element.cc @@ -872,6 +872,18 @@ ElementPtr Element::GetElement(const std::string &_name) return result; } +///////////////////////////////////////////////// +ElementPtr Element::FindElement(const std::string &_name) +{ + return this->GetElementImpl(_name); +} + +///////////////////////////////////////////////// +ElementConstPtr Element::FindElement(const std::string &_name) const +{ + return this->GetElementImpl(_name); +} + ///////////////////////////////////////////////// void Element::InsertElement(ElementPtr _elem) { diff --git a/src/Element_TEST.cc b/src/Element_TEST.cc index 4d4fd1f68..a35520d8f 100644 --- a/src/Element_TEST.cc +++ b/src/Element_TEST.cc @@ -880,6 +880,25 @@ TEST(Element, GetNextElementMultiple) ASSERT_EQ(child2->GetNextElement(""), nullptr); } +///////////////////////////////////////////////// +/// Helper function to add child elements without having to create descriptions +sdf::ElementPtr addChildElement(sdf::ElementPtr _parent, + const std::string &_elementName, + const bool _addNameAttribute, + const std::string &_childName) +{ + sdf::ElementPtr child = std::make_shared(); + child->SetParent(_parent); + child->SetName(_elementName); + _parent->InsertElement(child); + + if (_addNameAttribute) + { + child->AddAttribute("name", "string", _childName, false, "description"); + } + return child; +} + ///////////////////////////////////////////////// TEST(Element, CountNamedElements) { @@ -893,22 +912,6 @@ TEST(Element, CountNamedElements) EXPECT_TRUE(parent->HasUniqueChildNames()); EXPECT_TRUE(parent->HasUniqueChildNames("child")); - auto addChildElement = []( - sdf::ElementPtr _parent, - const std::string &_elementName, - const bool _addNameAttribute, - const std::string &_childName) - { - sdf::ElementPtr child = std::make_shared(); - child->SetParent(_parent); - child->SetName(_elementName); - _parent->InsertElement(child); - - if (_addNameAttribute) - { - child->AddAttribute("name", "string", _childName, false, "description"); - } - }; // The following calls should make the following child elements: // @@ -969,6 +972,81 @@ TEST(Element, CountNamedElements) EXPECT_EQ(allMap.at("child3"), 1u); } +TEST(Element, FindElement) +{ + // + // + // + // + // + // + // + // + // + // + sdf::ElementPtr root = std::make_shared(); + root->SetName("root"); + + // Create elements + { + auto elemA = addChildElement(root, "elem_A", false, ""); + addChildElement(elemA, "child_elem_A", false, ""); + + auto elemB = addChildElement(root, "elem_B", false, ""); + auto firstChildElemB = + addChildElement(elemB, "child_elem_B", true, "first_child"); + + addChildElement(elemB, "child_elem_B", false, ""); + } + + { + auto elemA = root->FindElement("elem_A"); + ASSERT_NE(nullptr, elemA); + EXPECT_NE(nullptr, elemA->FindElement("child_elem_A")); + EXPECT_EQ(nullptr, elemA->FindElement("non_existent_elem")); + + auto elemB = root->FindElement("elem_B"); + ASSERT_NE(nullptr, elemB); + // This should find the first "child_elem_B" element, which has the name + // attribute + auto childElemB = elemB->FindElement("child_elem_B"); + ASSERT_TRUE(childElemB->HasAttribute("name")); + EXPECT_EQ("first_child", childElemB->GetAttribute("name")->GetAsString()); + } + + // Check that it works with const pointers + { + sdf::ElementConstPtr rootConst = root; + sdf::ElementConstPtr elemA = rootConst->FindElement("elem_A"); + ASSERT_NE(nullptr, elemA); + EXPECT_NE(nullptr, elemA->FindElement("child_elem_A")); + EXPECT_EQ(nullptr, elemA->FindElement("non_existent_elem")); + + sdf::ElementConstPtr elemB = root->FindElement("elem_B"); + ASSERT_NE(nullptr, elemB); + // This should find the first "child_elem_B" element, which has the name + // attribute + sdf::ElementConstPtr childElemB = elemB->FindElement("child_elem_B"); + ASSERT_TRUE(childElemB->HasAttribute("name")); + EXPECT_EQ("first_child", childElemB->GetAttribute("name")->GetAsString()); + } + { + sdf::ElementConstPtr rootConst = root; + sdf::ElementConstPtr elemA = rootConst->FindElement("elem_A"); + ASSERT_NE(nullptr, elemA); + EXPECT_NE(nullptr, elemA->FindElement("child_elem_A")); + EXPECT_EQ(nullptr, elemA->FindElement("non_existent_elem")); + + sdf::ElementConstPtr elemB = root->FindElement("elem_B"); + ASSERT_NE(nullptr, elemB); + // This should find the first "child_elem_B" element, which has the name + // attribute + sdf::ElementConstPtr childElemB = elemB->FindElement("child_elem_B"); + ASSERT_TRUE(childElemB->HasAttribute("name")); + EXPECT_EQ("first_child", childElemB->GetAttribute("name")->GetAsString()); + } +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) diff --git a/src/Param.cc b/src/Param.cc index 4335fa890..5c3811e51 100644 --- a/src/Param.cc +++ b/src/Param.cc @@ -429,12 +429,22 @@ bool ParseColorUsingStringStream(const std::string &_input, ////////////////////////////////////////////////// bool Param::ValueFromString(const std::string &_value) +{ + return this->dataPtr->ValueFromStringImpl(this->dataPtr->typeName, + _value, + this->dataPtr->value); +} + +////////////////////////////////////////////////// +bool ParamPrivate::ValueFromStringImpl(const std::string &_typeName, + const std::string &_valueStr, + ParamVariant &_valueToSet) const { // Under some circumstances, latin locales (es_ES or pt_BR) will return a // comma for decimal position instead of a dot, making the conversion // to fail. See bug #60 for more information. Force to use always C setlocale(LC_NUMERIC, "C"); - std::string trimmed = sdf::trim(_value); + std::string trimmed = sdf::trim(_valueStr); std::string tmp(trimmed); std::string lowerTmp = lowercase(trimmed); @@ -460,15 +470,15 @@ bool Param::ValueFromString(const std::string &_value) numericBase = 16; } - if (this->dataPtr->typeName == "bool") + if (_typeName == "bool") { if (lowerTmp == "true" || lowerTmp == "1") { - this->dataPtr->value = true; + _valueToSet = true; } else if (lowerTmp == "false" || lowerTmp == "0") { - this->dataPtr->value = false; + _valueToSet = false; } else { @@ -476,86 +486,91 @@ bool Param::ValueFromString(const std::string &_value) return false; } } - else if (this->dataPtr->typeName == "char") + else if (_typeName == "char") { - this->dataPtr->value = tmp[0]; + _valueToSet = tmp[0]; } - else if (this->dataPtr->typeName == "std::string" || - this->dataPtr->typeName == "string") + else if (_typeName == "std::string" || + _typeName == "string") { - this->dataPtr->value = tmp; + _valueToSet = tmp; } - else if (this->dataPtr->typeName == "int") + else if (_typeName == "int") { - this->dataPtr->value = std::stoi(tmp, nullptr, numericBase); + _valueToSet = std::stoi(tmp, nullptr, numericBase); } - else if (this->dataPtr->typeName == "uint64_t") + else if (_typeName == "uint64_t") { - return ParseUsingStringStream(tmp, this->dataPtr->key, - this->dataPtr->value); + return ParseUsingStringStream(tmp, this->key, + _valueToSet); } - else if (this->dataPtr->typeName == "unsigned int") + else if (_typeName == "unsigned int") { - this->dataPtr->value = static_cast( + _valueToSet = static_cast( std::stoul(tmp, nullptr, numericBase)); } - else if (this->dataPtr->typeName == "double") + else if (_typeName == "double") + { + _valueToSet = std::stod(tmp); + } + else if (_typeName == "float") { - this->dataPtr->value = std::stod(tmp); + _valueToSet = std::stof(tmp); } - else if (this->dataPtr->typeName == "float") + else if (_typeName == "sdf::Time" || + _typeName == "time") { - this->dataPtr->value = std::stof(tmp); + return ParseUsingStringStream(tmp, this->key, + _valueToSet); } - else if (this->dataPtr->typeName == "sdf::Time" || - this->dataPtr->typeName == "time") + else if (_typeName == "ignition::math::Angle" || + _typeName == "angle") { - return ParseUsingStringStream(tmp, this->dataPtr->key, - this->dataPtr->value); + return ParseUsingStringStream( + tmp, this->key, _valueToSet); } - else if (this->dataPtr->typeName == "ignition::math::Color" || - this->dataPtr->typeName == "color") + else if (_typeName == "ignition::math::Color" || + _typeName == "color") { - return ParseColorUsingStringStream( - tmp, this->dataPtr->key, this->dataPtr->value); + return ParseColorUsingStringStream(tmp, this->key, _valueToSet); } - else if (this->dataPtr->typeName == "ignition::math::Vector2i" || - this->dataPtr->typeName == "vector2i") + else if (_typeName == "ignition::math::Vector2i" || + _typeName == "vector2i") { return ParseUsingStringStream( - tmp, this->dataPtr->key, this->dataPtr->value); + tmp, this->key, _valueToSet); } - else if (this->dataPtr->typeName == "ignition::math::Vector2d" || - this->dataPtr->typeName == "vector2d") + else if (_typeName == "ignition::math::Vector2d" || + _typeName == "vector2d") { return ParseUsingStringStream( - tmp, this->dataPtr->key, this->dataPtr->value); + tmp, this->key, _valueToSet); } - else if (this->dataPtr->typeName == "ignition::math::Vector3d" || - this->dataPtr->typeName == "vector3") + else if (_typeName == "ignition::math::Vector3d" || + _typeName == "vector3") { return ParseUsingStringStream( - tmp, this->dataPtr->key, this->dataPtr->value); + tmp, this->key, _valueToSet); } - else if (this->dataPtr->typeName == "ignition::math::Pose3d" || - this->dataPtr->typeName == "pose" || - this->dataPtr->typeName == "Pose") + else if (_typeName == "ignition::math::Pose3d" || + _typeName == "pose" || + _typeName == "Pose") { if (!tmp.empty()) { return ParseUsingStringStream( - tmp, this->dataPtr->key, this->dataPtr->value); + tmp, this->key, _valueToSet); } } - else if (this->dataPtr->typeName == "ignition::math::Quaterniond" || - this->dataPtr->typeName == "quaternion") + else if (_typeName == "ignition::math::Quaterniond" || + _typeName == "quaternion") { return ParseUsingStringStream( - tmp, this->dataPtr->key, this->dataPtr->value); + tmp, this->key, _valueToSet); } else { - sdferr << "Unknown parameter type[" << this->dataPtr->typeName << "]\n"; + sdferr << "Unknown parameter type[" << _typeName << "]\n"; return false; } } @@ -563,16 +578,16 @@ bool Param::ValueFromString(const std::string &_value) catch(std::invalid_argument &) { sdferr << "Invalid argument. Unable to set value [" - << _value << " ] for key[" - << this->dataPtr->key << "].\n"; + << _valueStr << " ] for key[" + << this->key << "].\n"; return false; } // Catch out of range exception from std::stoi/stoul/stod/stof catch(std::out_of_range &) { sdferr << "Out of range. Unable to set value [" - << _value << " ] for key[" - << this->dataPtr->key << "].\n"; + << _valueStr << " ] for key[" + << this->key << "].\n"; return false; } diff --git a/src/Param_TEST.cc b/src/Param_TEST.cc index 37899d889..ece457299 100644 --- a/src/Param_TEST.cc +++ b/src/Param_TEST.cc @@ -22,6 +22,8 @@ #include #include +#include +#include #include "sdf/Exception.hh" #include "sdf/Param.hh" @@ -101,6 +103,27 @@ TEST(SetFromString, Decimals) ASSERT_TRUE(check_double("0.2345")); } +//////////////////////////////////////////////////// +/// Test setting param as a string but getting it as different type +TEST(Param, StringTypeGet) +{ + sdf::Param stringParam("key", "string", "", false, "description"); + + // pose type + ignition::math::Pose3d pose; + EXPECT_TRUE(stringParam.SetFromString("1 1 1 0 0 0")); + EXPECT_TRUE(stringParam.Get(pose)); + EXPECT_EQ(ignition::math::Pose3d(1, 1, 1, 0, 0, 0), pose); + EXPECT_EQ("string", stringParam.GetTypeName()); + + // color type + ignition::math::Color color; + EXPECT_TRUE(stringParam.SetFromString("0 0 1 1")); + EXPECT_TRUE(stringParam.Get(color)); + EXPECT_EQ(ignition::math::Color(0, 0, 1, 1), color); + EXPECT_EQ("string", stringParam.GetTypeName()); +} + //////////////////////////////////////////////////// /// Test Inf TEST(SetFromString, DoublePositiveInf) @@ -112,10 +135,15 @@ TEST(SetFromString, DoublePositiveInf) { sdf::Param doubleParam("key", "double", "0", false, "description"); double value = 0.; - EXPECT_TRUE(doubleParam.SetFromString(infString)); doubleParam.Get(value); EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), value); + + sdf::Param stringParam("key", "string", "0", false, "description"); + value = 0; + EXPECT_TRUE(stringParam.SetFromString(infString)); + EXPECT_TRUE(stringParam.Get(value)); + EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), value); } } @@ -130,10 +158,15 @@ TEST(SetFromString, DoubleNegativeInf) { sdf::Param doubleParam("key", "double", "0", false, "description"); double value = 0.; - EXPECT_TRUE(doubleParam.SetFromString(infString)); doubleParam.Get(value); - EXPECT_DOUBLE_EQ(- std::numeric_limits::infinity(), value); + EXPECT_DOUBLE_EQ(-std::numeric_limits::infinity(), value); + + sdf::Param stringParam("key", "string", "0", false, "description"); + value = 0; + EXPECT_TRUE(stringParam.SetFromString(infString)); + EXPECT_TRUE(stringParam.Get(value)); + EXPECT_DOUBLE_EQ(-std::numeric_limits::infinity(), value); } } diff --git a/src/SDFExtension.cc b/src/SDFExtension.cc index d2d7d9d44..cd72f41fb 100644 --- a/src/SDFExtension.cc +++ b/src/SDFExtension.cc @@ -26,6 +26,7 @@ SDFExtension::SDFExtension() this->visual_blobs.clear(); this->collision_blobs.clear(); this->setStaticFlag = false; + this->isGravity = false; this->gravity = true; this->isDampingFactor = false; this->isMaxContacts = false; @@ -74,6 +75,7 @@ SDFExtension::SDFExtension(const SDFExtension &_ge) this->visual_blobs = _ge.visual_blobs; this->collision_blobs = _ge.collision_blobs; this->setStaticFlag = _ge.setStaticFlag; + this->isGravity = _ge.isGravity; this->gravity = _ge.gravity; this->isDampingFactor = _ge.isDampingFactor; this->isMaxContacts = _ge.isMaxContacts; diff --git a/src/SDFExtension.hh b/src/SDFExtension.hh index 53ca1df83..8ada34681 100644 --- a/src/SDFExtension.hh +++ b/src/SDFExtension.hh @@ -90,6 +90,7 @@ namespace sdf // body, default off public: bool setStaticFlag; + public: bool isGravity; public: bool gravity; public: bool isDampingFactor; public: double dampingFactor; diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 4021d5a02..375d95136 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -1357,6 +1357,7 @@ void URDF2SDF::ParseSDFExtension(tinyxml2::XMLDocument &_urdfXml) else if (strcmp(childElem->Name(), "turnGravityOff") == 0) { std::string valueStr = GetKeyValueAsString(childElem); + sdf->isGravity = true; // default of gravity is true if (lowerStr(valueStr) == "false" || lowerStr(valueStr) == "no" || @@ -2094,28 +2095,23 @@ void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem, sdfIt->second.begin(); ge != sdfIt->second.end(); ++ge) { // insert gravity - if ((*ge)->gravity) + if ((*ge)->isGravity) { - AddKeyValue(_elem, "gravity", "true"); - } - else - { - AddKeyValue(_elem, "gravity", "false"); + AddKeyValue(_elem, "gravity", (*ge)->gravity ? "true" : "false"); } // damping factor - - tinyxml2::XMLElement *velocityDecay = - _elem->GetDocument()->NewElement("velocity_decay"); if ((*ge)->isDampingFactor) { + tinyxml2::XMLElement *velocityDecay = + _elem->GetDocument()->NewElement("velocity_decay"); /// @todo separate linear and angular velocity decay AddKeyValue(velocityDecay, "linear", Values2str(1, &(*ge)->dampingFactor)); AddKeyValue(velocityDecay, "angular", Values2str(1, &(*ge)->dampingFactor)); + _elem->LinkEndChild(velocityDecay); } - _elem->LinkEndChild(velocityDecay); // selfCollide tag if ((*ge)->isSelfCollide) { diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc index 62924a0a4..50efeaacd 100644 --- a/test/integration/fixed_joint_reduction.cc +++ b/test/integration/fixed_joint_reduction.cc @@ -131,7 +131,9 @@ void FixedJointReductionCollisionVisualExtension(const std::string &_urdfFile, for (sdf::ElementPtr link = urdfModel->GetElement("link"); link; link = link->GetNextElement("link")) { + EXPECT_FALSE(link->HasElement("gravity")); EXPECT_FALSE(link->HasElement("self_collide")); + EXPECT_FALSE(link->HasElement("velocity_decay")); for (sdf::ElementPtr col = link->GetElement("collision"); col; col = col->GetNextElement("collision")) { @@ -181,7 +183,9 @@ void FixedJointReductionCollisionVisualExtension(const std::string &_urdfFile, for (sdf::ElementPtr link = sdfModel->GetElement("link"); link; link = link->GetNextElement("link")) { + EXPECT_FALSE(link->HasElement("gravity")); EXPECT_FALSE(link->HasElement("self_collide")); + EXPECT_FALSE(link->HasElement("velocity_decay")); for (sdf::ElementPtr col = link->GetElement("collision"); col; col = col->GetNextElement("collision")) { diff --git a/test/integration/fixed_joint_reduction_collision_visual_empty_root.sdf b/test/integration/fixed_joint_reduction_collision_visual_empty_root.sdf index 192134688..001d07ac3 100644 --- a/test/integration/fixed_joint_reduction_collision_visual_empty_root.sdf +++ b/test/integration/fixed_joint_reduction_collision_visual_empty_root.sdf @@ -51,8 +51,6 @@ - 1 - 0 diff --git a/test/integration/fixed_joint_reduction_collision_visual_extension.sdf b/test/integration/fixed_joint_reduction_collision_visual_extension.sdf index 37d798beb..2b1639373 100644 --- a/test/integration/fixed_joint_reduction_collision_visual_extension.sdf +++ b/test/integration/fixed_joint_reduction_collision_visual_extension.sdf @@ -134,10 +134,6 @@ - - - 1 - diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 45ba29698..504b2ba55 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -108,7 +108,36 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) if (linkName == "link0") { link0 = link; - break; + } + else if (linkName == "link1" || linkName == "link2") + { + EXPECT_TRUE(link->HasElement("enable_wind")); + + EXPECT_TRUE(link->HasElement("gravity")); + EXPECT_FALSE(link->Get("gravity")); + + EXPECT_TRUE(link->HasElement("velocity_decay")); + auto velocityDecay = link->GetElement("velocity_decay"); + EXPECT_DOUBLE_EQ(0.1, velocityDecay->Get("linear")); + + if (linkName == "link1") + { + EXPECT_TRUE(link->Get("enable_wind")); + EXPECT_DOUBLE_EQ(0.2, velocityDecay->Get("angular")); + } + else + { + // linkName == "link2" + EXPECT_FALSE(link->Get("enable_wind")); + EXPECT_DOUBLE_EQ(0.1, velocityDecay->Get("angular")); + } + } + else + { + // No gazebo tags added + EXPECT_FALSE(link->HasElement("enable_wind")); + EXPECT_FALSE(link->HasElement("gravity")); + EXPECT_FALSE(link->HasElement("velocity_decay")); } } ASSERT_TRUE(link0); diff --git a/test/integration/urdf_gazebo_extensions.urdf b/test/integration/urdf_gazebo_extensions.urdf index 558ca9357..1daf5a8cd 100644 --- a/test/integration/urdf_gazebo_extensions.urdf +++ b/test/integration/urdf_gazebo_extensions.urdf @@ -56,6 +56,14 @@ + + 1 + 0 + + 0.1 + 0.2 + + @@ -81,6 +89,11 @@ + + 0 + 1 + 0.1 +