core: wip on explainable constraints

core/kt-osrd-signaling/src/main/kotlin/fr/sncf/osrd/signaling/impl/MockSigSystemManager.kt

@@ -1,8 +1,22 @@
 package fr.sncf.osrd.signaling.impl
 
-import fr.sncf.osrd.signaling.*
-import fr.sncf.osrd.sim_infra.api.*
+import fr.sncf.osrd.signaling.BlockDiagReporter
+import fr.sncf.osrd.signaling.MovementAuthorityView
+import fr.sncf.osrd.signaling.SigBlock
+import fr.sncf.osrd.signaling.SigSystemManager
+import fr.sncf.osrd.signaling.SignalDiagReporter
+import fr.sncf.osrd.signaling.SignalingTrainState
+import fr.sncf.osrd.signaling.SpeedLimitView
+import fr.sncf.osrd.sim_infra.api.SigParameters
+import fr.sncf.osrd.sim_infra.api.SigParametersSchema
+import fr.sncf.osrd.sim_infra.api.SigSettings
+import fr.sncf.osrd.sim_infra.api.SigSettingsSchema
+import fr.sncf.osrd.sim_infra.api.SigState
+import fr.sncf.osrd.sim_infra.api.SigStateSchema
 import fr.sncf.osrd.sim_infra.api.SignalDriver
