OpenDRIVE import: support <object>/<repeat> placement

packages/drawtonomy-sdk/__tests__/exporter/odrToShapes.test.ts

@@ -466,6 +466,53 @@ const PARKING_RECT = `<?xml version="1.0"?>
   </road>
 </OpenDRIVE>`
 
+// Two rows of parking spaces declared purely via <repeat> (no <outlines>),
+// mirroring esmini parking_demo objects 11/12: each object sits at t=0 but its
+// repeat places instances at tStart=tEnd=∓12.7 so the two rows straddle the road
+// centre by ±12.7 m. distance=2.5 over length=10 => 5 instances per row (curS
+// 0/2.5/5/7.5/10; the 2.4 m footprint stays within the 20 m road at each).
+const PARKING_REPEAT_ROWS = `<?xml version="1.0"?>
+<OpenDRIVE>
+  <header revMajor="1" revMinor="8"/>
+  <road name="r" length="20" id="1" junction="-1">
+    <planView><geometry s="0" x="0" y="0" hdg="0" length="20"><line/></geometry></planView>
+    <lanes>
+      <laneSection s="0">
+        <right><lane id="-1" type="driving" level="false"><width sOffset="0" a="3.5" b="0" c="0" d="0"/></lane></right>
+      </laneSection>
+    </lanes>
+    <objects>
+      <object type="parkingSpace" name="p1" id="11" s="0" t="0.0" hdg="0" length="2.4" width="4.9">
+        <repeat distance="2.5" tStart="-12.7" tEnd="-12.7" length="10" s="1.3"/>
+      </object>
+      <object type="parkingSpace" name="p2" id="12" s="0" t="0.0" hdg="0" length="2.4" width="4.9">
+        <repeat distance="2.5" tStart="12.7" tEnd="12.7" length="10" s="1.3"/>
+      </object>
+    </objects>
+  </road>
+</OpenDRIVE>`
+
+// A single continuous object (distance=0): the repeat declares a swept footprint
+// rather than discrete copies, so exactly one polygon is materialized at the span
+// start (t=3 here). Guards against distance=0 objects silently vanishing.
+const PARKING_REPEAT_CONTINUOUS = `<?xml version="1.0"?>
+<OpenDRIVE>
+  <header revMajor="1" revMinor="8"/>
+  <road name="r" length="100" id="1" junction="-1">
+    <planView><geometry s="0" x="0" y="0" hdg="0" length="100"><line/></geometry></planView>
+    <lanes>
+      <laneSection s="0">
+        <right><lane id="-1" type="driving" level="false"><width sOffset="0" a="3.5" b="0" c="0" d="0"/></lane></right>
+      </laneSection>
+    </lanes>
+    <objects>
+      <object type="parkingSpace" name="cont" id="20" s="0" t="0.0" hdg="0" length="2.4" width="5">
+        <repeat distance="0" tStart="3" tEnd="3" length="30" s="10"/>
+      </object>
+    </objects>
+  </road>
+</OpenDRIVE>`
+
 describe('odrToShapes parkingSpace objects', () => {
   it('converts a <cornerLocal> parking space into a polygon footprint with origin markers', () => {
     const result = odrToShapes(parseOpenDriveXml(PARKING_LOCAL))
@@ -513,14 +560,70 @@ describe('odrToShapes parkingSpace objects', () => {
   })
 })
 
+// Average (centroid) y of a polygon's vertices, in metres (canvas y -> ENU y is
+// negated; here we only compare relative sign/magnitude so canvas px is fine).
+function centroidY(points: { x: number; y: number }[]): number {
+  return points.reduce((s, p) => s + p.y, 0) / points.length
+}
+
+describe('odrToShapes parkingSpace <repeat>', () => {
+  it('expands two repeat rows into instances split to ±12.7 m about the road centre', () => {
+    const result = odrToShapes(parseOpenDriveXml(PARKING_REPEAT_ROWS))
+    expect(result.parkingSpaces).toBeDefined()
+    // 5 instances per row (curS 0/2.5/5/7.5/10), two rows => 10 polygons.
+    expect(result.parkingSpaces).toHaveLength(10)
+
+    const row11 = result.parkingSpaces!.filter(ps => ps.attributes.odr_object_id === '11')
+    const row12 = result.parkingSpaces!.filter(ps => ps.attributes.odr_object_id === '12')
+    expect(row11).toHaveLength(5)
+    expect(row12).toHaveLength(5)
+
+    // The two rows must NOT collapse onto the road centre: their footprints sit on
+    // opposite sides, ~25.4 m (2 x 12.7) apart. (Before <repeat> support both rows
+    // rendered at t=0 and overlapped exactly.)
+    const y11 = centroidY(row11.flatMap(ps => ps.points)) / PIXELS_PER_METER
+    const y12 = centroidY(row12.flatMap(ps => ps.points)) / PIXELS_PER_METER
+    // tStart=-12.7 (ENU) maps to +12.7 canvas y and vice versa; assert opposite
+    // signs and the correct magnitude either way.
+    expect(Math.sign(y11)).toBe(-Math.sign(y12))
+    expect(Math.abs(y11)).toBeCloseTo(12.7, 1)
+    expect(Math.abs(y12)).toBeCloseTo(12.7, 1)
+    expect(Math.abs(y11 - y12)).toBeCloseTo(25.4, 1)
+
+    // Instances of one row are spaced along the road by the repeat distance (2.5 m).
+    const xs11 = row11
+      .map(ps => ps.points.reduce((s, p) => s + p.x, 0) / ps.points.length / PIXELS_PER_METER)
+      .sort((a, b) => a - b)
+    for (let i = 1; i < xs11.length; i++) {
+      expect(xs11[i] - xs11[i - 1]).toBeCloseTo(2.5, 1)
+    }
+  })
+
+  it('materializes a distance=0 continuous repeat as a single footprint at the span start', () => {
+    const result = odrToShapes(parseOpenDriveXml(PARKING_REPEAT_CONTINUOUS))
+    expect(result.parkingSpaces).toHaveLength(1)
+    const ps = result.parkingSpaces![0]
+    expect(ps.attributes.odr_object_id).toBe('20')
+    // Placed at the repeat span start s=10, t=3 (ENU) -> canvas y = -3 m.
+    const cy = centroidY(ps.points) / PIXELS_PER_METER
+    expect(cy).toBeCloseTo(-3, 1)
+    const cx = ps.points.reduce((s, p) => s + p.x, 0) / ps.points.length / PIXELS_PER_METER
+    expect(cx).toBeCloseTo(10, 1)
+  })
+})
+
 describe('esmini parking_demo (fixture)', () => {
   const fixturePath = join(__dirname, '..', 'fixtures', 'parking_demo.xodr')
   it.skipIf(!existsSync(fixturePath))(
-    'materializes 7 parkingSpace polygons and keeps the existing crosswalks',
+    'expands parkingSpace <repeat> rows into many polygons and keeps the existing crosswalks',
     () => {
       const xml = readFileSync(fixturePath, 'utf-8')
       const result = odrToShapes(parseOpenDriveXml(xml))
-      expect(result.parkingSpaces).toHaveLength(7)
+      // Objects 4/6/8/11/12 carry a <repeat>; 5/7 are placed once. With repeat
+      // support each replicated object materializes many instances (7 parking
+      // objects -> far more than 7 polygons). Assert the expansion happened
+      // rather than pinning an exact count that would move with fixture tweaks.
+      expect(result.parkingSpaces!.length).toBeGreaterThan(7)
       // Crosswalk conversion is unchanged by parkingSpace support: the fixture
       // carries 3 <object type="crosswalk"> but one lacks length/width, so 2 are
       // materialized (the pre-existing materializeCrosswalks guard).
@@ -535,6 +638,28 @@ describe('esmini parking_demo (fixture)', () => {
           expect(Number.isFinite(p.y)).toBe(true)
         }
       }
+
+      // The regression this fixes: objects 11 and 12 on road 3 both sit at t=0
+      // but their <repeat> declares tStart=tEnd=-12.7 and +12.7 respectively, so
+      // the two rows must straddle the road centre 25.4 m (2 x 12.7) apart. Road 3
+      // is angled, so the offset splits across x and y — assert the Euclidean
+      // distance between the row centroids. Before <repeat> support both rows
+      // rendered at t=0 and their centroids coincided (distance 0).
+      const row11 = result.parkingSpaces!.filter(
+        ps => ps.attributes.odr_object_id === '11' && ps.attributes.odr_road_id === '3'
+      )
+      const row12 = result.parkingSpaces!.filter(
+        ps => ps.attributes.odr_object_id === '12' && ps.attributes.odr_road_id === '3'
+      )
+      expect(row11.length).toBeGreaterThan(1)
+      expect(row12.length).toBeGreaterThan(1)
+      const c = (pts: { x: number; y: number }[]) => ({
+        x: pts.reduce((s, p) => s + p.x, 0) / pts.length / PIXELS_PER_METER,
+        y: centroidY(pts) / PIXELS_PER_METER,
+      })
+      const c11 = c(row11.flatMap(ps => ps.points))
+      const c12 = c(row12.flatMap(ps => ps.points))
+      expect(Math.hypot(c11.x - c12.x, c11.y - c12.y)).toBeCloseTo(25.4, 1)
     }
   )
 })

packages/drawtonomy-sdk/__tests__/exporter/roundtripFidelity.test.ts

@@ -1831,7 +1831,9 @@ describe('carry-through export (sidecar verbatim re-emission)', () => {
     if (!existsSync(parkingFixture)) return
     const xml = readFileSync(parkingFixture, 'utf-8')
     const imported = odrToShapesFull(parseOpenDriveXml(xml))
-    expect((imported.parkingSpaces ?? []).length).toBe(7)
+    // Repeated parkingSpace objects expand into many polygon instances, but the
+    // source <object>s stay verbatim in the sidecar (only the polygons multiply).
+    expect((imported.parkingSpaces ?? []).length).toBeGreaterThan(7)
     const out = exportToOpenDrive(snapshotFrom(imported), { sidecar: imported.sidecar })
     // Every original parkingSpace object survives the unedited round trip.
     const before = (xml.match(/type="parkingSpace"/g) || []).length

packages/drawtonomy-sdk/src/exporter/index.ts

@@ -87,6 +87,7 @@ export {
   type OdrSignal,
   type OdrSignalValidity,
   type OdrObject,
+  type OdrObjectRepeat,
   type OdrJunction,
   type OdrJunctionConnection,
   type OdrJunctionLaneLink,

packages/drawtonomy-sdk/src/exporter/odrToShapes.ts

@@ -31,6 +31,8 @@ import type {
   OdrLane,
   OdrLaneSection,
   OdrMap,
+  OdrObject,
+  OdrObjectRepeat,
   OdrRoad,
   OdrRoadMark,
   OdrSignalValidity,
@@ -826,28 +828,108 @@ export function odrToShapes(map: OdrMap, options: OdrToShapesOptions = {}): OdrI
    * the source <object> stays verbatim in the sidecar for carry-through export.
    */
   function materializeParkingSpaces(road: OdrRoad, samples: ReferenceSample[]): void {
-    for (const obj of road.objects) {
-      if (obj.type !== 'parkingSpace') continue
+    /**
+     * A single placement of an object along the reference line: the (s, t) station,
+     * the extra heading tilt of the repeat line, and the per-instance footprint
+     * length/width. A non-repeated object yields exactly one placement at its own
+     * pose with zero tilt and its own dimensions.
+     */
+    interface Placement {
+      s: number
+      t: number
+      /** Heading added to (road heading + object hdg), from the repeat line tilt. */
+      tilt: number
+      length: number
+      width: number
+    }
+
+    /**
+     * Expand a `<repeat>` into discrete instance placements. Instances are spaced
+     * `distance` metres from `s` to `s + length`; t / length / width are linearly
+     * interpolated from their start value to their end value across the span. An
+     * unauthored width/length (`undefined`) falls back to the object's dimension.
+     * A `distance` <= 0 (continuous object) collapses to one swept instance at the
+     * span start, so the object still materializes rather than vanishing.
+     * (ASAM OpenDRIVE 1.8 §13.2; placement math mirrors esmini's
+     * RMObject::GetRepeatInstances.)
+     */
+    const expandRepeat = (obj: OdrObject, rep: OdrObjectRepeat): Placement[] => {
+      const tilt = Math.atan2(rep.tEnd - rep.tStart, rep.length || 1)
+      const lenAt = (f: number) =>
+        rep.lengthStart !== undefined || rep.lengthEnd !== undefined
+          ? (rep.lengthStart ?? 0) + f * ((rep.lengthEnd ?? 0) - (rep.lengthStart ?? 0))
+          : obj.length
+      const widAt = (f: number) =>
+        rep.widthStart !== undefined || rep.widthEnd !== undefined
+          ? (rep.widthStart ?? 0) + f * ((rep.widthEnd ?? 0) - (rep.widthStart ?? 0))
+          : obj.width
+      const placements: Placement[] = []
+      const roadLen = road.length
+      if (!(rep.length > 0) || !(rep.distance > 0)) {
+        // Continuous object (distance == 0) or degenerate span: one instance at start.
+        const f = 0
+        placements.push({
+          s: rep.s,
+          t: rep.tStart,
+          tilt,
+          length: lenAt(f),
+          width: widAt(f),
+        })
+        return placements
+      }
+      // Iterate the span in accumulated length (curS), mirroring esmini's
+      // RMObject::GetRepeatInstances: the loop bound and the road-overflow guard
+      // both use curS (not rep.s + curS) so the repeat start offset does not
+      // reduce how many copies fit.
+      for (let curS = 0; curS < rep.length + S_EPS && curS < roadLen + S_EPS; curS += rep.distance) {
+        const f = curS / rep.length
+        const instLen = lenAt(f)
+        // Stop once an instance would extend past the end of the road.
+        if (curS + instLen > roadLen + S_EPS) break
+        placements.push({
+          s: rep.s + curS,
+          t: rep.tStart + f * (rep.tEnd - rep.tStart),
+          tilt,
+          length: instLen,
+          width: widAt(f),
+        })
+      }
+      return placements
+    }
+
+    /** Build the ENU footprint corners for one placement of an object. */
+    const footprintFor = (obj: OdrObject, pl: Placement): EnuPoint[] => {
       let enuCorners: EnuPoint[] = []
       if (obj.outline.length >= 3) {
         const localCorners = obj.outline.filter(c => c.u !== undefined && c.v !== undefined)
         const roadCorners = obj.outline.filter(c => c.s !== undefined && c.t !== undefined)
         if (localCorners.length >= 3) {
-          const pose = poseAt(samples, obj.s)
+          const pose = poseAt(samples, pl.s)
           // Object origin: reference-line pose at s, shifted by t along +normal.
-          const ox = pose.x - Math.sin(pose.hdg) * obj.t
-          const oy = pose.y + Math.cos(pose.hdg) * obj.t
-          const axisHdg = pose.hdg + obj.hdg
+          const ox = pose.x - Math.sin(pose.hdg) * pl.t
+          const oy = pose.y + Math.cos(pose.hdg) * pl.t
+          const axisHdg = pose.hdg + obj.hdg + pl.tilt
           const cu = Math.cos(axisHdg)
           const su = Math.sin(axisHdg)
-          enuCorners = localCorners.map(c => ({
+          // Scale the authored (start-of-span) outline toward this instance's
+          // dimensions, matching how esmini grows repeated copies.
+          const scaleU = obj.length > S_EPS ? pl.length / obj.length : 1
+          const scaleV = obj.width > S_EPS ? pl.width / obj.width : 1
+          enuCorners = localCorners.map(c => {
+            const u = (c.u as number) * scaleU
+            const v = (c.v as number) * scaleV
             // Local u axis along axisHdg, v axis 90° to its left (+normal).
-            x: ox + cu * (c.u as number) - su * (c.v as number),
-            y: oy + su * (c.u as number) + cu * (c.v as number),
-          }))
+            return { x: ox + cu * u - su * v, y: oy + su * u + cu * v }
+          })
         } else if (roadCorners.length >= 3) {
+          // cornerRoad corners are stations on the reference line. For a repeated
+          // object each corner's authored s is an offset re-based to the instance
+          // station (its minimum s becomes the instance s); a non-repeated object
+          // keeps the corners' absolute s.
+          const minCornerS = Math.min(...roadCorners.map(c => c.s as number))
+          const sBase = obj.repeats.length > 0 ? pl.s - minCornerS : 0
           enuCorners = roadCorners.map(c => {
-            const p = poseAt(samples, c.s as number)
+            const p = poseAt(samples, sBase + (c.s as number))
             return {
               x: p.x - Math.sin(p.hdg) * (c.t as number),
               y: p.y + Math.cos(p.hdg) * (c.t as number),
@@ -857,38 +939,53 @@ export function odrToShapes(map: OdrMap, options: OdrToShapesOptions = {}): OdrI
       }
       if (enuCorners.length < 3) {
         // No usable outline: derive an oriented rectangle from s/t/hdg/l/w.
-        if (!(obj.length > 0) || !(obj.width > 0)) continue
-        const pose = poseAt(samples, obj.s)
-        const cx = pose.x - Math.sin(pose.hdg) * obj.t
-        const cy = pose.y + Math.cos(pose.hdg) * obj.t
-        const axisHdg = pose.hdg + obj.hdg
+        if (!(pl.length > 0) || !(pl.width > 0)) return []
+        const pose = poseAt(samples, pl.s)
+        const cx = pose.x - Math.sin(pose.hdg) * pl.t
+        const cy = pose.y + Math.cos(pose.hdg) * pl.t
+        const axisHdg = pose.hdg + obj.hdg + pl.tilt
         const ux = Math.cos(axisHdg)
         const uy = Math.sin(axisHdg)
         // Normal (left of the object axis) for the width direction.
         const nx = -uy
         const ny = ux
-        const hl = obj.length / 2
-        const hw = obj.width / 2
+        const hl = pl.length / 2
+        const hw = pl.width / 2
         enuCorners = [
           { x: cx - ux * hl - nx * hw, y: cy - uy * hl - ny * hw },
           { x: cx + ux * hl - nx * hw, y: cy + uy * hl - ny * hw },
           { x: cx + ux * hl + nx * hw, y: cy + uy * hl + ny * hw },
           { x: cx - ux * hl + nx * hw, y: cy - uy * hl + ny * hw },
         ]
       }
+      return enuCorners
+    }
 
-      const data: ImportedParkingSpace = {
-        id: idAllocator.next('polygon'),
-        points: enuCorners.map(p => ({ x: enuToCanvasX(p.x), y: enuToCanvasY(p.y) })),
-        osmId: '',
-        attributes: {
-          type: 'parking_space',
-          odr_object_id: obj.id,
-          odr_road_id: road.id,
-          odr_type: obj.type,
-        },
+    for (const obj of road.objects) {
+      if (obj.type !== 'parkingSpace') continue
+      // A repeat replicates the object into many instances; without one the object
+      // is placed exactly once at its own pose.
+      const placements: Placement[] =
+        obj.repeats.length > 0
+          ? obj.repeats.flatMap(rep => expandRepeat(obj, rep))
+          : [{ s: obj.s, t: obj.t, tilt: 0, length: obj.length, width: obj.width }]
+
+      for (const pl of placements) {
+        const enuCorners = footprintFor(obj, pl)
+        if (enuCorners.length < 3) continue
+        const data: ImportedParkingSpace = {
+          id: idAllocator.next('polygon'),
+          points: enuCorners.map(p => ({ x: enuToCanvasX(p.x), y: enuToCanvasY(p.y) })),
+          osmId: '',
+          attributes: {
+            type: 'parking_space',
+            odr_object_id: obj.id,
+            odr_road_id: road.id,
+            odr_type: obj.type,
+          },
+        }
+        ;(result.parkingSpaces ??= []).push(data)
       }
-      ;(result.parkingSpaces ??= []).push(data)
       convertedObjectCount++
     }
   }