Added new library opendrive-map and updated orbit-georef

opendrive-map/DEVELOPMENT.md

@@ -0,0 +1,69 @@
+# opendrive-map — development notes & future improvements
+
+Scope: `opendrive-map` is a **read-only road-network + lane-geometry** library for a 2D
+map viewer (COSMO `trajectory-explorer`) and a 2D traffic-metrics tool (`data-metrics`).
+Keep it focused on roads/lanes/parking + the map offset. Add other categories only when a
+consumer actually needs them — resist becoming an everything-parser.
+
+## Done (2026-06-29)
+- Parser robustness: negative polynomial `sOffset` clamped to 0; non-finite (NaN/inf)
+  width/laneOffset records dropped with a warning; records sorted by `sOffset`.
+- Breakpoint-aware sampling: `sample_reference_line(road, interval, breakpoints=...)` forces
+  samples at lane-section / width / laneOffset transitions so polygon edges align with
+  width changes (not just the interval grid).
+
+---
+
+## Q1 — Other ORBIT-exported elements (potential future parsing)
+
+ORBIT can write these into a `.xodr` (most are absent from current drone maps, e.g.
+GbgSaroRound has no objects/signals at all). Add on demand only.
+
+| Element | OpenDRIVE form | Who'd use it | Recommendation |
+|---|---|---|---|
+| Buildings, land use (forest/farmland/meadow/water), trees, bushes, lampposts, guardrails | `<object>` with point/polyline/polygon `<outline>` (`cornerLocal` + `s/t/hdg`) | **Viewer only** (context layers); irrelevant to metrics | Add a generic `RoadObject(type, subtype, placed_polygon)` parser **iff the viewer wants context layers**. Cheap — reuse the parking placement path (`_parking_polygon` / `_road_point_at_s`). |
+| Signals: stop, give-way, speed-limit, traffic-lights, priority | `<signal>` with `type/subtype`, `s/t`, `dynamic`, `<validity>` lanes | **Metrics** (intersection control, speed-limit context) + viewer overlay | Highest-value non-road addition. Add `Signal(type, subtype, s, t, dynamic, validity_lanes)` **when a concrete metric consumes it** (YAGNI until then). |
+| Road marks (`<roadMark>`, dashed/solid + `<line>`) | per-lane boundary | Viewer cosmetic only | Skip unless rendering lane markings. |
+| Road `type` (town/motorway/…), lane `type` | road/lane attrs | lane type already used as a filter | Road type: add only if a metric needs road-class defaults. |
+| Elevation / superelevation profiles | `<elevationProfile>`, `<lateralProfile>` | 3D only | Skip (consumers are 2D). See Q2 §3. |
+| Junction lane-links (predecessor/successor, connection lane maps) | `<junction>`, lane `<link>` | routing / turn analysis | Basic `Junction`(id, connections) already parsed; add lane-level links only for routing. |
+
+Note: buildings & land use have **rich semantics only in ORBIT's OSM export**; in OpenDRIVE
+they are generic geometry objects. If a consumer needs semantic building/landuse data, read
+the OSM export, not the XODR.
+
+---
+
+## Q2 — Ideas from other projects (analyzed, not yet adopted)
+
+### Worth doing later
+1. **Spiral via Fresnel integrals** (`src/Geometries/Spiral/odrSpiral.cpp`). Our `geometry.py`
+   `_sample_spiral` uses forward-Euler integration; measured error vs exact `scipy.special.fresnel`
+   is ~2–5 cm on typical clothoids (R=30–50 m). **Low urgency** — current ORBIT maps use only
+   line + paramPoly3 (no spirals). When needed: either `scipy.special.fresnel` (exact, adds a
+   scipy dep) or a dependency-free RK2/midpoint integrator (~100× lower error than Euler, free).
+2. **Multi-profile s-sampling** — partially done (breakpoints now injected). libOpenDRIVE also
+   merges superelevation breakpoints; relevant only if we add 3D.
+3. **More parser repair** (`check_and_repair` pattern in `OpenDriveMap.cpp`): enforce lane-0
+   width = 0 (we skip center lanes, so N/A), `fromLane <= toLane` in signal validity (when we
+   parse signals), reasonable-coefficient checks. Add as we parse more elements.
+
+### Skip for 2D consumers (revisit only if scope changes)
+- **3D**: elevation / superelevation / crossfall / lane-height surfaces (`Road::get_xyz`,
+  `get_surface_pt`). Big effort; our consumers are 2D.
+- **Symbolic border polynomials** (`CubicSpline`/`CubicProfile::add`): compose width polys into
+  one border poly. Only pays off with adaptive sampling or 3D.
+- **Adaptive Bézier sampling** (`CubicBezier::approximate_linear`, eps-based): a perf/quality
+  optimization; our fixed interval + breakpoints is sufficient at current map sizes.
+- **Arc-length LUT** for paramPoly3 (`CubicBezier` ctor): only needed for true distance-based
+  sampling.
+- **Road marks**, **lane predecessor/successor links**: on-demand (see Q1).
+
+### Verified NON-issues (no action)
+- **`pRange` default**: a review flagged this as a bug — it is not. Our code already defaults to
+  `arcLength`; libOpenDRIVE/esmini default to `normalized`. It is moot for us because ORBIT writes
+  `pRange="normalized"` explicitly on every paramPoly3 (and we handle that correctly). If aligning
+  to the ecosystem default ever matters, confirm against the ASAM XSD before flipping it.
+- **line / arc / poly3 geometry math**: confirmed equivalent to libOpenDRIVE's.
+- **Lane assignment**: our shapely `STRtree` + `polygon.covers` handles arbitrary lane shapes;
+  no need for libOpenDRIVE's analytic `t -> lane_id` border lookup.

