Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

11 ➡️ 12 #823

Merged
merged 2 commits into from
Jan 19, 2022
Merged

11 ➡️ 12 #823

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d7d25a8
Add ParserConfig flag for preserveFixedJoint (#815)
varunagrawal Jan 14, 2022
1fa7466
Merge 11 into 12
azeey Jan 18, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions include/sdf/ParserConfig.hh
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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