From 58b71c9bc766d54635e073944a7940471bae05ca Mon Sep 17 00:00:00 2001
From: Aditya Agrawal <agraw17107@gapps.uwcsea.edu.sg>
Date: Fri, 16 Aug 2024 19:50:36 +0800
Subject: [PATCH] added other SDFExtensions

---
 src/parser_urdf.cc      | 25 +++++++++++++
 src/parser_urdf_TEST.cc | 79 ++++++++++++++++++++++++++++++++++++-----
 2 files changed, 96 insertions(+), 8 deletions(-)

diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc
index 2d5b938b9..99fb8159e 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 link_found = 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)
     {
+      link_found = 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 (!link_found) {
+    sdfwarn << "<collision> tag with 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 link_found = 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)
     {
+      link_found=true;
       // std::cerr << "============================\n";
       // std::cerr << "working on g_extensions for link ["
       //           << sdfIt->first << "]\n";
@@ -2102,6 +2112,12 @@ void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem,
       }
     }
   }
+  // If we didn't find the link, emit a warning
+  if (!link_found) {
+    sdfwarn << "<visual> tag with reference[" << _linkName << "] does not exist"
+            << " in the URDF model. Please ensure that the reference attribute"
+            << " matches the name of a link.";
+  }
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -2168,6 +2184,7 @@ void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
 void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem,
                              const std::string &_jointName)
 {
+  bool joint_found = false;
   auto* doc = _elem->GetDocument();
   for (StringSDFExtensionPtrMap::iterator
       sdfIt = g_extensions.begin();
@@ -2175,6 +2192,7 @@ void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem,
   {
     if (sdfIt->first == _jointName)
     {
+      joint_found = true;
       for (std::vector<SDFExtensionPtr>::iterator
           ge = sdfIt->second.begin();
           ge != sdfIt->second.end(); ++ge)
@@ -2309,6 +2327,13 @@ void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem,
       }
     }
   }
+
+  // If we didn't find the link, emit a warning
+  if (!joint_found) {
+    sdfwarn << "<joint> tag with name[" << _jointName << "] does not exist"
+            << " in the URDF model. Please ensure that the name attribute"
+            << " matches the name of a joint.";
+  }
 }
 
 ////////////////////////////////////////////////////////////////////////////////
diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc
index 648ac3ec9..3360466c7 100644
--- a/src/parser_urdf_TEST.cc
+++ b/src/parser_urdf_TEST.cc
@@ -2476,8 +2476,8 @@ TEST(URDFParser, ParseGazeboRefDoesntExistWarningMessage)
       });
 #endif
 
-	// test if reference to link exists
-	{
+  // test if reference to link exists
+  {
     // clear the contents of the buffer
     buffer.str("");
 
@@ -2509,12 +2509,75 @@ TEST(URDFParser, ParseGazeboRefDoesntExistWarningMessage)
         " 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 */
+
+  {
+    // 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>
+        <visual reference="lіnk1">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <color rgba="0.8 0.1 0.1 1.0"/>
+          </material>
+          <origin xyz="0 0 0.5" rpy="0 0 0"/>
+        </visual>
+      </robot>)";
+
+    sdf::URDF2SDF parser;
+    tinyxml2::XMLDocument sdfResult;
+    sdf::ParserConfig config;
+    parser.InitModelString(str, config, &sdfResult);
+
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+        "<visual> tag with 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"(
+      <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>
+        <collision reference="lіnk1">
+          <geometry>
+            <sphere>
+              <radius>0.5</radius>
+            </sphere>
+          </geometry>
+          <origin xyz="0 0 0.5" rpy="0 0 0"/>
+        </collision>
+      </robot>)";
+
+    sdf::URDF2SDF parser;
+    tinyxml2::XMLDocument sdfResult;
+    sdf::ParserConfig config;
+    parser.InitModelString(str, config, &sdfResult);
+
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+        "<collision> tag with reference[link1] does not exist"
+        " in the URDF model. Please ensure that the reference attribute"
+        " matches the name of a link.");
+  }
 }
 
 /////////////////////////////////////////////////