opendrive-map/README.md

@@ -0,0 +1,86 @@
+# opendrive-map
+
+Read-only OpenDRIVE road-network / lane-geometry model shared across the ORBIT
+drone toolchain (COSMO `trajectory-explorer`, `data-metrics`, and future tools).
+
+It is the single source of truth for interpreting an ORBIT-exported `.xodr`:
+reference-line geometry (line, arc, spiral/clothoid, poly3, paramPoly3), full
+lane-width polynomials, multiple lane sections, `<laneOffset>`, lane-type
+filtering, lane polygons + centerlines + lengths, and spatial lane assignment.
+
+## Coordinate frame
+
+All geometry is returned in the **local map frame** (the XODR `<offset>` is *not*
+applied). `RoadNetwork.offset` exposes the authoritative header `<offset>` —
+`absolute = local + offset`, in the `geo_reference` CRS. Use `to_global()` /
+`to_local()` to convert. Do **not** re-derive the offset from the geoReference
+`+lat_0/+lon_0` params; for UTM those are redundant and may be absent.
+
+## Usage
+
+```python
+from opendrive_map import RoadNetwork
+
+net = RoadNetwork.from_file("map.xodr", lane_types=["driving"])
+lane = net.assign_lane(x_local, y_local)   # object position in LOCAL frame
+if lane:
+    print(lane.road_id, lane.lane_id, lane.length_m, lane.width_at(0.0))
+```
+
+## API
+
+Public surface (everything in `opendrive_map.__all__`).
+
+### `RoadNetwork`
+
+The entry point. Build it, then query.
+
+- `RoadNetwork.from_file(path, *, lane_types=None, interval=DEFAULT_INTERVAL)`
+- `RoadNetwork.from_text(text, *, lane_types=None, interval=DEFAULT_INTERVAL)`
+
+`lane_types=None` keeps all lane types; pass e.g. `["driving"]` to filter.
+`interval` is the reference-line sampling step in metres.
+
+| Attribute | Description |
+|---|---|
+| `roads` | parsed `Road` models (local frame, no lanes-built logic) |
+| `lanes` | built `Lane` list (centerlines + polygons) |
+| `junctions` | `Junction` list (id + connections) |
+| `parking` | `ParkingObject` list (raw `<object type="parking">` records) |
+| `parking_polygons` | placed shapely `Polygon`s for parking, local frame |
+| `offset` | authoritative header `<offset>` `(x, y, z)`; `absolute = local + offset` |
+| `geo_reference` | raw `<geoReference>` PROJ string, or `None` |
+| `tree` | cached shapely `STRtree` over lane polygons (built on first use) |
+
+| Method | Description |
+|---|---|
+| `assign_lane(x, y)` | lane whose polygon covers the LOCAL point, or `None` |
+| `to_global(x, y)` | local → projected CRS coords (adds `offset`) |
+| `to_local(x, y)` | projected CRS coords → local (subtracts `offset`) |
+
+### `Lane`
+
+Built lane geometry. Fields: `road_id`, `lane_id`, `type`, `section_s`,
+`centerline` (Nx2 ndarray, local frame), `polygon` (shapely), `length_m`,
+and `width_at(s_rel)` (width at distance `s_rel` into the lane section).
+
+### Top-level functions
+
+- `read_offset(path)` — read the header `<offset>` `(x, y, z)` cheaply, without
+  building the network (used to localise object coordinates).
+- `sample_reference_line(road, interval, breakpoints=())` — sample a road's
+  reference line as `(s, x, y, hdg)` tuples; `breakpoints` forces extra samples
+  at given `s` values (e.g. width/section transitions).
+
+### Parking
+
+`<object type="parking">` outlines are parsed into `ParkingObject` records and
+placed into `parking_polygons` (local frame) using each object's road reference
+point, `s`/`t`/`hdg`, and `<outline>` corners.
+
+### Model dataclasses
+
+Also exported for typing/inspection: `Road`, `RawLane`, `LaneSection`,
+`GeomSegment`, `Poly`, `Junction`, `Connection`, `ParkingObject`. These are the
+parsed model types behind `RoadNetwork`; most consumers only need the surface
+above.

