diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc
index d8d2b2352..549e8a38d 100644
--- a/src/parser_urdf.cc
+++ b/src/parser_urdf.cc
@@ -1604,6 +1604,7 @@ void CopyBlob(tinyxml2::XMLElement *_src, tinyxml2::XMLElement *_blob_parent)
void InsertSDFExtensionCollision(tinyxml2::XMLElement *_elem,
const std::string &_linkName)
{
+ bool linkFound = false;
// loop through extensions for the whole model
// and see which ones belong to _linkName
// This might be complicated since there's:
@@ -1615,6 +1616,7 @@ void InsertSDFExtensionCollision(tinyxml2::XMLElement *_elem,
{
if (sdfIt->first == _linkName)
{
+ linkFound = true;
// std::cerr << "============================\n";
// std::cerr << "working on g_extensions for link ["
// << sdfIt->first << "]\n";
@@ -1908,12 +1910,19 @@ void InsertSDFExtensionCollision(tinyxml2::XMLElement *_elem,
}
}
}
+ // If we didn't find the link, emit a warning
+ if (!linkFound) {
+ sdfwarn << " tag reference[" << _linkName << "] does not exist"
+ << " in the URDF model. Please ensure that the reference attribute"
+ << " matches the name of a link.";
+ }
}
////////////////////////////////////////////////////////////////////////////////
void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem,
const std::string &_linkName)
{
+ bool linkFound = false;
// loop through extensions for the whole model
// and see which ones belong to _linkName
// This might be complicated since there's:
@@ -1925,6 +1934,7 @@ void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem,
{
if (sdfIt->first == _linkName)
{
+ linkFound = true;
// std::cerr << "============================\n";
// std::cerr << "working on g_extensions for link ["
// << sdfIt->first << "]\n";
@@ -2102,18 +2112,26 @@ void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem,
}
}
}
+ // If we didn't find the link, emit a warning
+ if (!linkFound) {
+ sdfwarn << " tag reference[" << _linkName << "] does not exist"
+ << " in the URDF model. Please ensure that the reference attribute"
+ << " matches the name of a link.";
+ }
}
////////////////////////////////////////////////////////////////////////////////
void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
const std::string &_linkName)
{
+ bool linkFound = false;
for (StringSDFExtensionPtrMap::iterator
sdfIt = g_extensions.begin();
sdfIt != g_extensions.end(); ++sdfIt)
{
if (sdfIt->first == _linkName)
{
+ linkFound = true;
sdfdbg << "inserting extension with reference ["
<< _linkName << "] into link.\n";
for (std::vector::iterator ge =
@@ -2153,12 +2171,19 @@ void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
}
}
}
+ // If we didn't find the link, emit a warning
+ if (!linkFound) {
+ sdfwarn << " tag reference[" << _linkName << "] does not exist"
+ << " in the URDF model. Please ensure that the reference attribute"
+ << " matches the name of a link.";
+ }
}
////////////////////////////////////////////////////////////////////////////////
void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem,
const std::string &_jointName)
{
+ bool jointFound = false;
auto* doc = _elem->GetDocument();
for (StringSDFExtensionPtrMap::iterator
sdfIt = g_extensions.begin();
@@ -2166,6 +2191,7 @@ void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem,
{
if (sdfIt->first == _jointName)
{
+ jointFound = true;
for (std::vector::iterator
ge = sdfIt->second.begin();
ge != sdfIt->second.end(); ++ge)
@@ -2300,6 +2326,13 @@ void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem,
}
}
}
+
+ // If we didn't find the link, emit a warning
+ if (!jointFound) {
+ sdfwarn << " tag reference[" << _jointName << "] does not exist"
+ << " in the URDF model. Please ensure that the reference attribute"
+ << " matches the name of a joint.";
+ }
}
////////////////////////////////////////////////////////////////////////////////
diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc
index 96b940d99..58a4f9643 100644
--- a/src/parser_urdf_TEST.cc
+++ b/src/parser_urdf_TEST.cc
@@ -2460,6 +2460,179 @@ TEST(URDFParser, ZeroMassLeafLink)
}
}
+/////////////////////////////////////////////////
+TEST(URDFParser, ParseGazeboRefDoesntExistWarningMessage)
+{
+ // Redirect sdfwarn output
+ std::stringstream buffer;
+ sdf::testing::RedirectConsoleStream redir(
+ sdf::Console::Instance()->GetMsgStream(), &buffer);
+#ifdef _WIN32
+ sdf::Console::Instance()->SetQuiet(false);
+ sdf::testing::ScopeExit revertSetQuiet(
+ []
+ {
+ sdf::Console::Instance()->SetQuiet(true);
+ });
+#endif
+
+ // test if reference to link exists
+ {
+ // clear the contents of the buffer
+ buffer.str("");
+
+ std::string str = R"(
+
+
+
+
+
+
+
+
+
+ 1
+ 100
+ 0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315
+
+
+
+ )";
+
+ sdf::URDF2SDF parser;
+ tinyxml2::XMLDocument sdfResult;
+ sdf::ParserConfig config;
+ parser.InitModelString(str, config, &sdfResult);
+
+ EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+ " tag reference[link1] does not exist"
+ " in the URDF model. Please ensure that the reference attribute"
+ " matches the name of a link.");
+ }
+
+ {
+ // clear the contents of the buffer
+ buffer.str("");
+
+ std::string str = R"(
+
+
+
+
+
+
+
+
+
+ 1 1 1
+
+
+
+
+
+
+
+
+
+
+ )";
+
+ sdf::URDF2SDF parser;
+ tinyxml2::XMLDocument sdfResult;
+ sdf::ParserConfig config;
+ parser.InitModelString(str, config, &sdfResult);
+
+ EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+ " tag reference[link1] does not exist"
+ " in the URDF model. Please ensure that the reference attribute"
+ " matches the name of a link.");
+ }
+
+ {
+ // clear the contents of the buffer
+ buffer.str("");
+
+ std::string str = R"(
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+ 100
+ 0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315
+
+
+
+ )";
+
+ sdf::URDF2SDF parser;
+ tinyxml2::XMLDocument sdfResult;
+ sdf::ParserConfig config;
+ parser.InitModelString(str, config, &sdfResult);
+
+ EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+ " tag reference[link1] does not exist"
+ " in the URDF model. Please ensure that the reference attribute"
+ " matches the name of a link.");
+ }
+
+ {
+ // clear the contents of the buffer
+ buffer.str("");
+
+ std::string str = R"(
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+ 100
+ 0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315
+
+
+
+ )";
+
+ sdf::URDF2SDF parser;
+ tinyxml2::XMLDocument sdfResult;
+ sdf::ParserConfig config;
+ parser.InitModelString(str, config, &sdfResult);
+
+ EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+ " tag reference[joint1] does not exist"
+ " in the URDF model. Please ensure that the reference attribute"
+ " matches the name of a joint.");
+ }
+}
+
/////////////////////////////////////////////////
/// Main
int main(int argc, char **argv)