added lane relations to roadspace objects

rtron-model/src/main/kotlin/io/rtron/model/roadspaces/roadspace/objects/RoadspaceObject.kt

@@ -16,7 +16,6 @@
 
 package io.rtron.model.roadspaces.roadspace.objects
 
-import arrow.core.Option
 import io.rtron.math.geometry.euclidean.threed.AbstractGeometry3D
 import io.rtron.model.roadspaces.identifier.LaneIdentifierRange
 import io.rtron.model.roadspaces.identifier.RoadspaceObjectIdentifier
@@ -33,7 +32,7 @@ data class RoadspaceObject(
     val id: RoadspaceObjectIdentifier,
     val type: RoadObjectType = RoadObjectType.NONE,
     val geometry: AbstractGeometry3D,
-    val laneRelations: Option<LaneIdentifierRange>,
+    val laneRelations: List<LaneIdentifierRange>,
     val attributes: AttributeList
 ) {
 

rtron-model/src/main/kotlin/io/rtron/model/roadspaces/roadspace/road/LaneSection.kt

@@ -25,6 +25,7 @@ import io.rtron.math.geometry.curved.oned.point.CurveRelativeVector1D
 import io.rtron.math.range.Range
 import io.rtron.math.std.sign
 import io.rtron.model.roadspaces.identifier.LaneIdentifier
+import io.rtron.model.roadspaces.identifier.LaneIdentifierRange
 import io.rtron.model.roadspaces.identifier.LaneSectionIdentifier
 import io.rtron.std.getValueEither
 import kotlin.math.abs
@@ -73,6 +74,12 @@ data class LaneSection(
     fun getLane(laneIdentifier: LaneIdentifier): Either<IllegalArgumentException, Lane> =
         lanes.getValueEither(laneIdentifier.laneId).mapLeft { it.toIllegalArgumentException() }
 
+    /** Returns the complete range of lane identifiers contained in this lane section. */
+    fun getCompleteLaneIdentifierRange(): LaneIdentifierRange {
+        val range = Range.closed(lanes.keys.min(), lanes.keys.max())
+        return LaneIdentifierRange(range, id)
+    }
+
     /**
      * Returns the lateral offset function located on a lane with [laneId].
      *
@@ -112,6 +119,12 @@ data class LaneSection(
         laneHeightOffset
     }
 
+    /**
+     * Returns the true, if the [curveRelativePoint] is within this [LaneSection] bounds.
+     *
+     **/
+    fun contains(curveRelativePoint: CurveRelativeVector1D) = curvePositionDomain.contains(curveRelativePoint.curvePosition)
+
     private fun getOuterLaneHeightOffset(laneIdentifier: LaneIdentifier):
         Either<IllegalArgumentException, UnivariateFunction> =
         getLane(laneIdentifier).map { it.outerHeightOffset }

rtron-model/src/main/kotlin/io/rtron/model/roadspaces/roadspace/road/Road.kt

@@ -24,10 +24,12 @@ import arrow.core.Some
 import arrow.core.continuations.either
 import arrow.core.getOrElse
 import arrow.core.left
+import arrow.core.right
 import io.rtron.math.analysis.function.univariate.UnivariateFunction
 import io.rtron.math.analysis.function.univariate.combination.SectionedUnivariateFunction
 import io.rtron.math.analysis.function.univariate.combination.StackedFunction
 import io.rtron.math.analysis.function.univariate.pure.ConstantFunction
+import io.rtron.math.geometry.curved.oned.point.CurveRelativeVector1D
 import io.rtron.math.geometry.curved.threed.surface.AbstractCurveRelativeSurface3D
 import io.rtron.math.geometry.curved.threed.surface.SectionedCurveRelativeParametricSurface3D
 import io.rtron.math.geometry.euclidean.threed.AbstractGeometry3D
@@ -133,6 +135,16 @@ class Road(
     fun getLaneSection(laneSectionIdentifier: LaneSectionIdentifier): Either<IllegalArgumentException, LaneSection> =
         laneSections.getValueEither(laneSectionIdentifier.laneSectionId).mapLeft { it.toIllegalArgumentException() }
 
+    /** Returns the lane section of the [curveRelativePoint]; if it does not exist, an [Either.Left] is returned. */
+    fun getLaneSection(curveRelativePoint: CurveRelativeVector1D): Either<IllegalArgumentException, LaneSection> {
+        val selectedLaneSections: List<LaneSection> = laneSections.filter { it.contains(curveRelativePoint) }
+        return when (selectedLaneSections.size) {
+            1 -> selectedLaneSections.first().right()
+            0 -> IllegalArgumentException("No laneSection found").left()
+            else -> throw IllegalArgumentException("Domains of lane sections must close flush.")
+        }
+    }
+
     /** Returns an individual lane referenced by [laneIdentifier]; if it does not exist, an [Either.Left] is returned. */
     fun getLane(laneIdentifier: LaneIdentifier): Either<IllegalArgumentException, Lane> = either.eager {
         val laneSection = getLaneSection(laneIdentifier.laneSectionIdentifier).bind()

rtron-transformer/src/main/kotlin/io/rtron/transformer/converter/opendrive2roadspaces/roadspaces/RoadspaceBuilder.kt

@@ -31,6 +31,7 @@ import io.rtron.model.roadspaces.identifier.ModelIdentifier
 import io.rtron.model.roadspaces.identifier.RoadspaceIdentifier
 import io.rtron.model.roadspaces.roadspace.Roadspace
 import io.rtron.model.roadspaces.roadspace.attribute.attributes
+import io.rtron.model.roadspaces.roadspace.objects.RoadspaceObject
 import io.rtron.transformer.converter.opendrive2roadspaces.Opendrive2RoadspacesParameters
 import io.rtron.transformer.converter.opendrive2roadspaces.analysis.FunctionBuilder
 import io.rtron.transformer.converter.opendrive2roadspaces.geometry.Curve3DBuilder
@@ -85,18 +86,18 @@ class RoadspaceBuilder(
         )
         val roadSurfaceWithoutTorsion = CurveRelativeParametricSurface3D(roadReferenceLine, lateralProfileRoadShape)
 
-        // build up the road containing only lane sections, lanes (no road side objects)
+        // build up the road containing only lane sections, lanes (no roadside objects)
         val roadspaceRoad = roadBuilder
             .buildRoad(roadspaceId, road, roadSurface, roadSurfaceWithoutTorsion, attributes)
             .handleMessageList { messageList += it }
 
         // build up the road space objects (OpenDRIVE: road objects & signals)
         val roadspaceObjectsFromRoadObjects = road.objects.fold({ emptyList() }, { roadObjects ->
-            roadObjectBuilder.buildRoadspaceObjects(roadspaceId, roadObjects, roadReferenceLine, attributes)
+            roadObjectBuilder.buildRoadspaceObjects(roadspaceId, roadObjects, roadReferenceLine, roadspaceRoad, attributes)
                 .handleMessageList { messageList += it }
         })
-        val roadspaceObjectsFromRoadSignals = road.signals.fold({ emptyList() }, { roadSignals ->
-            roadObjectBuilder.buildRoadspaceObjects(roadspaceId, roadSignals, roadReferenceLine, attributes)
+        val roadspaceObjectsFromRoadSignals: List<RoadspaceObject> = road.signals.fold({ emptyList() }, { roadSignals ->
+            roadObjectBuilder.buildRoadspaceObjects(roadspaceId, roadSignals, roadReferenceLine, roadspaceRoad, attributes)
         })
 
         // combine the models into a road space object

rtron-transformer/src/main/kotlin/io/rtron/transformer/converter/opendrive2roadspaces/roadspaces/RoadspaceObjectBuilder.kt

@@ -31,9 +31,11 @@ import io.rtron.math.geometry.curved.threed.point.CurveRelativeVector3D
 import io.rtron.math.geometry.euclidean.threed.AbstractGeometry3D
 import io.rtron.math.geometry.euclidean.threed.Rotation3D
 import io.rtron.math.geometry.euclidean.threed.curve.Curve3D
+import io.rtron.math.range.Range
 import io.rtron.model.opendrive.objects.EObjectType
 import io.rtron.model.opendrive.signal.RoadSignals
 import io.rtron.model.opendrive.signal.RoadSignalsSignal
+import io.rtron.model.roadspaces.identifier.LaneIdentifierRange
 import io.rtron.model.roadspaces.identifier.RoadspaceIdentifier
 import io.rtron.model.roadspaces.identifier.RoadspaceObjectIdentifier
 import io.rtron.model.roadspaces.roadspace.attribute.AttributeList
@@ -48,6 +50,7 @@ import io.rtron.transformer.converter.opendrive2roadspaces.geometry.Vector3DBuil
 import io.rtron.model.opendrive.objects.RoadObjects as OpendriveRoadObjects
 import io.rtron.model.opendrive.objects.RoadObjectsObject as OpendriveRoadObject
 import io.rtron.model.opendrive.objects.RoadObjectsObjectRepeat as OpendriveRoadObjectRepeat
+import io.rtron.model.roadspaces.roadspace.road.Road as RoadspaceRoad
 
 /**
  * Builder for [RoadspaceObject] which correspond to the OpenDRIVE road object class.
@@ -67,10 +70,11 @@ class RoadspaceObjectBuilder(
         roadspaceId: RoadspaceIdentifier,
         roadObjects: OpendriveRoadObjects,
         roadReferenceLine: Curve3D,
+        road: RoadspaceRoad,
         baseAttributes: AttributeList
     ): ContextMessageList<List<RoadspaceObject>> {
         return roadObjects.roadObject
-            .map { buildRoadObject(roadspaceId, it, roadReferenceLine, baseAttributes) }
+            .map { buildRoadObject(roadspaceId, it, roadReferenceLine, road, baseAttributes) }
             .mergeMessageLists()
             .map { it.flatten() }
     }
@@ -79,6 +83,7 @@ class RoadspaceObjectBuilder(
         id: RoadspaceIdentifier,
         roadObject: OpendriveRoadObject,
         roadReferenceLine: Curve3D,
+        road: RoadspaceRoad,
         baseAttributes: AttributeList
     ): ContextMessageList<NonEmptyList<RoadspaceObject>> {
         val messageList = DefaultMessageList()
@@ -91,19 +96,20 @@ class RoadspaceObjectBuilder(
             buildAttributes(roadObject) +
             buildAttributes(roadObject.curveRelativePosition) +
             buildAttributes(roadObject.referenceLinePointRelativeRotation)
+        val laneRelations = buildLaneRelations(roadObject, road)
 
         val roadObjectsFromRepeat = roadObject.repeat.map { currentRoadObjectRepeat ->
             val repeatIdentifier = currentRoadObjectRepeat.additionalId.toEither { IllegalStateException("Additional outline ID must be available.") }.getOrElse { throw it }
 
             val roadspaceObjectId = RoadspaceObjectIdentifier("${roadObject.id}_${repeatIdentifier.repeatIndex}", roadObject.name, id)
             val geometry = buildGeometries(roadObject, currentRoadObjectRepeat.some(), roadReferenceLine).handleMessageList { messageList += it }
-            RoadspaceObject(roadspaceObjectId, type, geometry, None, attributes)
+            RoadspaceObject(roadspaceObjectId, type, geometry, laneRelations, attributes)
         }
 
         val roadObjects = if (roadObjectsFromRepeat.isEmpty()) {
             val roadspaceObjectId = RoadspaceObjectIdentifier(roadObject.id, roadObject.name, id)
             val geometry = buildGeometries(roadObject, None, roadReferenceLine).handleMessageList { messageList += it }
-            nonEmptyListOf(RoadspaceObject(roadspaceObjectId, type, geometry, None, attributes))
+            nonEmptyListOf(RoadspaceObject(roadspaceObjectId, type, geometry, laneRelations, attributes))
         } else {
             roadObjectsFromRepeat.toNonEmptyListOrNull()!!
         }
@@ -215,26 +221,29 @@ class RoadspaceObjectBuilder(
         id: RoadspaceIdentifier,
         roadSignals: RoadSignals,
         roadReferenceLine: Curve3D,
+        road: RoadspaceRoad,
         baseAttributes: AttributeList
     ): List<RoadspaceObject> {
-        return roadSignals.signal.map { buildRoadSignalsSignal(id, it, roadReferenceLine, baseAttributes) }
+        return roadSignals.signal.map { buildRoadSignalsSignal(id, it, roadReferenceLine, road, baseAttributes) }
     }
 
     private fun buildRoadSignalsSignal(
         id: RoadspaceIdentifier,
         roadSignal: RoadSignalsSignal,
         roadReferenceLine: Curve3D,
+        road: RoadspaceRoad,
         baseAttributes: AttributeList
     ): RoadspaceObject {
         val objectId = RoadspaceObjectIdentifier(roadSignal.id, roadSignal.name, id)
 
         val geometry = buildGeometries(roadSignal, roadReferenceLine)
+        val laneRelations = buildLaneRelations(roadSignal, road)
         val attributes = baseAttributes +
             buildAttributes(roadSignal) +
             buildAttributes(roadSignal.curveRelativePosition) +
             buildAttributes(roadSignal.referenceLinePointRelativeRotation)
 
-        return RoadspaceObject(objectId, RoadObjectType.SIGNAL, geometry, None, attributes)
+        return RoadspaceObject(objectId, RoadObjectType.SIGNAL, geometry, laneRelations, attributes)
     }
 
     private fun buildAttributes(signal: RoadSignalsSignal): AttributeList =
@@ -256,4 +265,24 @@ class RoadspaceObjectBuilder(
 
         return Vector3DBuilder.buildVector3Ds(signal, curveAffine, force = true)
     }
+
+    private fun buildLaneRelations(roadObject: OpendriveRoadObject, road: RoadspaceRoad): List<LaneIdentifierRange> {
+        val laneSection = road.getLaneSection(roadObject.curveRelativePosition.toCurveRelative1D()).getOrElse { throw it }
+
+        return if (roadObject.validity.isEmpty()) {
+            listOf(laneSection.getCompleteLaneIdentifierRange())
+        } else {
+            roadObject.validity.map { LaneIdentifierRange(Range.closed(it.fromLane, it.toLane), laneSection.id) }
+        }
+    }
+
+    private fun buildLaneRelations(roadSignal: RoadSignalsSignal, road: RoadspaceRoad): List<LaneIdentifierRange> {
+        val laneSection = road.getLaneSection(roadSignal.curveRelativePosition.toCurveRelative1D()).getOrElse { throw it }
+
+        return if (roadSignal.validity.isEmpty()) {
+            listOf(laneSection.getCompleteLaneIdentifierRange())
+        } else {
+            roadSignal.validity.map { LaneIdentifierRange(Range.closed(it.fromLane, it.toLane), laneSection.id) }
+        }
+    }
 }