Improve URDF geom name loading API.

PiperOrigin-RevId: 505112774
Change-Id: I866e58e664089256a41908dddab9ad79745fb1da

src/xml/xml_urdf.cc

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <cstring>
+#include <iostream>
 #include <string>
 #include <vector>
 
@@ -64,6 +65,7 @@ void mjXURDF::Clear(void) {
   urChildren.clear();
   urMat.clear();
   urRGBA.clear();
+  urGeomNames.clear();
 }
 
 
@@ -245,6 +247,8 @@ void mjXURDF::Body(XMLElement* body_elem) {
 
   // process all visual and geometry elements in order
   float rgba[4] = {-1, 0, 0, 0};
+  std::string geom_name;
+
   elem = body_elem->FirstChildElement();
   while (elem) {
     name = elem->Value();
@@ -277,9 +281,15 @@ void mjXURDF::Body(XMLElement* body_elem) {
           std::memcpy(pgeom->rgba, rgba, 4*sizeof(float));
         }
 
-        // save name
-        if (model->geomnamesfromurdf) {
-          mjXUtil::ReadAttrTxt(elem, "name", pgeom->name);
+        // save name if it doesn't already exist.
+        mjXUtil::ReadAttrTxt(elem, "name", geom_name);
+        if (urGeomNames.find(geom_name) == urGeomNames.end()) {
+          pgeom->name = geom_name;
+          urGeomNames.insert(geom_name);
+        } else {
+          std::cerr << "WARNING: Geom with duplicate name '" << geom_name
+                    << "' encountered in URDF, creating an unnamed geom."
+                    << std::endl;
         }
       }
     }
@@ -293,9 +303,15 @@ void mjXURDF::Body(XMLElement* body_elem) {
         std::memcpy(pgeom->rgba, rgba, 4*sizeof(float));
       }
 
-      // save name
-      if (model->geomnamesfromurdf) {
-        mjXUtil::ReadAttrTxt(elem, "name", pgeom->name);
+      // save name if it doesn't already exist.
+      mjXUtil::ReadAttrTxt(elem, "name", geom_name);
+      if (urGeomNames.find(geom_name) == urGeomNames.end()) {
+        pgeom->name = geom_name;
+        urGeomNames.insert(geom_name);
+      } else {
+        std::cerr << "WARNING: Geom with duplicate name '" << geom_name
+                  << "' encountered in URDF, creating an unnamed geom."
+                  << std::endl;
       }
     }