Skip to content

Commit 0a15990

Browse files
azeeyazeey
authored
Merge pull request #823 from ignitionrobotics/azeey/11_to_12_20220118
11 ➡️ 12
2 parents 6404afd + 1fa7466 commit 0a15990

File tree

6 files changed

+123
-21
lines changed

6 files changed

+123
-21
lines changed

‎include/sdf/ParserConfig.hh

+6
Original file line numberDiff line numberDiff line change
@@ -168,6 +168,12 @@ class SDFORMAT_VISIBLE ParserConfig
168168
/// \brief Get the registered custom model parsers
169169
public: const std::vector<CustomModelParser> &CustomModelParsers() const;
170170

171+
/// \brief Set the preserveFixedJoint flag.
172+
public: void URDFSetPreserveFixedJoint(bool _preserveFixedJoint);
173+
174+
/// \brief Get the preserveFixedJoint flag value.
175+
public: bool URDFPreserveFixedJoint() const;
176+
171177
/// \brief Private data pointer.
172178
IGN_UTILS_IMPL_PTR(dataPtr)
173179
};

‎src/ParserConfig.cc

+16
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,10 @@ class sdf::ParserConfig::Implementation
4545

4646
/// \brief Collection of custom model parsers.
4747
public: std::vector<CustomModelParser> customParsers;
48+
49+
/// \brief Flag to explicitly preserve fixed joints when
50+
/// reading the SDF/URDF file.
51+
public: bool preserveFixedJoint = false;
4852
};
4953

5054

@@ -153,3 +157,15 @@ const std::vector<CustomModelParser> &ParserConfig::CustomModelParsers() const
153157
{
154158
return this->dataPtr->customParsers;
155159
}
160+
161+
/////////////////////////////////////////////////
162+
void ParserConfig::URDFSetPreserveFixedJoint(bool _preserveFixedJoint)
163+
{
164+
this->dataPtr->preserveFixedJoint = _preserveFixedJoint;
165+
}
166+
167+
/////////////////////////////////////////////////
168+
bool ParserConfig::URDFPreserveFixedJoint() const
169+
{
170+
return this->dataPtr->preserveFixedJoint;
171+
}

‎src/parser.cc

