From a6ffcad7c647fd9a4f6f24a722677412a8592c20 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 8 Feb 2023 03:29:00 +0000 Subject: [PATCH 1/3] Promote sdfdbg messages when ignoring urdf links to sdfwarn, added tests Signed-off-by: Aaron Chong --- src/parser_urdf.cc | 8 +- src/parser_urdf_TEST.cc | 261 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 265 insertions(+), 4 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 854165f35..4afba3998 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -2653,7 +2653,7 @@ void CreateSDF(TiXmlElement *_root, { if (!_link->child_links.empty()) { - sdfdbg << "urdf2sdf: link[" << _link->name + sdfwarn << "urdf2sdf: link[" << _link->name << "] has no inertia, [" << static_cast(_link->child_links.size()) << "] children links ignored.\n"; @@ -2661,7 +2661,7 @@ void CreateSDF(TiXmlElement *_root, if (!_link->child_joints.empty()) { - sdfdbg << "urdf2sdf: link[" << _link->name + sdfwarn << "urdf2sdf: link[" << _link->name << "] has no inertia, [" << static_cast(_link->child_links.size()) << "] children joints ignored.\n"; @@ -2669,13 +2669,13 @@ void CreateSDF(TiXmlElement *_root, if (_link->parent_joint) { - sdfdbg << "urdf2sdf: link[" << _link->name + sdfwarn << "urdf2sdf: link[" << _link->name << "] has no inertia, " << "parent joint [" << _link->parent_joint->name << "] ignored.\n"; } - sdfdbg << "urdf2sdf: link[" << _link->name + sdfwarn << "urdf2sdf: link[" << _link->name << "] has no inertia, not modeled in sdf\n"; return; } diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc index 06e27c111..52993c1b2 100644 --- a/src/parser_urdf_TEST.cc +++ b/src/parser_urdf_TEST.cc @@ -32,6 +32,20 @@ std::string getMinimalUrdfTxt() return stream.str(); } +///////////////////////////////////////////////// +/// Check that _a contains _b +static bool contains(const std::string &_a, const std::string &_b) +{ + return _a.find(_b) != std::string::npos; +} + +///////////////////////////////////////////////// +/// Check that _a does not contain _b +static bool not_contains(const std::string &_a, const std::string &_b) +{ + return _a.find(_b) == std::string::npos; +} + ///////////////////////////////////////////////// std::string convertUrdfStrToSdfStr(const std::string &_urdf) { @@ -844,6 +858,253 @@ TEST(URDFParser, OutputPrecision) EXPECT_EQ("0", poseValues[5]); } +///////////////////////////////////////////////// +TEST(URDFParser, WarningWhenIgnoringLinksWithNoInertia) +{ + // Capture sdferr output + std::stringstream buffer; + auto old = std::cerr.rdbuf(buffer.rdbuf()); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + #endif + + { + // clear the contents of the buffer + buffer.str(""); + + std::ostringstream stream; + stream << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + TiXmlDocument doc; + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser_; + SDF_SUPPRESS_DEPRECATED_END + doc.Parse(stream.str().c_str()); + TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); + + EXPECT_PRED2(not_contains, buffer.str(), + "urdf2sdf: link[link] has no inertia, not modeled in sdf"); + } + + { + // clear the contents of the buffer + buffer.str(""); + + std::ostringstream stream; + stream << "" + << " " + << " " + << ""; + TiXmlDocument doc; + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser_; + SDF_SUPPRESS_DEPRECATED_END + doc.Parse(stream.str().c_str()); + TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); + + EXPECT_PRED2(contains, buffer.str(), + "urdf2sdf: link[link] has no inertia, not modeled in sdf"); + } + + { + // clear the contents of the buffer + buffer.str(""); + + std::ostringstream stream; + stream << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + TiXmlDocument doc; + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser_; + SDF_SUPPRESS_DEPRECATED_END + doc.Parse(stream.str().c_str()); + TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); + + EXPECT_PRED2(contains, buffer.str(), + "urdf2sdf: link[link1] has no inertia, [1] children links ignored"); + EXPECT_PRED2(contains, buffer.str(), + "urdf2sdf: link[link1] has no inertia, [1] children joints ignored"); + EXPECT_PRED2(contains, buffer.str(), + "urdf2sdf: link[link1] has no inertia, not modeled in sdf"); + } + + { + // clear the contents of the buffer + buffer.str(""); + + std::ostringstream stream; + stream << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + TiXmlDocument doc; + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser_; + SDF_SUPPRESS_DEPRECATED_END + doc.Parse(stream.str().c_str()); + TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); + + EXPECT_PRED2(contains, buffer.str(), + "urdf2sdf: link[link2] has no inertia, parent joint [joint1_2] ignored"); + EXPECT_PRED2(contains, buffer.str(), + "urdf2sdf: link[link2] has no inertia, not modeled in sdf"); + } + + { + // clear the contents of the buffer + buffer.str(""); + + std::ostringstream stream; + stream << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + TiXmlDocument doc; + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser_; + SDF_SUPPRESS_DEPRECATED_END + doc.Parse(stream.str().c_str()); + TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); + + EXPECT_PRED2(contains, buffer.str(), + "urdf2sdf: link[link1] has no inertia, [1] children links ignored"); + EXPECT_PRED2(contains, buffer.str(), + "urdf2sdf: link[link1] has no inertia, [1] children joints ignored"); + EXPECT_PRED2(contains, buffer.str(), + "urdf2sdf: link[link1] has no inertia, not modeled in sdf"); + + // It parses in sequence, therefore once ignored, no warnings for link2 will + // be issued. + EXPECT_PRED2(not_contains, buffer.str(), + "urdf2sdf: link[link2] has no inertia, parent joint [joint1_2] will be " + "ignored"); + EXPECT_PRED2(not_contains, buffer.str(), + "urdf2sdf: link[link2] has no inertia, not modeled in sdf"); + } + + // Revert cerr rdbug so as to not interfere with other tests + std::cerr.rdbuf(old); + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(true); + #endif +} + +///////////////////////////////////////////////// +TEST(URDFParser, WarningWhenIgnoringLinksWithSmallInertia) +{ + // Capture sdferr output + std::stringstream buffer; + auto old = std::cerr.rdbuf(buffer.rdbuf()); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + #endif + + { + // clear the contents of the buffer + buffer.str(""); + + std::ostringstream stream; + stream << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + TiXmlDocument doc; + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser_; + SDF_SUPPRESS_DEPRECATED_END + doc.Parse(stream.str().c_str()); + TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); + + EXPECT_PRED2(contains, buffer.str(), + "urdf2sdf: link[link] has no inertia, not modeled in sdf"); + } + + { + // clear the contents of the buffer + buffer.str(""); + + std::ostringstream stream; + stream << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + TiXmlDocument doc; + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser_; + SDF_SUPPRESS_DEPRECATED_END + doc.Parse(stream.str().c_str()); + TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); + + EXPECT_PRED2(not_contains, buffer.str(), + "urdf2sdf: link[link] has no inertia, not modeled in sdf"); + } + + // Revert cerr rdbug so as to not interfere with other tests + std::cerr.rdbuf(old); + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(true); + #endif +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) From fcc67066a3a7b3f8c5f3b23d30852b4380f7597c Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 8 Feb 2023 06:00:27 +0000 Subject: [PATCH 2/3] Make warnings more epxlicit, whether inertia is missing or mass is small Signed-off-by: Aaron Chong --- src/parser_urdf.cc | 22 +++++++++++++++++----- src/parser_urdf_TEST.cc | 37 ++++++++++++++++++++++--------------- 2 files changed, 39 insertions(+), 20 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 4afba3998..5974e6d7d 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -2651,10 +2651,17 @@ void CreateSDF(TiXmlElement *_root, if (_link->name != "world" && ((!_link->inertial) || gz::math::equal(_link->inertial->mass, 0.0))) { + const std::string inertia_issue = + _link->inertial && gz::math::equal(_link->inertial->mass, 0.0) ? + "a mass value of less than 1e-6" : + "no inertia defined"; + if (!_link->child_links.empty()) { sdfwarn << "urdf2sdf: link[" << _link->name - << "] has no inertia, [" + << "] has " + << inertia_issue + << ", [" << static_cast(_link->child_links.size()) << "] children links ignored.\n"; } @@ -2662,7 +2669,9 @@ void CreateSDF(TiXmlElement *_root, if (!_link->child_joints.empty()) { sdfwarn << "urdf2sdf: link[" << _link->name - << "] has no inertia, [" + << "] has " + << inertia_issue + << ", [" << static_cast(_link->child_links.size()) << "] children joints ignored.\n"; } @@ -2670,13 +2679,16 @@ void CreateSDF(TiXmlElement *_root, if (_link->parent_joint) { sdfwarn << "urdf2sdf: link[" << _link->name - << "] has no inertia, " - << "parent joint [" << _link->parent_joint->name + << "] has " + << inertia_issue + << ", parent joint [" << _link->parent_joint->name << "] ignored.\n"; } sdfwarn << "urdf2sdf: link[" << _link->name - << "] has no inertia, not modeled in sdf\n"; + << "] has " + << inertia_issue + << ", not modeled in sdf\n"; return; } diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc index 52993c1b2..0a61aaa24 100644 --- a/src/parser_urdf_TEST.cc +++ b/src/parser_urdf_TEST.cc @@ -893,7 +893,7 @@ TEST(URDFParser, WarningWhenIgnoringLinksWithNoInertia) TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); EXPECT_PRED2(not_contains, buffer.str(), - "urdf2sdf: link[link] has no inertia, not modeled in sdf"); + "urdf2sdf: link[link] has no inertia defined, not modeled in sdf"); } { @@ -913,7 +913,7 @@ TEST(URDFParser, WarningWhenIgnoringLinksWithNoInertia) TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); EXPECT_PRED2(contains, buffer.str(), - "urdf2sdf: link[link] has no inertia, not modeled in sdf"); + "urdf2sdf: link[link] has no inertia defined, not modeled in sdf"); } { @@ -946,11 +946,13 @@ TEST(URDFParser, WarningWhenIgnoringLinksWithNoInertia) TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); EXPECT_PRED2(contains, buffer.str(), - "urdf2sdf: link[link1] has no inertia, [1] children links ignored"); + "urdf2sdf: link[link1] has no inertia defined, [1] children links " + "ignored"); EXPECT_PRED2(contains, buffer.str(), - "urdf2sdf: link[link1] has no inertia, [1] children joints ignored"); + "urdf2sdf: link[link1] has no inertia defined, [1] children joints " + "ignored"); EXPECT_PRED2(contains, buffer.str(), - "urdf2sdf: link[link1] has no inertia, not modeled in sdf"); + "urdf2sdf: link[link1] has no inertia defined, not modeled in sdf"); } { @@ -983,9 +985,10 @@ TEST(URDFParser, WarningWhenIgnoringLinksWithNoInertia) TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); EXPECT_PRED2(contains, buffer.str(), - "urdf2sdf: link[link2] has no inertia, parent joint [joint1_2] ignored"); + "urdf2sdf: link[link2] has no inertia defined, parent joint [joint1_2] " + "ignored"); EXPECT_PRED2(contains, buffer.str(), - "urdf2sdf: link[link2] has no inertia, not modeled in sdf"); + "urdf2sdf: link[link2] has no inertia defined, not modeled in sdf"); } { @@ -1011,19 +1014,21 @@ TEST(URDFParser, WarningWhenIgnoringLinksWithNoInertia) TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); EXPECT_PRED2(contains, buffer.str(), - "urdf2sdf: link[link1] has no inertia, [1] children links ignored"); + "urdf2sdf: link[link1] has no inertia defined, [1] children links " + "ignored"); EXPECT_PRED2(contains, buffer.str(), - "urdf2sdf: link[link1] has no inertia, [1] children joints ignored"); + "urdf2sdf: link[link1] has no inertia defined, [1] children joints " + "ignored"); EXPECT_PRED2(contains, buffer.str(), - "urdf2sdf: link[link1] has no inertia, not modeled in sdf"); + "urdf2sdf: link[link1] has no inertia defined, not modeled in sdf"); // It parses in sequence, therefore once ignored, no warnings for link2 will // be issued. EXPECT_PRED2(not_contains, buffer.str(), - "urdf2sdf: link[link2] has no inertia, parent joint [joint1_2] will be " - "ignored"); + "urdf2sdf: link[link2] has no inertia defined, parent joint [joint1_2] " + "will be ignored"); EXPECT_PRED2(not_contains, buffer.str(), - "urdf2sdf: link[link2] has no inertia, not modeled in sdf"); + "urdf2sdf: link[link2] has no inertia defined, not modeled in sdf"); } // Revert cerr rdbug so as to not interfere with other tests @@ -1068,7 +1073,8 @@ TEST(URDFParser, WarningWhenIgnoringLinksWithSmallInertia) TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); EXPECT_PRED2(contains, buffer.str(), - "urdf2sdf: link[link] has no inertia, not modeled in sdf"); + "urdf2sdf: link[link] has a mass value of less than 1e-6, not modeled " + "in sdf"); } { @@ -1095,7 +1101,8 @@ TEST(URDFParser, WarningWhenIgnoringLinksWithSmallInertia) TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); EXPECT_PRED2(not_contains, buffer.str(), - "urdf2sdf: link[link] has no inertia, not modeled in sdf"); + "urdf2sdf: link[link] has a mass value of less than 1e-6, not modeled " + "in sdf"); } // Revert cerr rdbug so as to not interfere with other tests From b32365707aa15a8e3fb8168abcfe25ec253dc6a9 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 8 Feb 2023 06:16:25 +0000 Subject: [PATCH 3/3] Fix line length lint Signed-off-by: Aaron Chong --- src/parser_urdf_TEST.cc | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc index 0a61aaa24..8723d3f90 100644 --- a/src/parser_urdf_TEST.cc +++ b/src/parser_urdf_TEST.cc @@ -927,7 +927,8 @@ TEST(URDFParser, WarningWhenIgnoringLinksWithNoInertia) << " " << " " << " " - << " " + << " " << " " << " " << " " @@ -973,7 +974,8 @@ TEST(URDFParser, WarningWhenIgnoringLinksWithNoInertia) << " " << " " << " " - << " " + << " " << " " << " " << ""; @@ -1002,7 +1004,8 @@ TEST(URDFParser, WarningWhenIgnoringLinksWithNoInertia) << " " << " " << " " - << " " + << " " << " " << " " << "";