opendrive-map/pyproject.toml

@@ -0,0 +1,18 @@
+[build-system]
+requires = ["hatchling>=1.24"]
+build-backend = "hatchling.build"
+
+[project]
+name = "opendrive-map"
+version = "0.1.0"
+description = "Read-only OpenDRIVE road-network / lane-geometry model shared across the ORBIT drone toolchain (COSMO, data-metrics)."
+readme = "README.md"
+requires-python = ">=3.9"
+license = { text = "MIT" }
+dependencies = ["numpy>=1.20.0", "shapely>=2.0", "pyproj>=3.0.0"]
+
+[project.optional-dependencies]
+dev = ["pytest"]
+
+[tool.hatch.build.targets.wheel]
+packages = ["src/opendrive_map"]

opendrive-map/src/opendrive_map/__init__.py

@@ -0,0 +1,32 @@
+"""opendrive-map: read-only OpenDRIVE road-network / lane-geometry model."""
+
+from .geometry import sample_reference_line
+from .model import (
+    Connection,
+    GeomSegment,
+    Junction,
+    Lane,
+    LaneSection,
+    ParkingObject,
+    Poly,
+    RawLane,
+    Road,
+)
+from .network import RoadNetwork, read_offset
+
+__all__ = [
+    "RoadNetwork",
+    "read_offset",
+    "sample_reference_line",
+    "Road",
+    "Lane",
+    "RawLane",
+    "LaneSection",
+    "GeomSegment",
+    "Poly",
+    "Junction",
+    "Connection",
+    "ParkingObject",
+]
+
+__version__ = "0.1.0"

opendrive-map/src/opendrive_map/geometry.py