+import fr.sncf.osrd.sim_infra.api.SignalDriverId
+import fr.sncf.osrd.sim_infra.api.SignalingSystem
+import fr.sncf.osrd.sim_infra.api.SignalingSystemId
 import fr.sncf.osrd.utils.indexing.StaticIdxSpace
 
 class MockSigSystemManager(
@@ -62,6 +76,10 @@ class MockSigSystemManager(
         return parametersSchema
     }
 
+    override fun getName(sigSystem: SignalingSystemId): String {
+        return this.sigSystem
+    }
+
     override val drivers: StaticIdxSpace<SignalDriver>
         get() = StaticIdxSpace(1u)
 

core/kt-osrd-signaling/src/main/kotlin/fr/sncf/osrd/signaling/impl/SigSystemManagerImpl.kt

@@ -1,8 +1,24 @@
 package fr.sncf.osrd.signaling.impl
 
-import fr.sncf.osrd.signaling.*
-import fr.sncf.osrd.sim_infra.api.*
+import fr.sncf.osrd.signaling.BlockDiagReporter
+import fr.sncf.osrd.signaling.MovementAuthorityView
+import fr.sncf.osrd.signaling.SigBlock
+import fr.sncf.osrd.signaling.SigSystemManager
+import fr.sncf.osrd.signaling.SignalDiagReporter
+import fr.sncf.osrd.signaling.SignalingSystemDriver
+import fr.sncf.osrd.signaling.SignalingTrainState
+import fr.sncf.osrd.signaling.SpeedLimitView
+import fr.sncf.osrd.sim_infra.api.SigParameters
+import fr.sncf.osrd.sim_infra.api.SigParametersSchema
+import fr.sncf.osrd.sim_infra.api.SigSettings
+import fr.sncf.osrd.sim_infra.api.SigSettingsSchema
+import fr.sncf.osrd.sim_infra.api.SigState
+import fr.sncf.osrd.sim_infra.api.SigStateSchema
 import fr.sncf.osrd.sim_infra.api.SignalDriver
+import fr.sncf.osrd.sim_infra.api.SignalDriverId
+import fr.sncf.osrd.sim_infra.api.SignalingSystem
+import fr.sncf.osrd.sim_infra.api.SignalingSystemId
+import fr.sncf.osrd.sim_infra.api.findSignalingSystemOrThrow
 import fr.sncf.osrd.utils.indexing.StaticIdxSpace
 import fr.sncf.osrd.utils.indexing.StaticPool
 
@@ -69,6 +85,10 @@ class SigSystemManagerImpl : SigSystemManager {
         return sigSystemPool[sigSystem].parametersSchema
     }
 
+    override fun getName(sigSystem: SignalingSystemId): String {
+        return sigSystemPool[sigSystem].id
+    }
+
     override val drivers
         get() = driverPool.space()
 

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

@@ -3,8 +3,14 @@ package fr.sncf.osrd.sim_infra.api
 import fr.sncf.osrd.reporting.exceptions.OSRDError
 import fr.sncf.osrd.sim_infra.impl.SignalParameters
 import fr.sncf.osrd.utils.Direction
-import fr.sncf.osrd.utils.indexing.*
-import fr.sncf.osrd.utils.units.*
+import fr.sncf.osrd.utils.indexing.DirStaticIdxList
+import fr.sncf.osrd.utils.indexing.MutableStaticIdxArraySet
+import fr.sncf.osrd.utils.indexing.StaticIdx
+import fr.sncf.osrd.utils.indexing.StaticIdxList
+import fr.sncf.osrd.utils.indexing.StaticIdxSpace
+import fr.sncf.osrd.utils.indexing.mutableStaticIdxArrayListOf
+import fr.sncf.osrd.utils.units.Length
+import fr.sncf.osrd.utils.units.OffsetList
 
 /** A type of signaling system, which is used both for blocks and signals */
 sealed interface SignalingSystem
@@ -45,6 +51,8 @@ interface InfraSigSystemManager {
 
     fun getParametersSchema(sigSystem: SignalingSystemId): SigParametersSchema
 
+    fun getName(sigSystem: SignalingSystemId): String
+
     val drivers: StaticIdxSpace<SignalDriver>
 
     fun findDriver(outputSig: SignalingSystemId, inputSig: SignalingSystemId): SignalDriverId

core/src/main/kotlin/fr/sncf/osrd/api/api_v2/pathfinding/PathfindingBlocksEndpointV2.kt

@@ -5,6 +5,7 @@ import fr.sncf.osrd.api.FullInfra
 import fr.sncf.osrd.api.InfraManager
 import fr.sncf.osrd.api.api_v2.TrackLocation
 import fr.sncf.osrd.api.pathfinding.constraints.*
+import fr.sncf.osrd.api.pathfinding.makePathProps
 import fr.sncf.osrd.conflicts.TravelledPath
 import fr.sncf.osrd.graph.*
 import fr.sncf.osrd.graph.Pathfinding.EdgeLocation
@@ -15,6 +16,7 @@ import fr.sncf.osrd.reporting.warnings.DiagnosticRecorderImpl
 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.Distance
 import fr.sncf.osrd.utils.units.Length
 import fr.sncf.osrd.utils.units.Offset
 import fr.sncf.osrd.utils.units.meters
@@ -99,8 +101,8 @@ private fun initConstraintsFromRSProps(
     rollingStockLoadingGauge: RJSLoadingGaugeType,
     rollingStockSupportedElectrification: List<String>,
     rollingStockSupportedSignalingSystems: List<String>
-): List<PathfindingConstraint<Block>> {
-    val res = mutableListOf<PathfindingConstraint<Block>>()
+): List<ExplainablePathfindingConstraint<Block>> {
+    val res = mutableListOf<ExplainablePathfindingConstraint<Block>>()
     if (!rollingStockIsThermal) {
         res.add(
             ElectrificationConstraints(
@@ -123,7 +125,7 @@ private fun initConstraintsFromRSProps(
 private fun computePaths(
     infra: FullInfra,
     waypoints: ArrayList<Collection<PathfindingEdgeLocationId<Block>>>,
-    constraints: List<PathfindingConstraint<Block>>,
+    constraints: List<ExplainablePathfindingConstraint<Block>>,
     remainingDistanceEstimators: List<AStarHeuristicId<Block>>,
     timeout: Double?
 ): PathfindingResultId<Block> {
@@ -152,38 +154,76 @@ private fun computePaths(
         val elecRanges = mutableListOf<RangeValue<String>>()
         val gaugeRanges = mutableListOf<Pathfinding.Range<TravelledPath>>()
         val signalRanges = mutableListOf<RangeValue<String>>()
-        var travelledPathBlockStart =
-            Offset<TravelledPath>(
-                -possiblePathWithoutErrorNoConstraints.ranges.first().start.distance
+
+        // TODO: combine runPathfindingPostProcessing and makePathProps
+        // val oldRoutePath = makeRoutePath(infra.blockInfra, infra.rawInfra,
+        // possiblePathWithoutErrorNoConstraints.ranges)
+        // val routeList = oldRoutePath.map { it.route }
+        val blockList = possiblePathWithoutErrorNoConstraints.ranges.map { it.edge }
+        val pathProps =
+            makePathProps(
+                infra.rawInfra,
+                infra.blockInfra,
+                blockList,
+                Offset(possiblePathWithoutErrorNoConstraints.ranges.first().start.distance)
             )
-        for (range in possiblePathWithoutErrorNoConstraints.ranges) {
-            for (currentConstraint in constraints) {
-                for (blockedRange in currentConstraint.apply(range.edge)) {
-                    if (blockedRange.end < range.start || range.end < blockedRange.start) {
-                        // The blocked range is outside the used part
-                        continue
+        val travelledPathLength =
+            Distance(
+                possiblePathWithoutErrorNoConstraints.ranges.sumOf {
+                    (it.end - it.start).millimeters
+                }
+            )
+        for (currentConstraint in constraints) {
+            val blockedRanges =
+                currentConstraint
+                    .getBlockedRanges(pathProps)
+                    .filter { it.start.distance <= travelledPathLength }
+                    .map {
+                        Pathfinding.Range(it.start, Offset.min(it.end, Offset(travelledPathLength)))
                     }
-                    val range =
-                        Pathfinding.Range(
-                            travelledPathBlockStart +
-                                Offset.max(blockedRange.start, range.start).distance,
-                            travelledPathBlockStart +
-                                Offset.min(blockedRange.end, range.end).distance
+            when (currentConstraint) {
+                is ElectrificationConstraints -> {
+                    for (blockedRange in blockedRanges) {
+                        val pathElec = pathProps.getElectrification()
+                        pathElec.truncate(blockedRange.start.distance, blockedRange.end.distance)
+                        elecRanges.addAll(
+                            pathElec.map {
+                                RangeValue(
+                                    Pathfinding.Range(Offset(it.lower), Offset(it.upper)),
+                                    it.value
+                                )
+                            }
+                        )
+                    }
+                }
+                is LoadingGaugeConstraints -> {
+                    for (blockedRange in blockedRanges) {
+                        val pathGauge = pathProps.getLoadingGauge()
+                        pathGauge.truncate(blockedRange.start.distance, blockedRange.end.distance)
+                        gaugeRanges.addAll(
+                            pathGauge.map { Pathfinding.Range(Offset(it.lower), Offset(it.upper)) }
                         )
-                    when (currentConstraint::class.java) {
-                        ElectrificationConstraints::class.java -> {
-                            elecRanges.add(RangeValue(range, "elec"))
-                        }
-                        LoadingGaugeConstraints::class.java -> {
-                            gaugeRanges.add(range)
-                        }
-                        SignalingSystemConstraints::class.java -> {
-                            signalRanges.add(RangeValue(range, "signal"))
-                        }
                     }
                 }
+                is SignalingSystemConstraints -> {
+                    var blockedValues =
+                        currentConstraint.getBlockedRangeValues(
+                            blockList,
+                            Offset(
+                                possiblePathWithoutErrorNoConstraints.ranges.first().start.distance
+                            )
+                        )
+                    blockedValues.truncate(Distance.ZERO, travelledPathLength)
+                    signalRanges.addAll(
+                        blockedValues.asList().map {
+                            RangeValue(
+                                Pathfinding.Range(Offset(it.lower), Offset(it.upper)),
+                                infra.signalingSimulator.sigModuleManager.getName(it.value)
+                            )
+                        }
+                    )
+                }
             }
-            travelledPathBlockStart += infra.blockInfra.getBlockLength(range.edge).distance
         }
 
         if (elecRanges.isNotEmpty() || gaugeRanges.isNotEmpty() || signalRanges.isNotEmpty()) {

core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/constraints/ElectrificationConstraints.kt

@@ -4,9 +4,13 @@ import com.google.common.collect.Range
 import com.google.common.collect.RangeSet
 import com.google.common.collect.TreeRangeSet
 import fr.sncf.osrd.api.pathfinding.makePathProps
+import fr.sncf.osrd.graph.ExplainablePathfindingConstraint
 import fr.sncf.osrd.graph.Pathfinding
-import fr.sncf.osrd.graph.PathfindingConstraint
-import fr.sncf.osrd.sim_infra.api.*
+import fr.sncf.osrd.sim_infra.api.Block
+import fr.sncf.osrd.sim_infra.api.BlockId
+import fr.sncf.osrd.sim_infra.api.BlockInfra
+import fr.sncf.osrd.sim_infra.api.PathProperties
+import fr.sncf.osrd.sim_infra.api.RawSignalingInfra
 import fr.sncf.osrd.utils.DistanceRangeMap
 import fr.sncf.osrd.utils.units.Distance
 import fr.sncf.osrd.utils.units.Offset
@@ -16,12 +20,16 @@ data class ElectrificationConstraints(
     val blockInfra: BlockInfra,
     val rawInfra: RawSignalingInfra,
     val compatibleElectrification: Collection<String>
-) : PathfindingConstraint<Block> {
+) : ExplainablePathfindingConstraint<Block> {
     override fun apply(edge: BlockId): Collection<Pathfinding.Range<Block>> {
         val path = makePathProps(blockInfra, rawInfra, edge)
         return getBlockedRanges(path, compatibleElectrification)
     }
 
+    override fun getBlockedRanges(path: PathProperties): Collection<Pathfinding.Range<Block>> {
+        return getBlockedRanges(path, compatibleElectrification)
+    }
+
     companion object {
         /**
          * Returns the sections of the given block that can't be used by the given rolling stock

core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/constraints/LoadingGaugeConstraints.kt

@@ -1,10 +1,16 @@
 package fr.sncf.osrd.api.pathfinding.constraints
 
 import fr.sncf.osrd.api.pathfinding.makePathProps
+import fr.sncf.osrd.graph.ExplainablePathfindingConstraint
 import fr.sncf.osrd.graph.Pathfinding
-import fr.sncf.osrd.graph.PathfindingConstraint
 import fr.sncf.osrd.railjson.schema.rollingstock.RJSLoadingGaugeType
-import fr.sncf.osrd.sim_infra.api.*
+import fr.sncf.osrd.sim_infra.api.Block
+import fr.sncf.osrd.sim_infra.api.BlockId
+import fr.sncf.osrd.sim_infra.api.BlockInfra
+import fr.sncf.osrd.sim_infra.api.LoadingGaugeConstraint
+import fr.sncf.osrd.sim_infra.api.LoadingGaugeTypeId
+import fr.sncf.osrd.sim_infra.api.PathProperties
+import fr.sncf.osrd.sim_infra.api.RawSignalingInfra
 import fr.sncf.osrd.utils.DistanceRangeMap
 import fr.sncf.osrd.utils.units.Offset
 import java.util.stream.Collectors
@@ -13,14 +19,18 @@ data class LoadingGaugeConstraints(
     val blockInfra: BlockInfra,
     val infra: RawSignalingInfra,
     val loadingGaugeType: RJSLoadingGaugeType
-) : PathfindingConstraint<Block> {
+) : ExplainablePathfindingConstraint<Block> {
     override fun apply(edge: BlockId): Collection<Pathfinding.Range<Block>> {
         val res = HashSet<Pathfinding.Range<Block>>()
         val path = makePathProps(blockInfra, infra, edge)
         res.addAll(getBlockedRanges(loadingGaugeType, path))
         return res
     }
 
+    override fun getBlockedRanges(path: PathProperties): Collection<Pathfinding.Range<Block>> {
+        return getBlockedRanges(loadingGaugeType, path)
+    }
+
     /** Returns the sections of the given block that can't be used by the given rolling stock */
     private fun getBlockedRanges(
         type: RJSLoadingGaugeType,

core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/constraints/SignalingSystemConstraints.kt

@@ -1,31 +1,67 @@
 package fr.sncf.osrd.api.pathfinding.constraints
 
+import fr.sncf.osrd.graph.ExplainablePathfindingConstraint
 import fr.sncf.osrd.graph.Pathfinding
-import fr.sncf.osrd.graph.PathfindingConstraint
 import fr.sncf.osrd.signaling.SignalingSimulator
-import fr.sncf.osrd.sim_infra.api.*
+import fr.sncf.osrd.sim_infra.api.Block
+import fr.sncf.osrd.sim_infra.api.BlockId
+import fr.sncf.osrd.sim_infra.api.BlockInfra
+import fr.sncf.osrd.sim_infra.api.Path
+import fr.sncf.osrd.sim_infra.api.PathProperties
+import fr.sncf.osrd.sim_infra.api.SignalingSystemId
 import fr.sncf.osrd.train.RollingStock
+import fr.sncf.osrd.utils.DistanceRangeMap
+import fr.sncf.osrd.utils.distanceRangeMapOf
+import fr.sncf.osrd.utils.units.Distance
 import fr.sncf.osrd.utils.units.Offset
 import fr.sncf.osrd.utils.units.meters
 
 data class SignalingSystemConstraints(
     val blockInfra: BlockInfra,
     val rollingStocksSupportedSigSystems: List<List<SignalingSystemId>>
-) : PathfindingConstraint<Block> {
+) : ExplainablePathfindingConstraint<Block> {
     override fun apply(edge: BlockId): MutableCollection<Pathfinding.Range<Block>> {
         val res = HashSet<Pathfinding.Range<Block>>()
         for (rollingStockSigSystems in rollingStocksSupportedSigSystems) {
             val edgeBlockedRanges = getBlockedRanges(edge, blockInfra, rollingStockSigSystems)
             if (edgeBlockedRanges.isNotEmpty()) {
                 res.addAll(edgeBlockedRanges)
-                break // if this edge is blocked for 2 RS, we will have the same exact range (the
-                // full edge
-                // range) twice
+                // if this edge is blocked for 2 RS, we will have the same exact range (the full
+                // edge range) twice
+                break
             }
         }
         return res
     }
 
+    override fun getBlockedRanges(path: PathProperties): Collection<Pathfinding.Range<Block>> {
+        return setOf()
+    }
+
+    fun getBlockedRangeValues(
+        blockList: List<BlockId>,
+        startOffset: Offset<Path>
+    ): DistanceRangeMap<SignalingSystemId> {
+        val blockedRanges = distanceRangeMapOf<SignalingSystemId>()
+        var currentBlockStart = -startOffset.distance
+        for (block in blockList) {
+            val blockSigSystem = blockInfra.getBlockSignalingSystem(block)
+            val blockLength = blockInfra.getBlockLength(block).distance
+            if (
+                rollingStocksSupportedSigSystems.any { !it.contains(blockSigSystem) } &&
+                    blockLength > -currentBlockStart
+            ) {
+                blockedRanges.put(
+                    Distance.max(Distance.ZERO, currentBlockStart),
+                    currentBlockStart + blockLength,
+                    blockSigSystem
+                )
+            }
+            currentBlockStart += blockLength
+        }
+        return blockedRanges
+    }
+
     /** Returns the sections of the given block that can't be used by the given rolling stock */
     private fun getBlockedRanges(
         edge: BlockId,