Draft: Parser warning message when <gazebo> reference does not exist in URDF

src/parser_urdf.cc

@@ -2108,12 +2108,14 @@ void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem,
 void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
                             const std::string &_linkName)
 {
+  bool link_found = false;
   for (StringSDFExtensionPtrMap::iterator
        sdfIt = g_extensions.begin();
        sdfIt != g_extensions.end(); ++sdfIt)
   {
     if (sdfIt->first == _linkName)
     {
+      link_found = true;
       sdfdbg << "inserting extension with reference ["
              << _linkName << "] into link.\n";
       for (std::vector<SDFExtensionPtr>::iterator ge =
@@ -2153,6 +2155,13 @@ void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
       }
     }
   }
+
+  // If we didn't find the link, emit a warning
+  if (!link_found) {
+    sdfwarn << "<gazebo> tag with reference[" << _linkName << "] does not exist"
+            << " in the URDF model. Please ensure that the reference attribute"
+            << " matches the name of a link.";
+  }
 }
 
 ////////////////////////////////////////////////////////////////////////////////

src/parser_urdf_TEST.cc

@@ -2460,6 +2460,63 @@ 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"(
+      <robot name="test_robot">
+        <link name="link1">
+          <inertial>
+            <mass value="1" />
+            <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
+          </inertial>
+        </link>
+        <gazebo reference="lіnk1">
+          <sensor name="link1_imu" type="imu">
+            <always_on>1</always_on>
+            <update_rate>100</update_rate>
+            <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+            <plugin name="sensor_plugin" filename="example_plugin.so" />
+          </sensor>
+        </gazebo>
+      </robot>)";
+
+    sdf::URDF2SDF parser;
+    tinyxml2::XMLDocument sdfResult;
+    sdf::ParserConfig config;
+    parser.InitModelString(str, config, &sdfResult);
+
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+        "<gazebo> tag with reference[link1] does not exist"
+        " in the URDF model. Please ensure that the reference attribute"
+        " matches the name of a link.");
+  }
+
+  /* TODO(aagrawal05): Similar tests for -
+      InsertSDFExtensionCollision,
+      InsertSDFExtensionRobot,
+      InsertSDFExtensionVisual,
+      InsertSDFExtensionJoint */
+}
+
 /////////////////////////////////////////////////
 /// Main
 int main(int argc, char **argv)