Handle inconsistent OpenDRIVE planView lengths (#467)

* Handle inconsistent OpenDRIVE planView lengths

* add docstring for makeCurve helper

* Add OpenDRIVE parser regression tests

Author: lola831

src/scenic/formats/opendrive/xodr_parser.py

@@ -212,6 +212,52 @@ def point_at(self, s):
         return self.rel_to_abs((s, 0, s))
 
 
+def makeCurve(x0, y0, hdg, length, curve_elem):
+    """Create a reference-line curve from an OpenDRIVE geometry element.
+
+    The given length is the effective length Scenic should use, which may
+    differ from the length declared in the map if the planView is inconsistent.
+    """
+    if curve_elem.tag == "line":
+        return Line(x0, y0, hdg, length)
+    elif curve_elem.tag == "arc":
+        # Arc is clothoid of constant curvature.
+        curv = float(curve_elem.get("curvature"))
+        return Clothoid(x0, y0, hdg, length, curv, curv)
+    elif curve_elem.tag == "spiral":
+        curv0 = float(curve_elem.get("curvStart"))
+        curv1 = float(curve_elem.get("curvEnd"))
+        return Clothoid(x0, y0, hdg, length, curv0, curv1)
+    elif curve_elem.tag == "poly3":
+        a, b, c, d = (
+            float(curve_elem.get("a")),
+            float(curve_elem.get("b")),
+            float(curve_elem.get("c")),
+            float(curve_elem.get("d")),
+        )
+        return Cubic(x0, y0, hdg, length, a, b, c, d)
+    elif curve_elem.tag == "paramPoly3":
+        au, bu, cu, du, av, bv, cv, dv = (
+            float(curve_elem.get("aU")),
+            float(curve_elem.get("bU")),
+            float(curve_elem.get("cU")),
+            float(curve_elem.get("dU")),
+            float(curve_elem.get("aV")),
+            float(curve_elem.get("bV")),
+            float(curve_elem.get("cV")),
+            float(curve_elem.get("dV")),
+        )
+        p_range = curve_elem.get("pRange")
+        if p_range and p_range != "normalized":
+            # TODO support arcLength
+            raise NotImplementedError("unsupported pRange for paramPoly3")
+        else:
+            p_range = 1
+        return ParamCubic(x0, y0, hdg, length, au, bu, cu, du, av, bv, cv, dv, p_range)
+    else:
+        raise NotImplementedError(f'unhandled OpenDRIVE geometry type "{curve_elem.tag}"')
+
+
 class Lane:
     def __init__(self, id_, type_, pred=None, succ=None):
         self.id_ = id_
@@ -1434,82 +1480,58 @@ def parse(self, path):
                 hdg = float(geom.get("hdg"))
                 length = float(geom.get("length"))
                 curve_elem = geom[0]
-                curve = None
-                if curve_elem.tag == "line":
-                    curve = Line(x0, y0, hdg, length)
-                elif curve_elem.tag == "arc":
-                    # Arc is clothoid of constant curvature.
-                    curv = float(curve_elem.get("curvature"))
-                    curve = Clothoid(x0, y0, hdg, length, curv, curv)
-                elif curve_elem.tag == "spiral":
-                    curv0 = float(curve_elem.get("curvStart"))
-                    curv1 = float(curve_elem.get("curvEnd"))
-                    curve = Clothoid(x0, y0, hdg, length, curv0, curv1)
-                elif curve_elem.tag == "poly3":
-                    a, b, c, d = (
-                        cubic_elem.get("a"),
-                        float(curve_elem.get("b")),
-                        float(curve_elem.get("c")),
-                        float(curve_elem.get("d")),
-                    )
-                    curve = Cubic(x0, y0, hdg, length, a, b, c, d)
-                elif curve_elem.tag == "paramPoly3":
-                    au, bu, cu, du, av, bv, cv, dv = (
-                        float(curve_elem.get("aU")),
-                        float(curve_elem.get("bU")),
-                        float(curve_elem.get("cU")),
-                        float(curve_elem.get("dU")),
-                        float(curve_elem.get("aV")),
-                        float(curve_elem.get("bV")),
-                        float(curve_elem.get("cV")),
-                        float(curve_elem.get("dV")),
-                    )
-                    p_range = curve_elem.get("pRange")
-                    if p_range and p_range != "normalized":
-                        # TODO support arcLength
-                        raise NotImplementedError("unsupported pRange for paramPoly3")
-                    else:
-                        p_range = 1
-                    curve = ParamCubic(
-                        x0, y0, hdg, length, au, bu, cu, du, av, bv, cv, dv, p_range
-                    )
-                curves.append((s0, curve))
+                curves.append((s0, x0, y0, hdg, length, curve_elem))
+
             if not curves:
                 raise ValueError(f"road {road.id_} has an empty planView")
             if not curves[0][0] == 0:
                 raise ValueError(
                     f"reference line of road {road.id_} does not start at s=0"
                 )
-            lastS = 0
-            lastCurve = curves[0][1]
+
+            lastS = curves[0][0]
+            lastCurve = curves[0]
             refLine = []
-            for s0, curve in curves[1:]:
+            for curve in curves[1:]:
+                s0 = curve[0]
                 l = s0 - lastS
-                if abs(lastCurve.length - l) > 1e-4:
-                    raise ValueError(
-                        f"planView of road {road.id_} has inconsistent length"
-                    )
+
                 if l < 0:
                     raise ValueError(f"planView of road {road.id_} is not in order")
-                elif l < 1e-6:
+
+                lastS0, x0, y0, hdg, length, curve_elem = lastCurve
+                if abs(length - l) > 1e-3:
+                    warn(
+                        f"planView of road {road.id_} has inconsistent length: "
+                        f"geometry at s={lastS0} has declared length {length}, "
+                        f"but the next geometry starts at s={s0}; "
+                        f"using length {l}"
+                    )
+                    length = l
+
+                if l < 1e-6:
                     warn(
                         f"road {road.id_} reference line has a geometry of "
                         f"length {l}; skipping it"
                     )
                 else:
-                    refLine.append(lastCurve)
+                    refLine.append(makeCurve(x0, y0, hdg, length, curve_elem))
+
                 lastS = s0
                 lastCurve = curve
-            if refLine and lastCurve.length < 1e-6:
+
+            lastS0, x0, y0, hdg, length, curve_elem = lastCurve
+            if refLine and length < 1e-6:
                 warn(
                     f"road {road.id_} reference line has a geometry of "
-                    f"length {lastCurve.length}; skipping it"
+                    f"length {length}; skipping it"
                 )
             else:
                 # even if the last curve is shorter than the threshold, we'll keep it if
                 # it is the only curve; getting rid of the road entirely is handled by
                 # road elision above
-                refLine.append(lastCurve)
+                refLine.append(makeCurve(x0, y0, hdg, length, curve_elem))
+
             assert refLine
             road.ref_line = refLine
 

tests/formats/opendrive/test_opendrive.py

@@ -1,12 +1,20 @@
 import glob
 import os
 from pathlib import Path
+import xml.etree.ElementTree as ET
 
 import matplotlib.pyplot as plt
 import pytest
 
 from scenic.core.geometry import TriangulationError
 from scenic.formats.opendrive import OpenDriveWorkspace
+from scenic.formats.opendrive.xodr_parser import (
+    Cubic,
+    OpenDriveWarning,
+    ParamCubic,
+    RoadMap,
+    makeCurve,
+)
 
 oldDir = os.getcwd()
 os.chdir(Path("tests") / "formats" / "opendrive")
@@ -30,3 +38,141 @@ def test_map(path, runLocally, pytestconfig):
             odw.show(plt)
             plt.show(block=False)
             plt.close()
+
+
+def write_xodr(tmp_path, plan_view):
+    path = tmp_path / "test.xodr"
+    path.write_text(
+        f"""<?xml version="1.0" encoding="UTF-8"?>
+<OpenDRIVE>
+  <road name="Road 7" length="20.0" id="7" junction="-1">
+    {plan_view}
+    <lanes>
+      <laneOffset s="0.0" a="0.0" b="0.0" c="0.0" d="0.0"/>
+      <laneSection s="0.0">
+        <center>
+          <lane id="0" type="none" level="false"/>
+        </center>
+        <right>
+          <lane id="-1" type="driving" level="false">
+            <width sOffset="0.0" a="3.5" b="0.0" c="0.0" d="0.0"/>
+          </lane>
+        </right>
+      </laneSection>
+    </lanes>
+  </road>
+</OpenDRIVE>
+"""
+    )
+    return path
+
+
+def test_inconsistent_planview_length_warns(tmp_path):
+    path = write_xodr(
+        tmp_path,
+        """<planView>
+      <geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="12.0">
+        <line/>
+      </geometry>
+      <geometry s="10.0" x="10.0" y="0.0" hdg="0.0" length="10.0">
+        <line/>
+      </geometry>
+    </planView>""",
+    )
+
+    road_map = RoadMap()
+
+    with pytest.warns(
+        OpenDriveWarning,
+        match="planView of road 7 has inconsistent length",
+    ):
+        road_map.parse(path)
+
+    road = road_map.roads[7]
+    assert len(road.ref_line) == 2
+    assert road.ref_line[0].length == pytest.approx(10.0)
+    assert road.ref_line[1].length == pytest.approx(10.0)
+
+
+def test_empty_planview_rejected(tmp_path):
+    path = write_xodr(tmp_path, "<planView/>")
+
+    road_map = RoadMap()
+
+    with pytest.raises(ValueError, match="road 7 has an empty planView"):
+        road_map.parse(path)
+
+
+def test_planview_must_start_at_zero(tmp_path):
+    path = write_xodr(
+        tmp_path,
+        """<planView>
+      <geometry s="1.0" x="0.0" y="0.0" hdg="0.0" length="10.0">
+        <line/>
+      </geometry>
+    </planView>""",
+    )
+
+    road_map = RoadMap()
+
+    with pytest.raises(
+        ValueError, match="reference line of road 7 does not start at s=0"
+    ):
+        road_map.parse(path)
+
+
+def test_planview_must_be_in_order(tmp_path):
+    path = write_xodr(
+        tmp_path,
+        """<planView>
+      <geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="10.0">
+        <line/>
+      </geometry>
+      <geometry s="-1.0" x="10.0" y="0.0" hdg="0.0" length="10.0">
+        <line/>
+      </geometry>
+    </planView>""",
+    )
+
+    road_map = RoadMap()
+
+    with pytest.raises(ValueError, match="planView of road 7 is not in order"):
+        road_map.parse(path)
+
+
+def test_make_curve_poly3():
+    curve_elem = ET.fromstring('<poly3 a="0.0" b="1.0" c="0.0" d="0.0"/>')
+
+    curve = makeCurve(0.0, 0.0, 0.0, 10.0, curve_elem)
+
+    assert isinstance(curve, Cubic)
+    assert curve.length == pytest.approx(10.0)
+
+
+def test_make_curve_param_poly3():
+    curve_elem = ET.fromstring(
+        '<paramPoly3 aU="0.0" bU="1.0" cU="0.0" dU="0.0" '
+        'aV="0.0" bV="0.0" cV="0.0" dV="0.0" pRange="normalized"/>'
+    )
+
+    curve = makeCurve(0.0, 0.0, 0.0, 10.0, curve_elem)
+
+    assert isinstance(curve, ParamCubic)
+    assert curve.length == pytest.approx(10.0)
+
+
+def test_make_curve_param_poly3_rejects_arc_length():
+    curve_elem = ET.fromstring(
+        '<paramPoly3 aU="0.0" bU="1.0" cU="0.0" dU="0.0" '
+        'aV="0.0" bV="0.0" cV="0.0" dV="0.0" pRange="arcLength"/>'
+    )
+
+    with pytest.raises(NotImplementedError, match="unsupported pRange for paramPoly3"):
+        makeCurve(0.0, 0.0, 0.0, 10.0, curve_elem)
+
+
+def test_make_curve_rejects_unknown_geometry_type():
+    curve_elem = ET.fromstring("<unknown/>")
+
+    with pytest.raises(NotImplementedError, match="unhandled OpenDRIVE geometry type"):
+        makeCurve(0.0, 0.0, 0.0, 10.0, curve_elem)