diff --git a/packages/drawtonomy-sdk/__tests__/validator/geometry.test.ts b/packages/drawtonomy-sdk/__tests__/validator/geometry.test.ts index f000f85..afb12d7 100644 --- a/packages/drawtonomy-sdk/__tests__/validator/geometry.test.ts +++ b/packages/drawtonomy-sdk/__tests__/validator/geometry.test.ts @@ -114,14 +114,23 @@ describe('checkGeometry', () => { }) describe('road link contact', () => { - const pair = (bx: number, offsetA = 0, offsetB = 0): string => ` + // Road A runs along y = 0 from x = 0 to x = 100. Road B starts at + // (bx, by). Each carries one 3.5 m right lane, so a road's lane boundaries + // sit at its centre line (the reference line shifted by its laneOffset) + // and 3.5 m to the right of it. + const pair = ( + bx: number, + opts: { by?: number; offsetA?: string; offsetB?: string } = {} + ): string => { + const { by = 0, offsetA = 'a="0" b="0"', offsetB = 'a="0" b="0"' } = opts + return `
- +
@@ -130,9 +139,9 @@ describe('checkGeometry', () => { - + - +
@@ -140,6 +149,48 @@ describe('checkGeometry', () => { ` + } + + /** + * Two roads linked end-to-start, with each side's lane content injectable. + * Road A runs along y = 0 from x = 0 to x = 100; road B starts at + * (100, by). Defaults give both a single 3.5 m right lane, so their lane + * boundaries coincide and the pair is clean. + */ + const twoRoads = (opts: { + by?: number + aLanes?: string + bLanes?: string + aSections?: string + bOffset?: string + }): string => { + const { + by = 0, + aLanes = '', + bLanes = '', + aSections, + bOffset = '', + } = opts + const aLaneXml = + aSections ?? + `
${aLanes}` + return ` + +
+ + + + ${aLaneXml} + + + + + ${bOffset} +
${bLanes} + + +` + } it('accepts roads that touch', () => { expect(geomRules(pair(100))).toEqual([]) @@ -153,10 +204,116 @@ describe('checkGeometry', () => { expect(validateOpenDrive(pair(150)).verdict).toBe('yellow') }) - it('subtracts the lane-offset difference', () => { - // fabriksgatan's pattern: a connecting road with laneOffset 1.75 linking - // to a mainline with 0. The reference lines are 1.75 m apart by design. - expect(geomRules(pair(101.75, 1.75, 0))).toEqual([]) + it('reports the lane distance, not the reference-line distance', () => { + const found = findingsFor(pair(150)).filter(f => f.rule === 'geom.road-link-gap') + expect(found[0].message).toContain('lane boundaries') + expect(found[0].message).toContain('50.000 m apart') + }) + + // The false positive this rule exists to avoid. Road B's reference line is + // pushed 5 m off road A's by a laneOffset that ramps along s, so a check + // comparing reference-line endpoints (even after subtracting the offset + // difference at the contact) sees a gap — while the lanes themselves meet + // exactly. Measured on a real map, every report of the old proxy was of + // this shape and every one had a true lane distance of 0.000 m. + it('accepts lanes that meet while the reference lines are far apart', () => { + // A: no offset, so its centre is y = 0 and its lane edge y = -3.5. + // B: reference line at y = -5, laneOffset ramping from +5 at s = 0, so + // its centre is also y = 0 at the contact and its lane edge y = -3.5. + const xml = pair(100, { by: -5, offsetB: 'a="5" b="-0.02"' }) + expect(geomRules(xml)).toEqual([]) + }) + + it('still detects lanes that are genuinely apart', () => { + // Same construction, but B's offset leaves its lanes 1.5 m off A's. + const xml = pair(100, { by: -5, offsetB: 'a="3.5" b="-0.02"' }) + const found = findingsFor(xml).filter(f => f.rule === 'geom.road-link-gap') + expect(found.length).toBeGreaterThan(0) + expect(found[0].message).toContain('1.500 m apart') + }) + + it('honours a caller-supplied threshold', () => { + const xml = pair(100, { by: -5, offsetB: 'a="3.5" b="-0.02"' }) + expect(geomRules(xml, { geometry: { roadLinkGapMeters: 2 } })).toEqual([]) + }) + + // A lateral mismatch is not a gap. Lane counts and widths routinely differ + // across a link (merges, ramps and junction connectors: 91 of the 156 + // links on one real map), so the rule asks whether the two cross sections + // *touch*, not whether they are congruent. What it must catch is the two + // sections being wholly apart, which is the soderleden defect. + it('accepts a link where the roads touch but the lane counts differ', () => { + const xml = twoRoads({ + aLanes: '', + bLanes: + '' + + '', + }) + expect(geomRules(xml)).toEqual([]) + }) + + it('measures a multi-section road at the section covering the contact', () => { + // Road A's lane widens to 5 m from s = 50, and road B is shifted so that + // it meets that widened edge. Reading A's *first* section would place its + // edge 1.5 m away and report a gap that does not exist. + const xml = twoRoads({ + aSections: + '
' + + '' + + '
' + + '', + // B is a lone boundary pair 5 m below the reference line: its centre is + // at y = -5.0, exactly A's widened outer edge. + by: -5, + bLanes: '', + }) + expect(xml).toContain('laneSection s="50"') + expect(geomRules(xml)).toEqual([]) + + // The same B against a road whose lane never widens is 1.5 m short. + const narrow = twoRoads({ + by: -5, + bLanes: '', + }) + const found = findingsFor(narrow).filter(f => f.rule === 'geom.road-link-gap') + expect(found.length).toBeGreaterThan(0) + expect(found[0].message).toContain('1.500 m apart') + }) + + it('evaluates the width record covering the contact, not the first', () => { + // One lane section, two records: 3.5 m up to sOffset 50 and 6.0 m + // after it. The contact at s = 100 must read 6.0 m, putting A's outer + // edge at y = -6.0 where B's lone boundary sits. + const xml = twoRoads({ + aLanes: + '' + + '' + + '' + + '', + by: -6, + bLanes: '', + }) + expect(geomRules(xml)).toEqual([]) + + // Reading only the first record would have put A's edge at -3.5, which + // is where this B sits — and that must be reported as 2.5 m away. + const wrong = twoRoads({ + aLanes: + '' + + '' + + '' + + '', + by: -3.5, + bLanes: '', + }) + const found = findingsFor(wrong).filter(f => f.rule === 'geom.road-link-gap') + expect(found.length).toBeGreaterThan(0) + expect(found[0].message).toContain('2.500 m apart') + }) + + it('takes the nearer end when the link declares no contact point', () => { + const xml = pair(100).replace(' contactPoint="start"', '') + expect(geomRules(xml)).toEqual([]) }) }) @@ -177,7 +334,7 @@ describe('checkGeometry', () => { expect(DEFAULT_GEOMETRY_THRESHOLDS).toEqual({ planViewGapMeters: 0.02, planViewHeadingRad: 0.005, - roadLinkGapMeters: 0.5, + roadLinkGapMeters: 0.3, lengthMismatchRatio: 0.01, negativeWidthToleranceMeters: 0.001, }) diff --git a/packages/drawtonomy-sdk/src/validator/layers/geometry.ts b/packages/drawtonomy-sdk/src/validator/layers/geometry.ts index 1102977..d41e691 100644 --- a/packages/drawtonomy-sdk/src/validator/layers/geometry.ts +++ b/packages/drawtonomy-sdk/src/validator/layers/geometry.ts @@ -24,24 +24,40 @@ // road length vs sum observed max 4.0e-16 default 1 % (huge) // // Between roads it is a different story, and the reason is structural rather -// than a matter of precision. `road@length`-scale gaps at a link are normal: +// than a matter of precision. `road@length`-scale gaps between *reference +// lines* at a link are normal: // // 1. A road carrying a has its reference line laterally shifted // from the lane geometry. Two roads with different offsets meet along -// their *lanes* while their reference lines stay apart by exactly the -// offset difference. fabriksgatan's connecting roads (laneOffset a=1.75) -// linking to mainlines (a=0) produce a uniform 1.75 m; multi_intersections -// produces 3.75 m the same way. Both maps are correct. +// their *lanes* while their reference lines stay apart. fabriksgatan's +// connecting roads (laneOffset a=1.75) linking to mainlines (a=0) produce +// a uniform 1.75 m; multi_intersections produces 3.75 m the same way. Both +// maps are correct. // 2. Some shipped maps simply contain link records their geometry does not // honour: soderleden road 7 declares its predecessor to be road 2 at that -// road's end, 66 m from where road 7 actually begins. esmini drives this -// map regardless, because it routes through the junction rather than the -// stray link. +// road's end, tens of metres from where road 7 actually begins. esmini +// drives this map regardless, because it routes through the junction +// rather than the stray link. // -// So the road-link check is reported as a *warning*: it is real evidence worth -// surfacing, but it must not redden a map that ships and works. The lane offset -// difference is subtracted before comparing, which removes cause (1) exactly -// and leaves cause (2) visible. +// An earlier revision of this rule compared reference-line endpoints and +// subtracted |laneOffsetA - laneOffsetB| as slack. That proxy is wrong whenever +// the offset varies with s or the two roads meet at an angle: measured over a +// real map, every one of its ten reports was a false positive whose lanes in +// fact touched to 0.000 m. The rule therefore measures what it actually claims +// to measure — the distance between the two roads' *lane boundary cross +// sections* at the contact — which removes cause (1) by construction, with no +// slack term to tune, and leaves cause (2) visible. +// +// What is compared is the *minimum* distance between the two boundary sets, +// i.e. "do these two cross sections touch anywhere", not "are they congruent". +// That is deliberate: lane counts and widths routinely differ across a link at +// merges, ramps and junction connectors (91 of 156 links on one measured map), +// so requiring congruence would re-introduce false positives of a new shape. +// A road whose lanes are wholly displaced from its neighbour's — the soderleden +// case — still separates every pair of boundaries and is still reported. +// +// The check is still reported as a *warning*: it is real evidence worth +// surfacing, but it must not redden a map that ships and works. // // Lane widths need a tolerance for the opposite reason — not structure, but // arithmetic. Merge and exit lanes are authored to close at exactly zero, e.g. @@ -53,7 +69,12 @@ // the two cleanly rather than splitting a continuum. import { evalGeometry } from '../../exporter/odrGeometry.js' -import type { OdrMap, OdrRoad, OdrWidth } from '../../exporter/opendriveParser.js' +import type { + OdrLaneSection, + OdrMap, + OdrRoad, + OdrWidth, +} from '../../exporter/opendriveParser.js' import type { OdrFinding, ResolvedGeometryThresholds } from '../types.js' /** Wrap an angle to (-pi, pi]. */ @@ -102,6 +123,91 @@ function poseAtEnd(road: OdrRoad, at: 'start' | 'end'): Pose | null { const stationAtEnd = (road: OdrRoad, at: 'start' | 'end'): number => at === 'start' ? 0 : road.length +/** + * Width of a lane at `ds` metres into its lane section. + * + * A `` record applies from its own `sOffset` until the next one, and its + * polynomial is in `ds - sOffset`. Records before the first `sOffset` (which a + * conforming file does not produce, but a hand-edited one might) fall back to + * the first record evaluated at its own origin. + */ +function laneWidthAt(widths: readonly OdrWidth[], ds: number): number { + if (widths.length === 0) return 0 + let applicable = widths[0] + for (const rec of widths) { + if (rec.sOffset <= ds) applicable = rec + else break + } + return evalCubic(applicable, Math.max(ds - applicable.sOffset, 0)) +} + +/** The lane section covering station `s` (the last one starting at or before it). */ +function sectionAt(road: OdrRoad, s: number): OdrLaneSection | null { + if (road.laneSections.length === 0) return null + let applicable = road.laneSections[0] + for (const section of road.laneSections) { + if (section.s <= s + 1e-9) applicable = section + else break + } + return applicable +} + +interface Point { + x: number + y: number +} + +/** + * World positions of every lane boundary at a road's start or end. + * + * The boundaries are the centre line (the reference line shifted laterally by + * ``) plus the running sum of lane widths outward in each + * direction: left lanes accumulate in +t, right lanes in -t, where the lateral + * unit vector is the reference-line normal `(-sin hdg, cos hdg)`. + * + * Returns null when the road has no plan view to evaluate. A road without lane + * sections still yields its centre point, which is the best available + * statement of where it ends. + */ +function laneCrossSection(road: OdrRoad, at: 'start' | 'end'): Point[] | null { + const pose = poseAtEnd(road, at) + if (!pose) return null + + const s = stationAtEnd(road, at) + const nx = -Math.sin(pose.hdg) + const ny = Math.cos(pose.hdg) + + const offsets: number[] = [laneOffsetAt(road, s)] + const section = sectionAt(road, s) + if (section) { + // `ds` is measured from the section's own start, which is where a + // `` record's sOffset is relative to. + const ds = Math.max(s - section.s, 0) + for (const side of [section.left, section.right] as const) { + let t = offsets[0] + for (const lane of side) { + const width = laneWidthAt(lane.widths, ds) + t += side === section.left ? width : -width + offsets.push(t) + } + } + } + + return offsets.map(t => ({ x: pose.x + nx * t, y: pose.y + ny * t })) +} + +/** Smallest distance between any point of `a` and any point of `b`. */ +function minPointDistance(a: readonly Point[], b: readonly Point[]): number { + let best = Infinity + for (const p of a) { + for (const q of b) { + const d = Math.hypot(p.x - q.x, p.y - q.y) + if (d < best) best = d + } + } + return best +} + export function checkGeometry( map: OdrMap, thresholds: ResolvedGeometryThresholds @@ -219,28 +325,37 @@ export function checkGeometry( if (!target) continue // layer 2 reported the dangling link const myEnd = which === 'successor' ? 'end' : 'start' - const theirEnd = link.contactPoint ?? (which === 'successor' ? 'start' : 'end') - const mine = poseAtEnd(road, myEnd) - const theirs = poseAtEnd(target, theirEnd) - if (!mine || !theirs) continue - - // Subtract the lane-offset difference: two roads whose lanes meet can - // have reference lines apart by exactly that amount, and flagging it - // would redden fabriksgatan and multi_intersections, which are correct. - const myOffset = laneOffsetAt(road, stationAtEnd(road, myEnd)) - const theirOffset = laneOffsetAt(target, stationAtEnd(target, theirEnd)) - const offsetSlack = Math.abs(myOffset - theirOffset) - - const raw = Math.hypot(mine.x - theirs.x, mine.y - theirs.y) - const gap = Math.max(raw - offsetSlack, 0) + const mine = laneCrossSection(road, myEnd) + if (!mine) continue + + // With an explicit contactPoint there is one cross section to compare + // against. Without one, the link is under-specified, so both of the + // target's ends are tried and the nearer is taken: reporting the larger + // of two readings would be inventing a defect the document never + // asserted. + const theirEnds: readonly ('start' | 'end')[] = link.contactPoint + ? [link.contactPoint] + : ['start', 'end'] + + let gap = Infinity + let theirEnd: 'start' | 'end' = theirEnds[0] + for (const end of theirEnds) { + const theirs = laneCrossSection(target, end) + if (!theirs) continue + const d = minPointDistance(mine, theirs) + if (d < gap) { + gap = d + theirEnd = end + } + } + if (!Number.isFinite(gap)) continue + if (gap > thresholds.roadLinkGapMeters) { findings.push({ severity: 'warning', category: 'MAP_DEFECT', rule: 'geom.road-link-gap', - message: - `road ${road.id} <${which}> declares road ${target.id} at its ${theirEnd}, but the two reference lines are ${gap.toFixed(3)} m apart` + - (offsetSlack > 0 ? ` (after allowing ${offsetSlack.toFixed(3)} m of lane offset)` : ''), + message: `road ${road.id} <${which}> declares road ${target.id} at its ${theirEnd}, but the lane boundaries of the two roads are ${gap.toFixed(3)} m apart at the contact`, location: { roadId: road.id }, }) } diff --git a/packages/drawtonomy-sdk/src/validator/types.ts b/packages/drawtonomy-sdk/src/validator/types.ts index 0997977..6d40070 100644 --- a/packages/drawtonomy-sdk/src/validator/types.ts +++ b/packages/drawtonomy-sdk/src/validator/types.ts @@ -90,8 +90,8 @@ export interface OdrGeometryThresholds { */ planViewHeadingRad?: number /** - * Maximum allowed position gap at a road-to-road link contact point (m). - * Default 0.5. + * Maximum allowed distance between the lane-boundary cross sections of two + * linked roads at their contact point (m). Default 0.3. */ roadLinkGapMeters?: number /** @@ -131,7 +131,7 @@ export interface ResolvedGeometryThresholds { export const DEFAULT_GEOMETRY_THRESHOLDS: ResolvedGeometryThresholds = { planViewGapMeters: 0.02, planViewHeadingRad: 0.005, - roadLinkGapMeters: 0.5, + roadLinkGapMeters: 0.3, lengthMismatchRatio: 0.01, negativeWidthToleranceMeters: 0.001, }