test(sim): pure parse helper and unit tests for spherical coords

simulation/simulation_resources/patches/extract_spherical_coords.py

@@ -3,36 +3,46 @@
 import sys
 import xml.etree.ElementTree as ET
 
+
+def parse_spherical_coordinates(sdf_file):
+    """Parse spherical_coordinates from an SDF world file.
+
+    Returns (latitude, longitude, elevation) as strings from the file
+    (defaults "0" when a child element is missing).
+    Raises ValueError if spherical_coordinates is absent or the file is unreadable.
+    """
+    try:
+        tree = ET.parse(sdf_file)
+    except Exception as e:
+        raise ValueError(f"Error parsing SDF file: {e}") from e
+    root = tree.getroot()
+    spherical_coords = root.find(".//spherical_coordinates")
+    if spherical_coords is None:
+        raise ValueError("spherical_coordinates element not found")
+    latitude = spherical_coords.findtext("latitude_deg", "0")
+    longitude = spherical_coords.findtext("longitude_deg", "0")
+    elevation = spherical_coords.findtext("elevation", "0")
+    return latitude, longitude, elevation
+
+
 def extract_spherical_coordinates(sdf_file):
     # Extract spherical coordinates from an SDF world file.
     # Returns: lat,lon,elev,0
     try:
-        tree = ET.parse(sdf_file)
-        root = tree.getroot()
-
-        # Find the spherical_coordinates element
-        spherical_coords = root.find('.//spherical_coordinates')
-
-        if spherical_coords is None:
+        latitude, longitude, elevation = parse_spherical_coordinates(sdf_file)
+        print(f"{latitude},{longitude},{elevation},0")
+    except ValueError as e:
+        if "not found" in str(e):
             print("0,0,0,0", file=sys.stderr)
             sys.exit(1)
-
-        # Extract values
-        latitude = spherical_coords.findtext('latitude_deg', '0')
-        longitude = spherical_coords.findtext('longitude_deg', '0')
-        elevation = spherical_coords.findtext('elevation', '0')
-
-        # Format as: lat,lon,elev,0
-        print(f"{latitude},{longitude},{elevation},0")
-
-    except Exception as e:
         print(f"Error parsing SDF file: {e}", file=sys.stderr)
         print("0,0,0,0")
         sys.exit(1)
 
+
 if __name__ == "__main__":
     if len(sys.argv) != 2:
         print("Usage: extract_spherical_coords.py <sdf_file>", file=sys.stderr)
         sys.exit(1)
-    
+
     extract_spherical_coordinates(sys.argv[1])

simulation/simulation_resources/patches/test_extract_spherical_coords.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+"""Offline unit tests for extract_spherical_coords (no Gazebo)."""
+import tempfile
+import unittest
+from pathlib import Path
+
+from extract_spherical_coords import parse_spherical_coordinates
+
+
+class TestParseSpherical(unittest.TestCase):
+    def _write(self, body: str) -> str:
+        f = tempfile.NamedTemporaryFile("w", suffix=".sdf", delete=False)
+        f.write(body)
+        f.close()
+        return f.name
+
+    def test_full_coords(self):
+        path = self._write(
+            """<?xml version="1.0"?>
+            <sdf version="1.6"><world name="w">
+              <spherical_coordinates>
+                <latitude_deg>47.4</latitude_deg>
+                <longitude_deg>8.5</longitude_deg>
+                <elevation>412</elevation>
+              </spherical_coordinates>
+            </world></sdf>"""
+        )
+        lat, lon, elev = parse_spherical_coordinates(path)
+        self.assertEqual(lat, "47.4")
+        self.assertEqual(lon, "8.5")
+        self.assertEqual(elev, "412")
+        Path(path).unlink(missing_ok=True)
+
+    def test_missing_children_default_zero(self):
+        path = self._write(
+            """<?xml version="1.0"?>
+            <sdf version="1.6"><world name="w">
+              <spherical_coordinates/>
+            </world></sdf>"""
+        )
+        self.assertEqual(parse_spherical_coordinates(path), ("0", "0", "0"))
+        Path(path).unlink(missing_ok=True)
+
+    def test_missing_element_raises(self):
+        path = self._write(
+            """<?xml version="1.0"?>
+            <sdf version="1.6"><world name="w"></world></sdf>"""
+        )
+        with self.assertRaises(ValueError):
+            parse_spherical_coordinates(path)
+        Path(path).unlink(missing_ok=True)
+
+    def test_bad_file_raises(self):
+        with self.assertRaises(ValueError):
+            parse_spherical_coordinates("/no/such/file.sdf")
+
+
+if __name__ == "__main__":
+    unittest.main()