+2-2
Original file line numberDiff line numberDiff line change
@@ -728,7 +728,7 @@ bool readFileInternal(const std::string &_filename, const bool _convert,
728728
{
729729
URDF2SDF u2g;
730730
auto doc = makeSdfDoc();
731-
u2g.InitModelFile(filename, &doc);
731+
u2g.InitModelFile(filename, _config, &doc);
732732
if (sdf::readDoc(&doc, _sdf, "urdf file", _convert, _config, _errors))
733733
{
734734
sdfdbg << "parse from urdf file [" << _filename << "].\n";
@@ -805,7 +805,7 @@ bool readStringInternal(const std::string &_xmlString, const bool _convert,
805805
{
806806
URDF2SDF u2g;
807807
auto doc = makeSdfDoc();
808-
u2g.InitModelString(_xmlString, &doc);
808+
u2g.InitModelString(_xmlString, _config, &doc);
809809

810810
if (sdf::readDoc(&doc, _sdf, std::string(kUrdfStringSource), _convert,
811811
_config, _errors))

‎src/parser_urdf.cc

+9-2
Original file line numberDiff line numberDiff line change
@@ -3213,6 +3213,7 @@ void CreateVisual(tinyxml2::XMLElement *_elem, urdf::LinkConstSharedPtr _link,
32133213

32143214
////////////////////////////////////////////////////////////////////////////////
32153215
void URDF2SDF::InitModelString(const std::string &_urdfStr,
3216+
const ParserConfig& _config,
32163217
tinyxml2::XMLDocument* _sdfXmlOut,
32173218
bool _enforceLimits)
32183219
{
@@ -3244,6 +3245,10 @@ void URDF2SDF::InitModelString(const std::string &_urdfStr,
32443245
sdferr << "Unable to parse URDF string: " << urdfXml.ErrorStr() << "\n";
32453246
return;
32463247
}
3248+
3249+
// Set g_reduceFixedJoints based on config value.
3250+
g_reduceFixedJoints = !_config.URDFPreserveFixedJoint();
3251+
32473252
g_extensions.clear();
32483253
g_fixedJointsTransformedInFixedJoints.clear();
32493254
g_fixedJointsTransformedInRevoluteJoints.clear();
@@ -3319,22 +3324,24 @@ void URDF2SDF::InitModelString(const std::string &_urdfStr,
33193324

33203325
////////////////////////////////////////////////////////////////////////////////
33213326
void URDF2SDF::InitModelDoc(const tinyxml2::XMLDocument *_xmlDoc,
3327+
const ParserConfig& _config,
33223328
tinyxml2::XMLDocument *_sdfXmlDoc)
33233329
{
33243330
tinyxml2::XMLPrinter printer;
33253331
_xmlDoc->Print(&printer);
33263332
std::string urdfStr = printer.CStr();
3327-
InitModelString(urdfStr, _sdfXmlDoc);
3333+
InitModelString(urdfStr, _config, _sdfXmlDoc);
33283334
}
33293335

33303336
////////////////////////////////////////////////////////////////////////////////
33313337
void URDF2SDF::InitModelFile(const std::string &_filename,
3338+
const ParserConfig& _config,
33323339
tinyxml2::XMLDocument *_sdfXmlDoc)
33333340
{
33343341
tinyxml2::XMLDocument xmlDoc;
33353342
if (!xmlDoc.LoadFile(_filename.c_str()))
33363343
{
3337-
this->InitModelDoc(&xmlDoc, _sdfXmlDoc);
3344+
this->InitModelDoc(&xmlDoc, _config, _sdfXmlDoc);
33383345
}
33393346
else
33403347
{

‎src/parser_urdf.hh

+7
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include <string>
2424

2525
#include "sdf/Console.hh"
26+
#include "sdf/ParserConfig.hh"
2627
#include "sdf/Types.hh"
2728
#include "sdf/system_util.hh"
2829

@@ -43,22 +44,28 @@ namespace sdf
4344

4445
/// \brief convert urdf xml document string to sdf xml document
4546
/// \param[in] _xmlDoc document containing the urdf model.
47+
/// \param[in] _config Custom parser configuration
4648
/// \param[inout] _sdfXmlDoc document to populate with the sdf model.
4749
public: void InitModelDoc(const tinyxml2::XMLDocument* _xmlDoc,
50+
const ParserConfig& _config,
4851
tinyxml2::XMLDocument *_sdfXmlDoc);
4952

5053
/// \brief convert urdf file to sdf xml document
5154
/// \param[in] _urdfStr a string containing filename of the urdf model.
55+
/// \param[in] _config Custom parser configuration
5256
/// \param[inout] _sdfXmlDoc document to populate with the sdf model.
5357
public: void InitModelFile(const std::string &_filename,
58+
const ParserConfig &_config,
5459
tinyxml2::XMLDocument *_sdfXmlDoc);
5560

5661
/// \brief convert urdf string to sdf xml document, with option to enforce
5762
/// limits.
5863
/// \param[in] _urdfStr a string containing model urdf
64+
/// \param[in] _config Custom parser configuration
5965
/// \param[inout] _sdfXmlDoc document to populate with the sdf model.
6066
/// \param[in] _enforceLimits option to enforce joint limits
6167
public: void InitModelString(const std::string &_urdfStr,
68+
const ParserConfig& _parserConfig,
6269
tinyxml2::XMLDocument *_sdfXmlDoc,
6370
bool _enforceLimits = true);
6471

‎src/parser_urdf_TEST.cc

+83-17
Original file line numberDiff line numberDiff line change
@@ -33,20 +33,24 @@ std::string getMinimalUrdfTxt()
3333
}
3434

3535
/////////////////////////////////////////////////
36-
std::string convertUrdfStrToSdfStr(const std::string &_urdf)
36+
std::string convertUrdfStrToSdfStr(
37+
const std::string &_urdf,
38+
const sdf::ParserConfig &_config = sdf::ParserConfig::GlobalConfig())
3739
{
3840
sdf::URDF2SDF parser_;
3941
tinyxml2::XMLDocument sdf_result;
40-
parser_.InitModelString(_urdf, &sdf_result);
42+
parser_.InitModelString(_urdf, _config, &sdf_result);
4143
tinyxml2::XMLPrinter printer;
4244
sdf_result.Accept(&printer);
4345
return printer.CStr();
4446
}
4547

4648
/////////////////////////////////////////////////
47-
void convertUrdfStrToSdf(const std::string &_urdf, sdf::SDF &_sdf)
49+
void convertUrdfStrToSdf(
50+
const std::string &_urdf, sdf::SDF &_sdf,
51+
const sdf::ParserConfig &_config = sdf::ParserConfig::GlobalConfig())
4852
{
49-
_sdf.SetFromString(convertUrdfStrToSdfStr(_urdf));
53+
_sdf.SetFromString(convertUrdfStrToSdfStr(_urdf, _config));
5054
}
5155

5256
/////////////////////////////////////////////////
@@ -57,8 +61,9 @@ TEST(URDFParser, InitModelDoc_EmptyDoc_NoThrow)
5761
ASSERT_NO_THROW(
5862
tinyxml2::XMLDocument doc = tinyxml2::XMLDocument();
5963
sdf::URDF2SDF parser_;
64+
sdf::ParserConfig config_;
6065
tinyxml2::XMLDocument sdf_result;
61-
parser_.InitModelDoc(&doc, &sdf_result);
66+
parser_.InitModelDoc(&doc, config_, &sdf_result);
6267
); // NOLINT(whitespace/parens)
6368
}
6469

@@ -70,8 +75,9 @@ TEST(URDFParser, InitModelDoc_BasicModel_NoThrow)
7075
tinyxml2::XMLDocument doc;
7176
doc.Parse(getMinimalUrdfTxt().c_str());
7277
sdf::URDF2SDF parser_;
78+
sdf::ParserConfig config_;
7379
tinyxml2::XMLDocument sdf_result;
74-
parser_.InitModelDoc(&doc, &sdf_result);
80+
parser_.InitModelDoc(&doc, config_, &sdf_result);
7581
); // NOLINT(whitespace/parens)
7682
}
7783

@@ -82,9 +88,10 @@ TEST(URDFParser, ParseResults_BasicModel_ParseEqualToModel)
8288
tinyxml2::XMLDocument doc;
8389
doc.Parse(getMinimalUrdfTxt().c_str());
8490
sdf::URDF2SDF parser_;
91+
sdf::ParserConfig config_;
8592

8693
tinyxml2::XMLDocument sdf_result;
87-
parser_.InitModelDoc(&doc, &sdf_result);
94+
parser_.InitModelDoc(&doc, config_, &sdf_result);
8895

8996
tinyxml2::XMLPrinter printer;
9097
sdf_result.Print(&printer);
@@ -117,8 +124,9 @@ TEST(URDFParser, ParseRobotOriginXYZBlank)
117124
tinyxml2::XMLDocument doc;
118125
doc.Parse(stream.str().c_str());
119126
sdf::URDF2SDF parser_;
127+
sdf::ParserConfig config_;
120128
tinyxml2::XMLDocument sdf_result;
121-
parser_.InitModelDoc(&doc, &sdf_result);
129+
parser_.InitModelDoc(&doc, config_, &sdf_result);
122130
tinyxml2::XMLElement *sdf = sdf_result.FirstChildElement("sdf");
123131
ASSERT_NE(nullptr, sdf);
124132
tinyxml2::XMLElement *model = sdf->FirstChildElement("model");
@@ -138,8 +146,9 @@ TEST(URDFParser, ParseRobotOriginRPYBlank)
138146
tinyxml2::XMLDocument doc;
139147
sdf::URDF2SDF parser_;
140148
doc.Parse(stream.str().c_str());
149+
sdf::ParserConfig config_;
141150
tinyxml2::XMLDocument sdf_result;
142-
parser_.InitModelDoc(&doc, &sdf_result);
151+
parser_.InitModelDoc(&doc, config_, &sdf_result);
143152
tinyxml2::XMLElement *sdf = sdf_result.FirstChildElement("sdf");
144153
ASSERT_NE(nullptr, sdf);
145154
tinyxml2::XMLElement *model = sdf->FirstChildElement("model");
@@ -172,8 +181,9 @@ TEST(URDFParser, ParseRobotMaterialBlank)
172181
tinyxml2::XMLDocument doc;
173182
doc.Parse(stream.str().c_str());
174183
sdf::URDF2SDF parser;
184+
sdf::ParserConfig config_;
175185
tinyxml2::XMLDocument sdfXml;
176-
parser.InitModelDoc(&doc, &sdfXml);
186+
parser.InitModelDoc(&doc, config_, &sdfXml);
177187
auto sdfElem = sdfXml.FirstChildElement("sdf");
178188
ASSERT_NE(nullptr, sdfElem);
179189
auto modelElem = sdfElem->FirstChildElement("model");
@@ -214,9 +224,10 @@ TEST(URDFParser, ParseRobotMaterialName)
214224
tinyxml2::XMLDocument doc;
215225
doc.Parse(stream.str().c_str());
216226
sdf::URDF2SDF parser;
227+
sdf::ParserConfig config_;
217228

218229
tinyxml2::XMLDocument sdfXml;
219-
parser.InitModelDoc(&doc, &sdfXml);
230+
parser.InitModelDoc(&doc, config_, &sdfXml);
220231

221232
auto sdfElem = sdfXml.FirstChildElement("sdf");
222233
ASSERT_NE(nullptr, sdfElem);
@@ -254,8 +265,9 @@ TEST(URDFParser, ParseRobotOriginInvalidXYZ)
254265
tinyxml2::XMLDocument doc;
255266
sdf::URDF2SDF parser_;
256267
doc.Parse(stream.str().c_str());
268+
sdf::ParserConfig config_;
257269
tinyxml2::XMLDocument sdf_result;
258-
parser_.InitModelDoc(&doc, &sdf_result);
270+
parser_.InitModelDoc(&doc, config_, &sdf_result);
259271
tinyxml2::XMLElement *sdf = sdf_result.FirstChildElement("sdf");
260272
ASSERT_NE(nullptr, sdf);
261273
tinyxml2::XMLElement *model = sdf->FirstChildElement("model");
@@ -316,8 +328,9 @@ TEST(URDFParser, ParseGazeboLinkFactors)
316328
tinyxml2::XMLDocument doc;
317329
sdf::URDF2SDF parser_;
318330
doc.Parse(stream.str().c_str());
331+
sdf::ParserConfig config_;
319332
tinyxml2::XMLDocument sdf_result;
320-
parser_.InitModelDoc(&doc, &sdf_result);
333+
parser_.InitModelDoc(&doc, config_, &sdf_result);
321334

322335
tinyxml2::XMLElement *tmp = sdf_result.FirstChildElement("sdf");
323336
ASSERT_NE(nullptr, tmp);
@@ -355,8 +368,9 @@ TEST(URDFParser, ParseGazeboInvalidDampingFactor)
355368
tinyxml2::XMLDocument doc;
356369
sdf::URDF2SDF parser_;
357370
doc.Parse(stream.str().c_str());
371+
sdf::ParserConfig config_;
358372
tinyxml2::XMLDocument sdf_result;
359-
ASSERT_THROW(parser_.InitModelDoc(&doc, &sdf_result),
373+
ASSERT_THROW(parser_.InitModelDoc(&doc, config_, &sdf_result),
360374
std::invalid_argument);
361375

362376
parser_.ListSDFExtensions();
@@ -428,8 +442,9 @@ TEST(URDFParser, ParseGazeboJointElements)
428442
tinyxml2::XMLDocument doc;
429443
sdf::URDF2SDF parser_;
430444
doc.Parse(stream.str().c_str());
445+
sdf::ParserConfig config_;
431446
tinyxml2::XMLDocument sdf_result;
432-
parser_.InitModelDoc(&doc, &sdf_result);
447+
parser_.InitModelDoc(&doc, config_, &sdf_result);
433448

434449
tinyxml2::XMLElement *tmp = sdf_result.FirstChildElement("sdf");
435450
ASSERT_NE(nullptr, tmp);
@@ -583,6 +598,55 @@ TEST(URDFParser, CheckFixedJointOptions_preserveFixedJoint)
583598
ASSERT_EQ(jointType, "fixed");
584599
}
585600

601+
/////////////////////////////////////////////////
602+
TEST(URDFParser, CheckParserConfig_preserveFixedJoint)
603+
{
604+
// Convert a fixed joint with only preserveFixedJoint
605+
// (i.e. converted to fixed joint)
606+
std::ostringstream fixedJointPreserveFixedJoint;
607+
fixedJointPreserveFixedJoint << "<robot name='test_robot'>"
608+
<< " <link name='link1'>"
609+
<< " <inertial>"
610+
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 0.0'/>"
611+
<< " <mass value='1.0'/>"
612+
<< " <inertia ixx='1.0' ixy='0.0' ixz='0.0'"
613+
<< " iyy='1.0' iyz='0.0' izz='1.0'/>"
614+
<< " </inertial>"
615+
<< " </link>"
616+
<< " <link name='link2'>"
617+
<< " <inertial>"
618+
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 0.0'/>"
619+
<< " <mass value='1.0'/>"
620+
<< " <inertia ixx='1.0' ixy='0.0' ixz='0.0'"
621+
<< " iyy='1.0' iyz='0.0' izz='1.0'/>"
622+
<< " </inertial>"
623+
<< " </link>"
624+
<< " <joint name='joint1_2' type='fixed'>"
625+
<< " <parent link='link1' />"
626+
<< " <child link='link2' />"
627+
<< " <origin xyz='0.0 0.0 0.0' rpy='0.0 0.0 0.0' />"
628+
<< " </joint>"
629+
<< "</robot>";
630+
631+
// Check that there is a fixed joint in the converted SDF
632+
sdf::SDF fixedJointPreserveFixedJointSDF;
633+
634+
// Set the config option to preserve fixed joints.
635+
sdf::ParserConfig config;
636+
config.URDFSetPreserveFixedJoint(true);
637+
638+
convertUrdfStrToSdf(fixedJointPreserveFixedJoint.str(),
639+
fixedJointPreserveFixedJointSDF, config);
640+
sdf::ElementPtr elem = fixedJointPreserveFixedJointSDF.Root();
641+
ASSERT_NE(nullptr, elem);
642+
ASSERT_TRUE(elem->HasElement("model"));
643+
elem = elem->GetElement("model");
644+
ASSERT_TRUE(elem->HasElement("joint"));
645+
elem = elem->GetElement("joint");
646+
std::string jointType = elem->Get<std::string>("type");
647+
ASSERT_EQ(jointType, "fixed");
648+
}
649+
586650
/////////////////////////////////////////////////
587651
TEST(URDFParser,
588652
CheckFixedJointOptions_preserveFixedJoint_and_disableJointLumping)
@@ -817,8 +881,9 @@ TEST(URDFParser, OutputPrecision)
817881
</robot>)";
818882

819883
sdf::URDF2SDF parser;
884+
sdf::ParserConfig config_;
820885
tinyxml2::XMLDocument sdfResult;
821-
parser.InitModelString(str, &sdfResult);
886+
parser.InitModelString(str, config_, &sdfResult);
822887

823888
auto root = sdfResult.RootElement();
824889
auto model = root->FirstChildElement("model");
@@ -891,8 +956,9 @@ TEST(URDFParser, ParseWhitespace)
891956
doc.Parse(str.c_str());
892957

893958
sdf::URDF2SDF parser;
959+
sdf::ParserConfig config_;
894960
tinyxml2::XMLDocument sdfXml;
895-
parser.InitModelDoc(&doc, &sdfXml);
961+
parser.InitModelDoc(&doc, config_, &sdfXml);
896962

897963
auto root = sdfXml.RootElement();
898964
ASSERT_NE(nullptr, root);

0 commit comments

Comments
 (0)