initial draft of warning message functionality

Author: aagrawal05

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 referenced attribute matches the name of a link, consider checking unicode "
+												<< "representation for reference attribute in case of invisible characters and homoglyphs";
+		}
 }
 
 ////////////////////////////////////////////////////////////////////////////////

src/parser_urdf_TEST.cc

@@ -2460,6 +2460,64 @@ 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 referenced attribute matches the name of a link, consider checking unicode "
+								"representation for reference attribute in case of invisible characters and homoglyphs");
+		}
+
+		// TODO: Similar tests for - 
+		// InsertSDFExtensionCollision, 
+		// InsertSDFExtensionRobot, 
+		// InsertSDFExtensionVisual, 
+		// InsertSDFExtensionJoint
+}
+
 /////////////////////////////////////////////////
 /// Main
 int main(int argc, char **argv)