core: add temporary speed limits to STDCM

Co-authored-by: Younes Khoudli <[email protected]> Signed-off-by: Loup Federico <[email protected]>
OpenRailAssociation · Nov 28, 2024 · 1891f5b · 1891f5b
1 parent 9e66625
commit 1891f5b
Show file tree

Hide file tree

Showing 46 changed files with 632 additions and 146 deletions.
diff --git a/core/kt-osrd-rjs-parser/src/main/kotlin/fr/sncf/osrd/RawInfraRJSParser.kt b/core/kt-osrd-rjs-parser/src/main/kotlin/fr/sncf/osrd/RawInfraRJSParser.kt
@@ -53,7 +53,8 @@ private fun parseLineString(rjsLineString: RJSLineString?): LineString? {
 private fun getSlopes(rjsTrackSection: RJSTrackSection): DistanceRangeMap<Double> {
     val slopes =
         distanceRangeMapOf(
-            listOf(DistanceRangeMap.RangeMapEntry(0.meters, rjsTrackSection.length.meters, 0.0))
+            *listOf(DistanceRangeMap.RangeMapEntry(0.meters, rjsTrackSection.length.meters, 0.0))
+                .toTypedArray()
         )
     if (rjsTrackSection.slopes != null) {
         for (rjsSlope in rjsTrackSection.slopes) {
@@ -75,7 +76,8 @@ private fun getSlopes(rjsTrackSection: RJSTrackSection): DistanceRangeMap<Double
 private fun getCurves(rjsTrackSection: RJSTrackSection): DistanceRangeMap<Double> {
     val curves =
         distanceRangeMapOf(
-            listOf(DistanceRangeMap.RangeMapEntry(0.meters, rjsTrackSection.length.meters, 0.0))
+            *listOf(DistanceRangeMap.RangeMapEntry(0.meters, rjsTrackSection.length.meters, 0.0))
+                .toTypedArray()
         )
     if (rjsTrackSection.curves != null) {
         for (rjsCurve in rjsTrackSection.curves) {

diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/PathProperties.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/PathProperties.kt
@@ -3,6 +3,7 @@ package fr.sncf.osrd.sim_infra.api
 import fr.sncf.osrd.geom.LineString
 import fr.sncf.osrd.sim_infra.impl.ChunkPath
 import fr.sncf.osrd.sim_infra.impl.PathPropertiesImpl
+import fr.sncf.osrd.sim_infra.impl.TemporarySpeedLimitManager
 import fr.sncf.osrd.sim_infra.impl.buildChunkPath
 import fr.sncf.osrd.utils.DistanceRangeMap
 import fr.sncf.osrd.utils.indexing.DirStaticIdxList
@@ -43,7 +44,10 @@ interface PathProperties {
     fun getNeutralSections(): DistanceRangeMap<NeutralSection>
 
     @JvmName("getSpeedLimitProperties")
-    fun getSpeedLimitProperties(trainTag: String?): DistanceRangeMap<SpeedLimitProperty>
+    fun getSpeedLimitProperties(
+        trainTag: String?,
+        temporarySpeedLimitManager: TemporarySpeedLimitManager?
+    ): DistanceRangeMap<SpeedLimitProperty>
 
     fun getZones(): DistanceRangeMap<ZoneId>
 
@@ -74,7 +78,7 @@ fun buildPathPropertiesFrom(
     chunks: DirStaticIdxList<TrackChunk>,
     pathBeginOffset: Offset<Path>,
     pathEndOffset: Offset<Path>,
-    routes: List<RouteId>? = null
+    routes: List<RouteId>? = null,
 ): PathProperties {
     val chunkPath = buildChunkPath(infra, chunks, pathBeginOffset, pathEndOffset)
     return makePathProperties(infra, chunkPath, routes)
@@ -84,7 +88,8 @@ fun buildPathPropertiesFrom(
 fun makePathProperties(
     infra: RawSignalingInfra,
     chunkPath: ChunkPath,
-    routes: List<RouteId>? = null
+    routes: List<RouteId>? = null,
+    temporarySpeedLimitManager: TemporarySpeedLimitManager? = null,
 ): PathProperties {
     return PathPropertiesImpl(infra, chunkPath, routes)
 }

diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/RawSignalingInfra.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/RawSignalingInfra.kt
@@ -63,6 +63,8 @@ interface RawSignalingInfra : RoutingInfra {
     fun getRawParameters(signal: LogicalSignalId): RawSignalParameters
 
     fun getNextSignalingSystemIds(signal: LogicalSignalId): List<String>
+
+    fun findDetector(detectorName: String): DetectorId?
 }
 
 fun RawSignalingInfra.getLogicalSignalName(signal: LogicalSignalId): String? {

diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/TrackInfra.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/TrackInfra.kt
@@ -33,7 +33,7 @@ interface TrackInfra {
 /** A directional detector encodes a direction over a detector */
 typealias DirDetectorId = DirStaticIdx<Detector>
 
-/** A directional detector encodes a direction over a track chunk */
+/** A directional track chunk encodes a direction over a track chunk */
 typealias DirTrackChunkId = DirStaticIdx<TrackChunk>
 
 typealias OptDirTrackChunkId = OptDirStaticIdx<TrackChunk>

diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/TrackProperties.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/TrackProperties.kt
@@ -18,7 +18,11 @@ data class NeutralSection(
 data class SpeedLimitProperty(
     val speed: Speed,
     val source: SpeedLimitSource? // if train-tag used, source of the speed-limit
-)
+) : Comparable<SpeedLimitProperty> {
+    override fun compareTo(other: SpeedLimitProperty): Int {
+        return this.speed.compareTo(other.speed)
+    }
+}
 
 sealed class SpeedLimitSource : SelfTypeHolder {
     override val selfType: Class<out SelfTypeHolder>

diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt
@@ -42,7 +42,7 @@ data class ChunkPath(
 data class PathPropertiesImpl(
     val infra: RawSignalingInfra,
     val chunkPath: ChunkPath,
-    val pathRoutes: List<RouteId>?
+    val pathRoutes: List<RouteId>?,
 ) : PathProperties {
     override fun getSlopes(): DistanceRangeMap<Double> {
         return getRangeMap { dirChunkId -> infra.getTrackChunkSlope(dirChunkId) }
@@ -84,7 +84,10 @@ data class PathPropertiesImpl(
         return getRangeMap { dirChunkId -> infra.getTrackChunkNeutralSections(dirChunkId) }
     }
 
-    override fun getSpeedLimitProperties(trainTag: String?): DistanceRangeMap<SpeedLimitProperty> {
+    override fun getSpeedLimitProperties(
+        trainTag: String?,
+        temporarySpeedLimitManager: TemporarySpeedLimitManager?
+    ): DistanceRangeMap<SpeedLimitProperty> {
         assert(pathRoutes != null) {
             "the routes on a path should be set when attempting to compute a speed limit"
         }
@@ -103,7 +106,23 @@ data class PathPropertiesImpl(
             //           \
             // - start - - - commonChunk - ->
             val route = routeOnChunk.firstOrNull()?.let { routeId -> infra.getRouteName(routeId) }
-            infra.getTrackChunkSpeedLimitProperties(dirChunkId, trainTag, route)
+            val permanentSpeedLimits =
+                infra.getTrackChunkSpeedLimitProperties(dirChunkId, trainTag, route)
+            if (temporarySpeedLimitManager != null) {
+                temporarySpeedLimitManager.speedLimits[dirChunkId]?.let { applicableSpeedLimits ->
+                    permanentSpeedLimits.updateMap(
+                        applicableSpeedLimits,
+                        { s1, s2 ->
+                            if (s1.speed < s2.speed) {
+                                s1
+                            } else {
+                                s2
+                            }
+                        }
+                    )
+                }
+            }
+            permanentSpeedLimits
         }
     }
 
@@ -113,7 +132,8 @@ data class PathPropertiesImpl(
             if (zoneId != null) {
                 val chunkLength = infra.getTrackChunkLength(chunkId).distance
                 distanceRangeMapOf(
-                    listOf(DistanceRangeMap.RangeMapEntry(Distance.ZERO, chunkLength, zoneId))
+                    *listOf(DistanceRangeMap.RangeMapEntry(Distance.ZERO, chunkLength, zoneId))
+                        .toTypedArray()
                 )
             } else {
                 distanceRangeMapOf()

diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/RawInfraBuilder.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/RawInfraBuilder.kt
@@ -471,12 +471,10 @@ class RawInfraBuilder {
     ): TrackChunkId {
         val initSpeedSections = {
             distanceRangeMapOf(
-                listOf(
-                    DistanceRangeMap.RangeMapEntry(
-                        0.meters,
-                        length.distance,
-                        SpeedSection(Double.POSITIVE_INFINITY.metersPerSecond, mapOf(), mapOf())
-                    )
+                DistanceRangeMap.RangeMapEntry(
+                    0.meters,
+                    length.distance,
+                    SpeedSection(Double.POSITIVE_INFINITY.metersPerSecond, mapOf(), mapOf())
                 )
             )
         }
@@ -499,7 +497,8 @@ class RawInfraBuilder {
                 loadingGaugeConstraints,
                 // Electrifications will be filled later on
                 distanceRangeMapOf(
-                    listOf(DistanceRangeMap.RangeMapEntry(0.meters, length.distance, ""))
+                    *listOf(DistanceRangeMap.RangeMapEntry(0.meters, length.distance, ""))
+                        .toTypedArray()
                 ),
                 // NeutralSections will be filled later on
                 DirectionalMap(distanceRangeMapOf(), distanceRangeMapOf()),

diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/RawInfraImpl.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/RawInfraImpl.kt
@@ -216,6 +216,7 @@ class RawInfraImpl(
     private val dirDetExitToRouteMap: Map<DirDetectorId, StaticIdxList<Route>>,
 ) : RawInfra {
     private val zoneNameMap: HashMap<String, ZoneId> = HashMap()
+    private val detectorNameMap: HashMap<String, DetectorId> = HashMap()
     private val cachePerDirTrackChunk = IdxMap<DirTrackChunkId, MutableList<TrackChunkSignal>>()
     private val cachePerZonePath: StaticPool<ZonePath, ZonePathCache>
     private val trackChunksBounds =
@@ -266,6 +267,10 @@ class RawInfraImpl(
         return chunkDescriptor.offset + chunkDescriptor.length.distance
     }
 
+    override fun findDetector(detectorName: String): DetectorId? {
+        return detectorNameMap[detectorName]
+    }
+
     private fun findChunkOffset(
         trackSection: TrackSectionId,
         chunkIndex: Int,
@@ -296,6 +301,9 @@ class RawInfraImpl(
             if (nextZone != null) zoneDetectors[nextZone]!!.add(detector.increasing)
             val prevZone = getNextZone(detector.decreasing)
             if (prevZone != null) zoneDetectors[prevZone]!!.add(detector.decreasing)
+            for (detectorName in detectorPool[detector].names) {
+                detectorNameMap.put(detectorName, detector)
+            }
         }
 
         // initialize zone names

diff --git a/...-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/TemporarySpeedLimitManager.kt b/...-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/TemporarySpeedLimitManager.kt
@@ -0,0 +1,11 @@
+package fr.sncf.osrd.sim_infra.impl
+
+import fr.sncf.osrd.sim_infra.api.DirTrackChunkId
+import fr.sncf.osrd.sim_infra.api.SpeedLimitProperty
+import fr.sncf.osrd.utils.DistanceRangeMap
+
+class TemporarySpeedLimitManager(
+    val speedLimits: Map<DirTrackChunkId, DistanceRangeMap<SpeedLimitProperty>>,
+) {
+    constructor() : this(speedLimits = mutableMapOf()) {}
+}
diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/utils/PathPropertiesView.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/utils/PathPropertiesView.kt
@@ -2,6 +2,7 @@ package fr.sncf.osrd.sim_infra.utils
 
 import fr.sncf.osrd.geom.LineString
 import fr.sncf.osrd.sim_infra.api.*
+import fr.sncf.osrd.sim_infra.impl.TemporarySpeedLimitManager
 import fr.sncf.osrd.utils.DistanceRangeMap
 import fr.sncf.osrd.utils.units.Distance
 import fr.sncf.osrd.utils.units.Offset
@@ -56,8 +57,11 @@ data class PathPropertiesView(
         return sliceRangeMap(base.getNeutralSections())
     }
 
-    override fun getSpeedLimitProperties(trainTag: String?): DistanceRangeMap<SpeedLimitProperty> {
-        return sliceRangeMap(base.getSpeedLimitProperties(trainTag))
+    override fun getSpeedLimitProperties(
+        trainTag: String?,
+        temporarySpeedLimitManager: TemporarySpeedLimitManager?
+    ): DistanceRangeMap<SpeedLimitProperty> {
+        return sliceRangeMap(base.getSpeedLimitProperties(trainTag, temporarySpeedLimitManager))
     }
 
     override fun getZones(): DistanceRangeMap<ZoneId> {

diff --git a/core/kt-osrd-utils/src/main/kotlin/fr/sncf/osrd/utils/DistanceRangeMap.kt b/core/kt-osrd-utils/src/main/kotlin/fr/sncf/osrd/utils/DistanceRangeMap.kt
@@ -73,10 +73,8 @@ interface DistanceRangeMap<T> : Iterable<DistanceRangeMap.RangeMapEntry<T>> {
     fun clear()
 }
 
-fun <T> distanceRangeMapOf(
-    entries: List<DistanceRangeMap.RangeMapEntry<T>> = emptyList()
-): DistanceRangeMap<T> {
-    return DistanceRangeMapImpl(entries)
+fun <T> distanceRangeMapOf(vararg entries: DistanceRangeMap.RangeMapEntry<T>): DistanceRangeMap<T> {
+    return DistanceRangeMapImpl(entries.asList())
 }
 
 /**
@@ -107,7 +105,7 @@ fun <T> mergeDistanceRangeMaps(
     }
 
     // Build the whole map at once to avoid redundant computations.
-    return distanceRangeMapOf(resEntries)
+    return distanceRangeMapOf(*resEntries.toTypedArray())
 }
 
 /**

diff --git a/core/kt-osrd-utils/src/test/kotlin/fr/sncf/osrd/utils/TestDistanceRangeMap.kt b/core/kt-osrd-utils/src/test/kotlin/fr/sncf/osrd/utils/TestDistanceRangeMap.kt
@@ -21,7 +21,7 @@ class TestDistanceRangeMap {
         rangeMapMany.putMany(entries)
         assertEquals(expected, rangeMapMany.asList())
 
-        val rangeMapCtor = distanceRangeMapOf<T>(entries)
+        val rangeMapCtor = DistanceRangeMapImpl(entries)
         assertEquals(expected, rangeMapCtor.asList())
     }
 
@@ -247,7 +247,7 @@ class TestDistanceRangeMap {
 
         val mark3 = timeSource.markNow()
         val mark4 = mark3 + oneSecond
-        val rangeMapCtor = distanceRangeMapOf<Int>(entries)
+        val rangeMapCtor = DistanceRangeMapImpl(entries)
         assert(!mark4.hasPassedNow())
         assertEquals(entries, rangeMapCtor.asList())
     }
@@ -263,10 +263,8 @@ class TestDistanceRangeMap {
     fun testMergeDistanceRangeMapsSimple() {
         val inputMap =
             distanceRangeMapOf<Int>(
-                listOf(
-                    DistanceRangeMap.RangeMapEntry(Distance(0), Distance(50), 1),
-                    DistanceRangeMap.RangeMapEntry(Distance(50), Distance(100), 2),
-                )
+                DistanceRangeMap.RangeMapEntry(Distance(0), Distance(50), 1),
+                DistanceRangeMap.RangeMapEntry(Distance(50), Distance(100), 2),
             )
         val distances = listOf(Distance(100))
 
@@ -299,7 +297,7 @@ class TestDistanceRangeMap {
                         i * n + it
                     )
                 }
-            maps.add(distanceRangeMapOf<Int>(entries))
+            maps.add(DistanceRangeMapImpl<Int>(entries))
         }
         val mergedEntries =
             List(n * n) {

diff --git a/...osrd-railjson/src/main/java/fr/sncf/osrd/railjson/schema/common/RJSTemporarySpeedLimit.kt b/...osrd-railjson/src/main/java/fr/sncf/osrd/railjson/schema/common/RJSTemporarySpeedLimit.kt
@@ -0,0 +1,10 @@
+package fr.sncf.osrd.railjson.schema.common
+
+import com.squareup.moshi.Json
+import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSDirectionalTrackRange
+
+data class RJSTemporarySpeedLimit(
+    @Json(name = "speed_limit") val speedLimit: Double,
+    @Json(name = "track_ranges") val trackRanges: List<RJSDirectionalTrackRange>,
+)
+// TODO delete me
diff --git a/core/src/main/java/fr/sncf/osrd/standalone_sim/StandaloneSim.java b/core/src/main/java/fr/sncf/osrd/standalone_sim/StandaloneSim.java
@@ -61,8 +61,8 @@ public static StandaloneSimResult run(
                 var rollingStock = trainSchedule.rollingStock;
 
                 // MRSP & SpeedLimits
-                var mrsp = computeMRSP(trainPath, rollingStock, true, trainSchedule.tag, null);
-                var speedLimits = computeMRSP(trainPath, rollingStock, false, trainSchedule.tag, null);
+                var mrsp = computeMRSP(trainPath, rollingStock, true, trainSchedule.tag, null, null);
+                var speedLimits = computeMRSP(trainPath, rollingStock, false, trainSchedule.tag, null, null);
                 mrsp = driverBehaviour.applyToMRSP(mrsp);
                 cacheSpeedLimits.put(trainSchedule, ResultEnvelopePoint.from(speedLimits));
 

diff --git a/core/src/main/kotlin/fr/sncf/osrd/api/api_v2/pathfinding/PathfindingBlocksEndpointV2.kt b/core/src/main/kotlin/fr/sncf/osrd/api/api_v2/pathfinding/PathfindingBlocksEndpointV2.kt
@@ -198,7 +198,14 @@ private fun getStartLocations(
     val firstStep = waypoints[0]
     val stops = listOf(waypoints.last())
     for (location in firstStep) {
-        val infraExplorers = initInfraExplorer(rawInfra, blockInfra, location, stops, constraints)
+        val infraExplorers =
+            initInfraExplorer(
+                rawInfra,
+                blockInfra,
+                location,
+                stops = stops,
+                constraints = constraints
+            )
         val extended = infraExplorers.flatMap { extendLookaheadUntil(it, 1) }
         for (explorer in extended) {
             val edge = PathfindingEdge(explorer)