core: migrate stdcm to new infra

Change summary:
* Migrate train simulation code to work with the new infra interfaces
* Migrate STDCM to work with the new infra interfaces
* Run the STDCM pathfinding on blocks instead of routes
* Migrate tests, using a new class to build simplified infras
* Cleanup some legacy code (not all of it)

Author: Erashin

core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/EnvelopeSimContext.java

@@ -18,6 +18,7 @@ public EnvelopeSimContext(
         this.rollingStock = rollingStock;
         this.path = path;
         this.timeStep = timeStep;
+        assert tractiveEffortCurveMap != null;
         this.tractiveEffortCurveMap = tractiveEffortCurveMap;
     }
 

core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/TrainPhysicsIntegrator.java

@@ -9,9 +9,11 @@
  * the next position and speed of the train.
  */
 public final class TrainPhysicsIntegrator {
-    // a position delta lower than this value will be considered zero
-    public static final double POSITION_EPSILON = 1E-6;
-    // a speed lower than this value will be considered zero
+    // A position delta lower than this value will be considered zero
+    // Going back and forth with Distance and double (meters) often causes 1e-3 errors,
+    // we need the tolerance to be higher than this
+    public static final double POSITION_EPSILON = 1E-2;
+    // A speed lower than this value will be considered zero
     public static final double SPEED_EPSILON = 1E-6;
 
     private final PhysicsRollingStock rollingStock;

core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_utils/RangeMapUtils.java

@@ -34,7 +34,7 @@ public static <T> RangeMap<Double, T> mergeRanges(RangeMap<Double, T> map) {
     }
 
     /**
-     * Return the first map updated with another, using a merge function to fuse the values of intersecting ranges
+     * Returns the first map updated with another, using a merge function to fuse the values of intersecting ranges
      */
     public static <T, U> TreeRangeMap<Double, T> updateRangeMap(RangeMap<Double, T> map, RangeMap<Double, U> update,
                                              BiFunction<T, U, T> mergeFunction) {

core/kt-osrd-sim-infra/build.gradle

@@ -18,6 +18,7 @@ dependencies {
     // PLEASE ADD AND UPDATE DEPENDENCIES USING libs.versions.toml
     implementation project(':kt-fast-collections')
     implementation project(path: ':osrd-geom')
+    implementation project(path: ':envelope-sim')
     ksp project(':kt-fast-collections-generator')
 
     api project(":kt-osrd-utils")

core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/InterlockingInfra.kt

@@ -85,16 +85,22 @@ interface RoutingInfra : ReservationInfra {
     fun getChunksOnRoute(route: RouteId): DirStaticIdxList<TrackChunk>
     @JvmName("getRoutesOnTrackChunk")
     fun getRoutesOnTrackChunk(trackChunk: DirTrackChunkId): StaticIdxList<Route>
+    @JvmName("getRoutesStartingAtDet")
+    fun getRoutesStartingAtDet(dirDetector: DirDetectorId): StaticIdxList<Route>
+    @JvmName("getRoutesEndingAtDet")
+    fun getRoutesEndingAtDet(dirDetector: DirDetectorId): StaticIdxList<Route>
 }
 
 fun ReservationInfra.findZonePath(entry: DirDetectorId, exit: DirDetectorId): ZonePathId? {
     return findZonePath(entry, exit, mutableStaticIdxArrayListOf(), mutableStaticIdxArrayListOf())
 }
 
+@JvmName("getRouteEntry")
 fun RoutingInfra.getRouteEntry(route: RouteId): DirDetectorId {
     return getZonePathEntry(getRoutePath(route).first())
 }
 
+@JvmName("getRouteExit")
 fun RoutingInfra.getRouteExit(route: RouteId): DirDetectorId {
     return getZonePathExit(getRoutePath(route).last())
 }

core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/PathProperties.kt

@@ -1,13 +1,17 @@
 package fr.sncf.osrd.sim_infra.api
 
 import fr.sncf.osrd.geom.LineString
+import fr.sncf.osrd.reporting.exceptions.ErrorType
+import fr.sncf.osrd.reporting.exceptions.OSRDError
 import fr.sncf.osrd.sim_infra.impl.NeutralSection
 import fr.sncf.osrd.sim_infra.impl.PathPropertiesImpl
+import fr.sncf.osrd.utils.DistanceRangeMap
 import fr.sncf.osrd.utils.indexing.DirStaticIdxList
 import fr.sncf.osrd.utils.indexing.StaticIdx
-import fr.sncf.osrd.utils.units.Distance
-import fr.sncf.osrd.utils.DistanceRangeMap
 import fr.sncf.osrd.utils.indexing.mutableDirStaticIdxArrayListOf
+import fr.sncf.osrd.utils.units.Distance
+import fr.sncf.osrd.utils.units.Offset
+import fr.sncf.osrd.utils.units.Speed
 import fr.sncf.osrd.utils.units.meters
 
 data class IdxWithOffset<T>(
@@ -21,7 +25,7 @@ data class TrackLocation(
     @get:JvmName("getTrackId")
     val trackId: TrackSectionId,
     @get:JvmName("getOffset")
-    val offset: Distance
+    val offset: Offset<TrackSection>
 )
 
 @Suppress("INAPPLICABLE_JVM_NAME")
@@ -36,11 +40,27 @@ interface PathProperties {
     fun getCatenary(): DistanceRangeMap<String>
     @JvmName("getNeutralSections")
     fun getNeutralSections(): DistanceRangeMap<NeutralSection>
+    @JvmName("getSpeedLimits")
+    fun getSpeedLimits(trainTag: String?): DistanceRangeMap<Speed>
     @JvmName("getLength")
     fun getLength(): Distance
-
     @JvmName("getTrackLocationAtOffset")
     fun getTrackLocationAtOffset(pathOffset: Distance): TrackLocation
+    fun getElectricalProfiles(mapping: HashMap<String, DistanceRangeMap<String>>): DistanceRangeMap<String>
+    fun getTrackLocationOffset(location: TrackLocation): Distance?
+
+    val chunks: DirStaticIdxList<TrackChunk>
+    /** Returns the offset where the train starts (must be located on the first chunk) */
+    val beginOffset: Distance
+}
+
+/** Wraps the method without returning an optional, which can't be handled in java for value types */
+@JvmName("getTrackLocationOffsetOrThrow")
+fun PathProperties.getTrackLocationOffsetOrThrow(location: TrackLocation): Distance {
+    val res = getTrackLocationOffset(location)
+    if (res != null)
+        return res
+    throw OSRDError(ErrorType.InvalidScheduleTrackLocationNotIncludedInPath)
 }
 
 /** Build a Path from chunks and offsets, filtering the chunks outside the offsets */
@@ -73,3 +93,9 @@ fun buildPathPropertiesFrom(
     }
     return PathPropertiesImpl(infra, filteredChunks, mutBeginOffset, mutEndOffset)
 }
+
+/** For java interoperability purpose */
+@JvmName("makeTrackLocation")
+fun makeTrackLocation(track: TrackSectionId, offset: Offset<TrackSection>): TrackLocation {
+    return TrackLocation(track, offset)
+}

core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/RawSignalingInfra.kt

@@ -44,4 +44,14 @@ fun RawSignalingInfra.getLogicalSignalName(signal: LogicalSignalId): String? {
     return getPhysicalSignalName(getPhysicalSignal(signal))
 }
 
+/** Returns the length of a route  */
+@JvmName("getRouteLength")
+fun RawSignalingInfra.getRouteLength(route: RouteId): Distance {
+    var res = 0.meters
+    for (zonePath in getRoutePath(route)) {
+        res += getZonePathLength(zonePath).distance
+    }
+    return res
+}
+
 typealias RawInfra = RawSignalingInfra

core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/TrackProperties.kt

@@ -39,6 +39,9 @@ interface TrackProperties {
     fun getTrackChunkSpeedSections(trackChunk: DirTrackChunkId, trainTag: String?): DistanceRangeMap<Speed>
     @JvmName("getTrackChunkGeom")
     fun getTrackChunkGeom(trackChunk: TrackChunkId): LineString
+    fun getTrackChunkElectricalProfile(
+        trackChunk: TrackChunkId, mapping: HashMap<String, DistanceRangeMap<String>>
+    ): DistanceRangeMap<String>
 
     // Operational points
     fun getTrackChunkOperationalPointParts(trackChunk: TrackChunkId): StaticIdxList<OperationalPointPart>

core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/BlockInfraImpl.kt

@@ -3,7 +3,8 @@ package fr.sncf.osrd.sim_infra.impl
 import fr.sncf.osrd.sim_infra.api.*
 import fr.sncf.osrd.utils.Direction
 import fr.sncf.osrd.utils.indexing.*
-import fr.sncf.osrd.utils.units.*
+import fr.sncf.osrd.utils.units.Length
+import fr.sncf.osrd.utils.units.OffsetList
 
 class BlockDescriptor(
     val length: Length<Block>,
@@ -121,3 +122,21 @@ class BlockInfraImpl(
         return blockPool[block].length
     }
 }
+
+@JvmName("getBlockEntry")
+fun BlockInfra.getRouteEntry(rawInfra: RawInfra, block: BlockId): DirDetectorId {
+    val blockPath: StaticIdxList<ZonePath> = getBlockPath(block)
+    val firstZone: ZonePathId = blockPath[0]
+    return rawInfra.getZonePathEntry(firstZone)
+}
+
+@JvmName("getBlockExit")
+fun BlockInfra.getRouteExit(rawInfra: RawInfra, block: BlockId): DirDetectorId {
+    val blockPath: StaticIdxList<ZonePath> = getBlockPath(block)
+    val lastZonePath: ZonePathId = blockPath[blockPath.size - 1]
+    return rawInfra.getZonePathExit(lastZonePath)
+}
+
+fun BlockInfra.getTrackChunksFromBlocks(blocks: List<BlockId>): List<DirStaticIdx<TrackChunk>> {
+    return blocks.flatMap { getTrackChunksFromBlock(it) }
+}

core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/DebugViewers.kt

@@ -4,9 +4,9 @@ import fr.sncf.osrd.sim_infra.api.*
 import fr.sncf.osrd.sim_infra.utils.recoverBlocks
 import fr.sncf.osrd.sim_infra.utils.toList
 import fr.sncf.osrd.utils.Direction
+import fr.sncf.osrd.utils.indexing.DirStaticIdx
 import fr.sncf.osrd.utils.indexing.MutableStaticIdxArrayList
 import fr.sncf.osrd.utils.indexing.StaticIdx
-import fr.sncf.osrd.utils.indexing.StaticIdxList
 import fr.sncf.osrd.utils.units.Distance
 import fr.sncf.osrd.utils.units.Length
 import fr.sncf.osrd.utils.units.Offset
@@ -15,6 +15,7 @@ import fr.sncf.osrd.utils.units.Offset
  * when using a debugger. They can be used to create watches containing the object properties. */
 
 data class DirectedViewer<T>(
+    val rawId: UInt,
     val direction: Direction,
     val value: T,
 )
@@ -34,6 +35,9 @@ data class ZonePathViewer(
 data class BlockViewer(
     val zones: List<ZonePathViewer>,
     val id: BlockId,
+    val entry: DirectedViewer<String>,
+    val exit: DirectedViewer<String>,
+    val length: Length<Block>,
 )
 
 data class RouteViewer(
@@ -42,6 +46,10 @@ data class RouteViewer(
     val id: RouteId,
 )
 
+fun <T, U> makeDirViewer(id: DirStaticIdx<T>, value: U): DirectedViewer<U> {
+    return DirectedViewer(id.data, id.direction, value)
+}
+
 @JvmName("makeChunk")
 fun makeChunk(infra: RawInfra, id: StaticIdx<TrackChunk>): ChunkViewer {
     return ChunkViewer(
@@ -52,36 +60,45 @@ fun makeChunk(infra: RawInfra, id: StaticIdx<TrackChunk>): ChunkViewer {
     )
 }
 
+@JvmName("makeDirChunk")
+fun makeDirChunk(infra: RawInfra, id: DirStaticIdx<TrackChunk>): DirectedViewer<ChunkViewer> {
+    return makeDirViewer(id, makeChunk(infra, id.value))
+}
+
 @JvmName("makeZonePath")
 fun makeZonePath(infra: RawInfra, id: StaticIdx<ZonePath>): ZonePathViewer {
     return ZonePathViewer(
         infra.getZonePathChunks(id)
-            .map { dirChunk -> DirectedViewer(dirChunk.direction, makeChunk(infra, dirChunk.value)) },
+            .map { dirChunk -> makeDirViewer(dirChunk, makeChunk(infra, dirChunk.value)) },
         id,
     )
 }
 
 @JvmName("makeBlock")
 fun makeBlock(rawInfra: RawInfra, blockInfra: BlockInfra, id: StaticIdx<Block>): BlockViewer {
+    val entry = rawInfra.getZonePathEntry(blockInfra.getBlockPath(id)[0])
+    val exit = rawInfra.getZonePathExit(blockInfra.getBlockPath(id).last())
     return BlockViewer(
         blockInfra.getBlockPath(id)
             .map { path -> makeZonePath(rawInfra, path) },
         id,
+        makeDirViewer(entry, rawInfra.getDetectorName(entry.value)!!),
+        makeDirViewer(exit, rawInfra.getDetectorName(exit.value)!!),
+        blockInfra.getBlockLength(id),
     )
 }
 
 @JvmName("makeRoute")
 fun makeRoute(
     rawInfra: RawInfra,
     blockInfra: BlockInfra,
-    signalingSystems: StaticIdxList<SignalingSystem>,
     id: StaticIdx<Route>,
 ): RouteViewer {
     val ids = MutableStaticIdxArrayList<Route>()
     ids.add(id)
     return RouteViewer(
         rawInfra.getRouteName(id)!!,
-        recoverBlocks(rawInfra, blockInfra, ids, signalingSystems)[0]
+        recoverBlocks(rawInfra, blockInfra, ids, null)[0]
             .toList()
             .map { blockPathElement -> blockPathElement.block }
             .map { block -> makeBlock(rawInfra, blockInfra, block) },

core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt

@@ -1,20 +1,29 @@
 package fr.sncf.osrd.sim_infra.impl
 
 import fr.sncf.osrd.geom.LineString
-import fr.sncf.osrd.sim_infra.api.*
+import fr.sncf.osrd.sim_infra.api.DirTrackChunkId
+import fr.sncf.osrd.sim_infra.api.IdxWithOffset
+import fr.sncf.osrd.sim_infra.api.LoadingGaugeConstraint
+import fr.sncf.osrd.sim_infra.api.OperationalPointPart
+import fr.sncf.osrd.sim_infra.api.PathProperties
+import fr.sncf.osrd.sim_infra.api.TrackChunk
+import fr.sncf.osrd.sim_infra.api.TrackChunkId
+import fr.sncf.osrd.sim_infra.api.TrackLocation
+import fr.sncf.osrd.sim_infra.api.TrackProperties
 import fr.sncf.osrd.utils.Direction
 import fr.sncf.osrd.utils.DistanceRangeMap
 import fr.sncf.osrd.utils.distanceRangeMapOf
 import fr.sncf.osrd.utils.indexing.DirStaticIdxList
 import fr.sncf.osrd.utils.units.Distance
+import fr.sncf.osrd.utils.units.Offset
+import fr.sncf.osrd.utils.units.Speed
 import fr.sncf.osrd.utils.units.meters
-import java.lang.RuntimeException
 
 data class PathPropertiesImpl(
     val infra: TrackProperties,
-    val chunks: DirStaticIdxList<TrackChunk>,
+    override val chunks: DirStaticIdxList<TrackChunk>,
     @get:JvmName("getBeginOffset")
-    val beginOffset: Distance,
+    override val beginOffset: Distance,
     @get:JvmName("getEndOffset")
     val endOffset: Distance,
 ) : PathProperties {
@@ -53,6 +62,10 @@ data class PathPropertiesImpl(
         return getRangeMap { dirChunkId -> infra.getTrackChunkNeutralSections(dirChunkId) }
     }
 
+    override fun getSpeedLimits(trainTag: String?): DistanceRangeMap<Speed> {
+        return getRangeMap { dirChunkId -> infra.getTrackChunkSpeedSections(dirChunkId, trainTag) }
+    }
+
     override fun getLength(): Distance {
         return endOffset - beginOffset
     }
@@ -67,15 +80,26 @@ data class PathPropertiesImpl(
                 val startChunkOffset = infra.getTrackChunkOffset(chunk.value).distance
                 val offsetOnChunk = offset - lengthPrevChunks
                 return if (chunk.direction == Direction.INCREASING)
-                    TrackLocation(trackId, offsetOnChunk + startChunkOffset)
+                    TrackLocation(trackId, Offset(offsetOnChunk + startChunkOffset))
                 else
-                    TrackLocation(trackId, startChunkOffset + chunkLength - offsetOnChunk)
+                    TrackLocation(trackId, Offset(startChunkOffset + chunkLength - offsetOnChunk))
             }
             lengthPrevChunks += chunkLength
         }
         throw RuntimeException("The given path offset is larger than the path length")
     }
 
+    override fun getElectricalProfiles(mapping: HashMap<String, DistanceRangeMap<String>>): DistanceRangeMap<String> {
+        return getRangeMapFromUndirected { chunkId -> infra.getTrackChunkElectricalProfile(chunkId, mapping) }
+    }
+
+    override fun getTrackLocationOffset(location: TrackLocation): Distance? {
+        val offset = getOffsetOfTrackLocationOnChunks(infra, location, chunks) ?: return null
+        if (offset < beginOffset || offset > endOffset)
+            return null
+        return offset - beginOffset
+    }
+
     private fun projectLineString(getData: (chunkId: TrackChunkId) -> LineString): LineString {
         fun getDirData(dirChunkId: DirTrackChunkId): LineString {
             val data = getData(dirChunkId.value)
@@ -201,3 +225,37 @@ data class PathPropertiesImpl(
         return res
     }
 }
+
+/** Returns the offset of a location on a given list of chunks */
+fun getOffsetOfTrackLocationOnChunks(
+    infra: TrackProperties,
+    location: TrackLocation,
+    chunks: DirStaticIdxList<TrackChunk>,
+): Distance? {
+    var offsetAfterFirstChunk = 0.meters
+    for (dirChunk in chunks) {
+        val chunkLength = infra.getTrackChunkLength(dirChunk.value)
+        if (location.trackId == infra.getTrackFromChunk(dirChunk.value)) {
+            val chunkOffset = infra.getTrackChunkOffset(dirChunk.value)
+            if (chunkOffset <= location.offset && location.offset <= (chunkOffset + chunkLength.distance)) {
+                val distanceToChunkStart = if (dirChunk.direction == Direction.INCREASING)
+                    location.offset - chunkOffset
+                else
+                    (chunkOffset + chunkLength.distance) - location.offset
+                return offsetAfterFirstChunk + distanceToChunkStart
+            }
+        }
+        offsetAfterFirstChunk += chunkLength.distance
+    }
+    return null
+}
+
+/** Returns the offset of a location on a given list of chunks, throws if not found */
+@JvmName("getOffsetOfTrackLocationOnChunksOrThrow")
+fun getOffsetOfTrackLocationOnChunksOrThrow(
+    infra: TrackProperties,
+    location: TrackLocation,
+    chunks: DirStaticIdxList<TrackChunk>,
+): Distance {
+    return getOffsetOfTrackLocationOnChunks(infra, location, chunks) ?: throw RuntimeException()
+}