@@ -0,0 +1,146 @@
+"""Reference-line sampling for all OpenDRIVE planView primitives.
+
+Geometry math ported from ORBIT's opendrive_geometry.py (the complete
+implementation: line, arc, spiral/clothoid, poly3, paramPoly3), extended to
+return per-sample heading so lanes can be offset perpendicular to the road.
+"""
+
+from __future__ import annotations
+
+import math
+from typing import List, Tuple
+
+from .model import GeomSegment, Road
+
+Sample = Tuple[float, float, float, float]  # (s_road, x, y, hdg) in local map frame
+
+
+def _to_global(seg: GeomSegment, lx: float, ly: float, lhdg: float) -> Tuple[float, float, float]:
+    ch, sh = math.cos(seg.hdg), math.sin(seg.hdg)
+    return (
+        seg.x + lx * ch - ly * sh,
+        seg.y + lx * sh + ly * ch,
+        seg.hdg + lhdg,
+    )
+
+
+def _sample_positions(length: float, interval: float, extra=()) -> List[float]:
+    n = max(2, int(math.ceil(length / max(interval, 1e-6))) + 1)
+    pts = {length * i / (n - 1) for i in range(n)}
+    pts.update(e for e in extra if 0.0 <= e <= length)
+    return sorted(pts)
+
+
+def sample_segment(seg: GeomSegment, interval: float, extra_local=()) -> List[Sample]:
+    """Sample one geometry primitive; returns [(s_local, x, y, hdg), ...].
+
+    extra_local forces sample points at the given local s-offsets (e.g. lane width /
+    laneOffset polynomial breakpoints) in addition to the regular interval grid.
+    """
+    L = seg.length
+    if L <= 0:
+        return [(0.0, seg.x, seg.y, seg.hdg)]
+
+    kind = seg.kind
+    p = seg.params
+    ss = _sample_positions(L, interval, extra_local)
+    out: List[Sample] = []
+
+    if kind == "arc" and abs(float(p.get("curvature", 0.0))) >= 1e-12:
+        k = float(p["curvature"])
+        r = 1.0 / k
+        ch, sh = math.cos(seg.hdg), math.sin(seg.hdg)
+        cx, cy = seg.x - r * sh, seg.y + r * ch
+        for s in ss:
+            hd = seg.hdg + s * k
+            out.append((s, cx + r * math.sin(hd), cy - r * math.cos(hd), hd))
+
+    elif kind == "spiral" and (
+        abs(float(p.get("curvStart", 0.0))) >= 1e-12 or abs(float(p.get("curvEnd", 0.0))) >= 1e-12
+    ):
+        out = _sample_spiral(seg, ss)
+
+    elif kind == "poly3":
+        a, b, c, d = (float(p.get(k_, 0.0)) for k_ in ("a", "b", "c", "d"))
+        for v in ss:
+            u = a + b * v + c * v ** 2 + d * v ** 3
+            du = b + 2 * c * v + 3 * d * v ** 2
+            gx, gy, gh = _to_global(seg, v, u, math.atan2(du, 1.0))
+            out.append((v, gx, gy, gh))
+
+    elif kind == "paramPoly3":
+        out = _sample_param_poly3(seg, ss)
+
+    else:  # line (and degenerate arc/spiral)
+        ch, sh = math.cos(seg.hdg), math.sin(seg.hdg)
+        for s in ss:
+            out.append((s, seg.x + s * ch, seg.y + s * sh, seg.hdg))
+
+    return out
+
+
+def _sample_spiral(seg: GeomSegment, ss: List[float]) -> List[Sample]:
+    """Clothoid via forward Euler integration in the local frame."""
+    L = seg.length
+    k0 = float(seg.params.get("curvStart", 0.0))
+    k1 = float(seg.params.get("curvEnd", 0.0))
+    kd = (k1 - k0) / L
+    steps = max(len(ss) * 4, int(L / 0.05) + 1)
+    ds = L / steps
+
+    out: List[Sample] = []
+    lx = ly = lhdg = 0.0
+    ti = 0
+    for i in range(steps + 1):
+        s_here = i * ds
+        while ti < len(ss) and ss[ti] <= s_here + 1e-9:
+            gx, gy, gh = _to_global(seg, lx, ly, lhdg)
+            out.append((ss[ti], gx, gy, gh))
+            ti += 1
+        kappa = k0 + kd * s_here
+        lx += math.cos(lhdg) * ds
+        ly += math.sin(lhdg) * ds
+        lhdg += kappa * ds
+    while ti < len(ss):
+        gx, gy, gh = _to_global(seg, lx, ly, lhdg)
+        out.append((ss[ti], gx, gy, gh))
+        ti += 1
+    return out
+
+
+def _sample_param_poly3(seg: GeomSegment, ss: List[float]) -> List[Sample]:
+    p = seg.params
+    aU, bU, cU, dU = (float(p.get(k_, 0.0)) for k_ in ("aU", "bU", "cU", "dU"))
+    aV, bV, cV, dV = (float(p.get(k_, 0.0)) for k_ in ("aV", "bV", "cV", "dV"))
+    normalized = p.get("pRange", "arcLength") == "normalized"
+    L = seg.length
+
+    out: List[Sample] = []
+    for s in ss:
+        pv = (s / L) if normalized else s
+        u = aU + bU * pv + cU * pv ** 2 + dU * pv ** 3
+        v = aV + bV * pv + cV * pv ** 2 + dV * pv ** 3
+        du = bU + 2 * cU * pv + 3 * dU * pv ** 2
+        dv = bV + 2 * cV * pv + 3 * dV * pv ** 2
+        lhdg = math.atan2(dv, du) if (du ** 2 + dv ** 2) > 1e-12 else 0.0
+        gx, gy, gh = _to_global(seg, u, v, lhdg)
+        out.append((s, gx, gy, gh))
+    return out
+
+
+def sample_reference_line(road: Road, interval: float, breakpoints=()) -> List[Sample]:
+    """Sample the whole road reference line; returns [(s_road, x, y, hdg), ...].
+
+    breakpoints are road-frame s-values that must appear as samples (e.g. lane width /
+    laneOffset transitions), so lane polygon edges align with those transitions.
+    """
+    bps = sorted(set(breakpoints))
+    samples: List[Sample] = []
+    for seg in road.geom_segments:
+        seg_extra = [b - seg.s for b in bps if seg.s - 1e-9 <= b <= seg.s + seg.length + 1e-9]
+        for s_local, x, y, hdg in sample_segment(seg, interval, seg_extra):
+            s_road = seg.s + s_local
+            if samples and abs(samples[-1][1] - x) < 1e-6 and abs(samples[-1][2] - y) < 1e-6:
+                continue  # drop duplicate point at segment boundary
+            samples.append((s_road, x, y, hdg))
+    return samples