diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index 2850a2c0b..100d91012 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -168,6 +168,12 @@ class SDFORMAT_VISIBLE ParserConfig /// \brief Get the registered custom model parsers public: const std::vector &CustomModelParsers() const; + /// \brief Set the preserveFixedJoint flag. + public: void URDFSetPreserveFixedJoint(bool _preserveFixedJoint); + + /// \brief Get the preserveFixedJoint flag value. + public: bool URDFPreserveFixedJoint() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/src/ParserConfig.cc b/src/ParserConfig.cc index f0253eefa..f110c63a3 100644 --- a/src/ParserConfig.cc +++ b/src/ParserConfig.cc @@ -45,6 +45,10 @@ class sdf::ParserConfig::Implementation /// \brief Collection of custom model parsers. public: std::vector customParsers; + + /// \brief Flag to explicitly preserve fixed joints when + /// reading the SDF/URDF file. + public: bool preserveFixedJoint = false; }; @@ -153,3 +157,15 @@ const std::vector &ParserConfig::CustomModelParsers() const { return this->dataPtr->customParsers; } + +///////////////////////////////////////////////// +void ParserConfig::URDFSetPreserveFixedJoint(bool _preserveFixedJoint) +{ + this->dataPtr->preserveFixedJoint = _preserveFixedJoint; +} + +///////////////////////////////////////////////// +bool ParserConfig::URDFPreserveFixedJoint() const +{ + return this->dataPtr->preserveFixedJoint; +} diff --git a/src/parser.cc b/src/parser.cc index 7e9d65f46..9d4749a48 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -728,7 +728,7 @@ bool readFileInternal(const std::string &_filename, const bool _convert, { URDF2SDF u2g; auto doc = makeSdfDoc(); - u2g.InitModelFile(filename, &doc); + u2g.InitModelFile(filename, _config, &doc); if (sdf::readDoc(&doc, _sdf, "urdf file", _convert, _config, _errors)) { sdfdbg << "parse from urdf file [" << _filename << "].\n"; @@ -805,7 +805,7 @@ bool readStringInternal(const std::string &_xmlString, const bool _convert, { URDF2SDF u2g; auto doc = makeSdfDoc(); - u2g.InitModelString(_xmlString, &doc); + u2g.InitModelString(_xmlString, _config, &doc); if (sdf::readDoc(&doc, _sdf, std::string(kUrdfStringSource), _convert, _config, _errors)) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index ed1aa470e..9c191d4d9 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -3213,6 +3213,7 @@ void CreateVisual(tinyxml2::XMLElement *_elem, urdf::LinkConstSharedPtr _link, //////////////////////////////////////////////////////////////////////////////// void URDF2SDF::InitModelString(const std::string &_urdfStr, + const ParserConfig& _config, tinyxml2::XMLDocument* _sdfXmlOut, bool _enforceLimits) { @@ -3244,6 +3245,10 @@ void URDF2SDF::InitModelString(const std::string &_urdfStr, sdferr << "Unable to parse URDF string: " << urdfXml.ErrorStr() << "\n"; return; } + + // Set g_reduceFixedJoints based on config value. + g_reduceFixedJoints = !_config.URDFPreserveFixedJoint(); + g_extensions.clear(); g_fixedJointsTransformedInFixedJoints.clear(); g_fixedJointsTransformedInRevoluteJoints.clear(); @@ -3319,22 +3324,24 @@ void URDF2SDF::InitModelString(const std::string &_urdfStr, //////////////////////////////////////////////////////////////////////////////// void URDF2SDF::InitModelDoc(const tinyxml2::XMLDocument *_xmlDoc, + const ParserConfig& _config, tinyxml2::XMLDocument *_sdfXmlDoc) { tinyxml2::XMLPrinter printer; _xmlDoc->Print(&printer); std::string urdfStr = printer.CStr(); - InitModelString(urdfStr, _sdfXmlDoc); + InitModelString(urdfStr, _config, _sdfXmlDoc); } //////////////////////////////////////////////////////////////////////////////// void URDF2SDF::InitModelFile(const std::string &_filename, + const ParserConfig& _config, tinyxml2::XMLDocument *_sdfXmlDoc) { tinyxml2::XMLDocument xmlDoc; if (!xmlDoc.LoadFile(_filename.c_str())) { - this->InitModelDoc(&xmlDoc, _sdfXmlDoc); + this->InitModelDoc(&xmlDoc, _config, _sdfXmlDoc); } else { diff --git a/src/parser_urdf.hh b/src/parser_urdf.hh index a5b960cbd..e3e2d3614 100644 --- a/src/parser_urdf.hh +++ b/src/parser_urdf.hh @@ -23,6 +23,7 @@ #include #include "sdf/Console.hh" +#include "sdf/ParserConfig.hh" #include "sdf/Types.hh" #include "sdf/system_util.hh" @@ -43,22 +44,28 @@ namespace sdf /// \brief convert urdf xml document string to sdf xml document /// \param[in] _xmlDoc document containing the urdf model. + /// \param[in] _config Custom parser configuration /// \param[inout] _sdfXmlDoc document to populate with the sdf model. public: void InitModelDoc(const tinyxml2::XMLDocument* _xmlDoc, + const ParserConfig& _config, tinyxml2::XMLDocument *_sdfXmlDoc); /// \brief convert urdf file to sdf xml document /// \param[in] _urdfStr a string containing filename of the urdf model. + /// \param[in] _config Custom parser configuration /// \param[inout] _sdfXmlDoc document to populate with the sdf model. public: void InitModelFile(const std::string &_filename, + const ParserConfig &_config, tinyxml2::XMLDocument *_sdfXmlDoc); /// \brief convert urdf string to sdf xml document, with option to enforce /// limits. /// \param[in] _urdfStr a string containing model urdf + /// \param[in] _config Custom parser configuration /// \param[inout] _sdfXmlDoc document to populate with the sdf model. /// \param[in] _enforceLimits option to enforce joint limits public: void InitModelString(const std::string &_urdfStr, + const ParserConfig& _parserConfig, tinyxml2::XMLDocument *_sdfXmlDoc, bool _enforceLimits = true); diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc index cf65e1845..70b3ef8c5 100644 --- a/src/parser_urdf_TEST.cc +++ b/src/parser_urdf_TEST.cc @@ -33,20 +33,24 @@ std::string getMinimalUrdfTxt() } ///////////////////////////////////////////////// -std::string convertUrdfStrToSdfStr(const std::string &_urdf) +std::string convertUrdfStrToSdfStr( + const std::string &_urdf, + const sdf::ParserConfig &_config = sdf::ParserConfig::GlobalConfig()) { sdf::URDF2SDF parser_; tinyxml2::XMLDocument sdf_result; - parser_.InitModelString(_urdf, &sdf_result); + parser_.InitModelString(_urdf, _config, &sdf_result); tinyxml2::XMLPrinter printer; sdf_result.Accept(&printer); return printer.CStr(); } ///////////////////////////////////////////////// -void convertUrdfStrToSdf(const std::string &_urdf, sdf::SDF &_sdf) +void convertUrdfStrToSdf( + const std::string &_urdf, sdf::SDF &_sdf, + const sdf::ParserConfig &_config = sdf::ParserConfig::GlobalConfig()) { - _sdf.SetFromString(convertUrdfStrToSdfStr(_urdf)); + _sdf.SetFromString(convertUrdfStrToSdfStr(_urdf, _config)); } ///////////////////////////////////////////////// @@ -57,8 +61,9 @@ TEST(URDFParser, InitModelDoc_EmptyDoc_NoThrow) ASSERT_NO_THROW( tinyxml2::XMLDocument doc = tinyxml2::XMLDocument(); sdf::URDF2SDF parser_; + sdf::ParserConfig config_; tinyxml2::XMLDocument sdf_result; - parser_.InitModelDoc(&doc, &sdf_result); + parser_.InitModelDoc(&doc, config_, &sdf_result); ); // NOLINT(whitespace/parens) } @@ -70,8 +75,9 @@ TEST(URDFParser, InitModelDoc_BasicModel_NoThrow) tinyxml2::XMLDocument doc; doc.Parse(getMinimalUrdfTxt().c_str()); sdf::URDF2SDF parser_; + sdf::ParserConfig config_; tinyxml2::XMLDocument sdf_result; - parser_.InitModelDoc(&doc, &sdf_result); + parser_.InitModelDoc(&doc, config_, &sdf_result); ); // NOLINT(whitespace/parens) } @@ -82,9 +88,10 @@ TEST(URDFParser, ParseResults_BasicModel_ParseEqualToModel) tinyxml2::XMLDocument doc; doc.Parse(getMinimalUrdfTxt().c_str()); sdf::URDF2SDF parser_; + sdf::ParserConfig config_; tinyxml2::XMLDocument sdf_result; - parser_.InitModelDoc(&doc, &sdf_result); + parser_.InitModelDoc(&doc, config_, &sdf_result); tinyxml2::XMLPrinter printer; sdf_result.Print(&printer); @@ -117,8 +124,9 @@ TEST(URDFParser, ParseRobotOriginXYZBlank) tinyxml2::XMLDocument doc; doc.Parse(stream.str().c_str()); sdf::URDF2SDF parser_; + sdf::ParserConfig config_; tinyxml2::XMLDocument sdf_result; - parser_.InitModelDoc(&doc, &sdf_result); + parser_.InitModelDoc(&doc, config_, &sdf_result); tinyxml2::XMLElement *sdf = sdf_result.FirstChildElement("sdf"); ASSERT_NE(nullptr, sdf); tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); @@ -138,8 +146,9 @@ TEST(URDFParser, ParseRobotOriginRPYBlank) tinyxml2::XMLDocument doc; sdf::URDF2SDF parser_; doc.Parse(stream.str().c_str()); + sdf::ParserConfig config_; tinyxml2::XMLDocument sdf_result; - parser_.InitModelDoc(&doc, &sdf_result); + parser_.InitModelDoc(&doc, config_, &sdf_result); tinyxml2::XMLElement *sdf = sdf_result.FirstChildElement("sdf"); ASSERT_NE(nullptr, sdf); tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); @@ -172,8 +181,9 @@ TEST(URDFParser, ParseRobotMaterialBlank) tinyxml2::XMLDocument doc; doc.Parse(stream.str().c_str()); sdf::URDF2SDF parser; + sdf::ParserConfig config_; tinyxml2::XMLDocument sdfXml; - parser.InitModelDoc(&doc, &sdfXml); + parser.InitModelDoc(&doc, config_, &sdfXml); auto sdfElem = sdfXml.FirstChildElement("sdf"); ASSERT_NE(nullptr, sdfElem); auto modelElem = sdfElem->FirstChildElement("model"); @@ -214,9 +224,10 @@ TEST(URDFParser, ParseRobotMaterialName) tinyxml2::XMLDocument doc; doc.Parse(stream.str().c_str()); sdf::URDF2SDF parser; + sdf::ParserConfig config_; tinyxml2::XMLDocument sdfXml; - parser.InitModelDoc(&doc, &sdfXml); + parser.InitModelDoc(&doc, config_, &sdfXml); auto sdfElem = sdfXml.FirstChildElement("sdf"); ASSERT_NE(nullptr, sdfElem); @@ -254,8 +265,9 @@ TEST(URDFParser, ParseRobotOriginInvalidXYZ) tinyxml2::XMLDocument doc; sdf::URDF2SDF parser_; doc.Parse(stream.str().c_str()); + sdf::ParserConfig config_; tinyxml2::XMLDocument sdf_result; - parser_.InitModelDoc(&doc, &sdf_result); + parser_.InitModelDoc(&doc, config_, &sdf_result); tinyxml2::XMLElement *sdf = sdf_result.FirstChildElement("sdf"); ASSERT_NE(nullptr, sdf); tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); @@ -316,8 +328,9 @@ TEST(URDFParser, ParseGazeboLinkFactors) tinyxml2::XMLDocument doc; sdf::URDF2SDF parser_; doc.Parse(stream.str().c_str()); + sdf::ParserConfig config_; tinyxml2::XMLDocument sdf_result; - parser_.InitModelDoc(&doc, &sdf_result); + parser_.InitModelDoc(&doc, config_, &sdf_result); tinyxml2::XMLElement *tmp = sdf_result.FirstChildElement("sdf"); ASSERT_NE(nullptr, tmp); @@ -355,8 +368,9 @@ TEST(URDFParser, ParseGazeboInvalidDampingFactor) tinyxml2::XMLDocument doc; sdf::URDF2SDF parser_; doc.Parse(stream.str().c_str()); + sdf::ParserConfig config_; tinyxml2::XMLDocument sdf_result; - ASSERT_THROW(parser_.InitModelDoc(&doc, &sdf_result), + ASSERT_THROW(parser_.InitModelDoc(&doc, config_, &sdf_result), std::invalid_argument); parser_.ListSDFExtensions(); @@ -428,8 +442,9 @@ TEST(URDFParser, ParseGazeboJointElements) tinyxml2::XMLDocument doc; sdf::URDF2SDF parser_; doc.Parse(stream.str().c_str()); + sdf::ParserConfig config_; tinyxml2::XMLDocument sdf_result; - parser_.InitModelDoc(&doc, &sdf_result); + parser_.InitModelDoc(&doc, config_, &sdf_result); tinyxml2::XMLElement *tmp = sdf_result.FirstChildElement("sdf"); ASSERT_NE(nullptr, tmp); @@ -583,6 +598,55 @@ TEST(URDFParser, CheckFixedJointOptions_preserveFixedJoint) ASSERT_EQ(jointType, "fixed"); } +///////////////////////////////////////////////// +TEST(URDFParser, CheckParserConfig_preserveFixedJoint) +{ + // Convert a fixed joint with only preserveFixedJoint + // (i.e. converted to fixed joint) + std::ostringstream fixedJointPreserveFixedJoint; + fixedJointPreserveFixedJoint << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + + // Check that there is a fixed joint in the converted SDF + sdf::SDF fixedJointPreserveFixedJointSDF; + + // Set the config option to preserve fixed joints. + sdf::ParserConfig config; + config.URDFSetPreserveFixedJoint(true); + + convertUrdfStrToSdf(fixedJointPreserveFixedJoint.str(), + fixedJointPreserveFixedJointSDF, config); + sdf::ElementPtr elem = fixedJointPreserveFixedJointSDF.Root(); + ASSERT_NE(nullptr, elem); + ASSERT_TRUE(elem->HasElement("model")); + elem = elem->GetElement("model"); + ASSERT_TRUE(elem->HasElement("joint")); + elem = elem->GetElement("joint"); + std::string jointType = elem->Get("type"); + ASSERT_EQ(jointType, "fixed"); +} + ///////////////////////////////////////////////// TEST(URDFParser, CheckFixedJointOptions_preserveFixedJoint_and_disableJointLumping) @@ -817,8 +881,9 @@ TEST(URDFParser, OutputPrecision) )"; sdf::URDF2SDF parser; + sdf::ParserConfig config_; tinyxml2::XMLDocument sdfResult; - parser.InitModelString(str, &sdfResult); + parser.InitModelString(str, config_, &sdfResult); auto root = sdfResult.RootElement(); auto model = root->FirstChildElement("model"); @@ -891,8 +956,9 @@ TEST(URDFParser, ParseWhitespace) doc.Parse(str.c_str()); sdf::URDF2SDF parser; + sdf::ParserConfig config_; tinyxml2::XMLDocument sdfXml; - parser.InitModelDoc(&doc, &sdfXml); + parser.InitModelDoc(&doc, config_, &sdfXml); auto root = sdfXml.RootElement(); ASSERT_NE(nullptr, root);