Skip to content

Commit

Permalink
Merge pull request #823 from ignitionrobotics/azeey/11_to_12_20220118
Browse files Browse the repository at this point in the history 
11 ➡️ 12
  • Loading branch information
@azeey
azeey authored Jan 19, 2022
2 parents 6404afd + 1fa7466 commit 0a15990
Show file tree
Hide file tree
Showing 6 changed files with 123 additions and 21 deletions.
6 changes: 6 additions & 0 deletions include/sdf/ParserConfig.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -168,6 +168,12 @@ class SDFORMAT_VISIBLE ParserConfig
/// \brief Get the registered custom model parsers
public: const std::vector<CustomModelParser> &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)
};
Expand Down
16 changes: 16 additions & 0 deletions src/ParserConfig.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,10 @@ class sdf::ParserConfig::Implementation

/// \brief Collection of custom model parsers.
public: std::vector<CustomModelParser> customParsers;

/// \brief Flag to explicitly preserve fixed joints when
/// reading the SDF/URDF file.
public: bool preserveFixedJoint = false;
};


Expand Down Expand Up @@ -153,3 +157,15 @@ const std::vector<CustomModelParser> &ParserConfig::CustomModelParsers() const
{
return this->dataPtr->customParsers;
}

/////////////////////////////////////////////////
void ParserConfig::URDFSetPreserveFixedJoint(bool _preserveFixedJoint)
{
this->dataPtr->preserveFixedJoint = _preserveFixedJoint;
}

/////////////////////////////////////////////////
bool ParserConfig::URDFPreserveFixedJoint() const
{
return this->dataPtr->preserveFixedJoint;
}
4 changes: 2 additions & 2 deletions src/parser.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down Expand Up @@ -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))
Expand Down
11 changes: 9 additions & 2 deletions src/parser_urdf.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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
{
Expand Down
7 changes: 7 additions & 0 deletions src/parser_urdf.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <string>

#include "sdf/Console.hh"
#include "sdf/ParserConfig.hh"
#include "sdf/Types.hh"
#include "sdf/system_util.hh"

Expand All @@ -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);

Expand Down
100 changes: 83 additions & 17 deletions src/parser_urdf_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}

/////////////////////////////////////////////////
Expand All @@ -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)
}

Expand All @@ -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)
}

Expand All @@ -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);
Expand Down Expand Up @@ -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");
Expand All @@ -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");
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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 << "<robot name='test_robot'>"
<< " <link name='link1'>"
<< " <inertial>"
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 0.0'/>"
<< " <mass value='1.0'/>"
<< " <inertia ixx='1.0' ixy='0.0' ixz='0.0'"
<< " iyy='1.0' iyz='0.0' izz='1.0'/>"
<< " </inertial>"
<< " </link>"
<< " <link name='link2'>"
<< " <inertial>"
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 0.0'/>"
<< " <mass value='1.0'/>"
<< " <inertia ixx='1.0' ixy='0.0' ixz='0.0'"
<< " iyy='1.0' iyz='0.0' izz='1.0'/>"
<< " </inertial>"
<< " </link>"
<< " <joint name='joint1_2' type='fixed'>"
<< " <parent link='link1' />"
<< " <child link='link2' />"
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 0.0' />"
<< " </joint>"
<< "</robot>";

// 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<std::string>("type");
ASSERT_EQ(jointType, "fixed");
}

/////////////////////////////////////////////////
TEST(URDFParser,
CheckFixedJointOptions_preserveFixedJoint_and_disableJointLumping)
Expand Down Expand Up @@ -817,8 +881,9 @@ TEST(URDFParser, OutputPrecision)
</robot>)";

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");
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 0a15990

Please sign in to comment.