[wip] core: project RS constraints on the relaxed path

OpenRailAssociation · Jul 9, 2024 · 2f67537 · 2f67537
1 parent c783c53
commit 2f67537
Show file tree

Hide file tree

Showing 2 changed files with 60 additions and 59 deletions.
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
@@ -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.conflicts.TravelledPath
 import fr.sncf.osrd.graph.*
 import fr.sncf.osrd.graph.Pathfinding.EdgeLocation
 import fr.sncf.osrd.railjson.schema.rollingstock.RJSLoadingGaugeType
@@ -144,69 +145,56 @@ private fun computePaths(
             .setEdgeToLength { block -> infra.blockInfra.getBlockLength(block) }
             .setRemainingDistanceEstimator(remainingDistanceEstimators)
             .runPathfinding(waypoints)
-    if (possiblePathWithoutErrorNoConstraints != null) {
-        for (currentConstraint in constraints) {
-            val res =
-                Pathfinding(GraphAdapter(infra.blockInfra, infra.rawInfra))
-                    .setTimeout(timeout)
-                    .setEdgeToLength { block: BlockId -> infra.blockInfra.getBlockLength(block) }
-                    .addBlockedRangeOnEdges(currentConstraint)
-                    .setRemainingDistanceEstimator(remainingDistanceEstimators)
-                    .runPathfinding(waypoints)
-            if (res == null) {
-                // This way of handling it is suboptimal, but it should be reworked soon
-                val relaxedPathResponse =
-                    runPathfindingPostProcessing(infra, possiblePathWithoutErrorNoConstraints)
-
-                when (currentConstraint::class.java) {
-                    ElectrificationConstraints::class.java -> {
-                        throw NoPathFoundException(
-                            IncompatibleConstraintsPathResponse(
-                                relaxedPathResponse,
-                                IncompatibleConstraints(
-                                    listOf(
-                                        RangeValue(
-                                            Pathfinding.Range(Offset.zero(), Offset.zero()),
-                                            "elec"
-                                        )
-                                    ),
-                                    listOf(),
-                                    listOf()
-                                )
-                            )
-                        )
-                    }
-                    LoadingGaugeConstraints::class.java -> {
-                        throw NoPathFoundException(
-                            IncompatibleConstraintsPathResponse(
-                                relaxedPathResponse,
-                                IncompatibleConstraints(
-                                    listOf(),
-                                    listOf(Pathfinding.Range(Offset.zero(), Offset.zero())),
-                                    listOf()
-                                )
-                            )
-                        )
+    if (
+        possiblePathWithoutErrorNoConstraints != null &&
+            possiblePathWithoutErrorNoConstraints.ranges.isNotEmpty()
+    ) {
+        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
+            )
+        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
                     }
-                    SignalingSystemConstraints::class.java -> {
-                        throw NoPathFoundException(
-                            IncompatibleConstraintsPathResponse(
-                                relaxedPathResponse,
-                                IncompatibleConstraints(
-                                    listOf(),
-                                    listOf(),
-                                    listOf(
-                                        RangeValue(
-                                            Pathfinding.Range(Offset.zero(), Offset.zero()),
-                                            "signal"
-                                        )
-                                    )
-                                )
-                            )
+                    val range =
+                        Pathfinding.Range(
+                            travelledPathBlockStart +
+                                Offset.max(blockedRange.start, range.start).distance,
+                            travelledPathBlockStart +
+                                Offset.min(blockedRange.end, range.end).distance
                         )
+                    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"))
+                        }
                     }
                 }
             }
+            travelledPathBlockStart += infra.blockInfra.getBlockLength(range.edge).distance
+        }
+
+        if (elecRanges.isNotEmpty() || gaugeRanges.isNotEmpty() || signalRanges.isNotEmpty()) {
+            val relaxedPathResponse =
+                runPathfindingPostProcessing(infra, possiblePathWithoutErrorNoConstraints)
+            throw NoPathFoundException(
+                IncompatibleConstraintsPathResponse(
+                    relaxedPathResponse,
+                    IncompatibleConstraints(elecRanges, gaugeRanges, signalRanges)
+                )
+            )
         }
     }
     // It didn’t fail due to a RS constraint, no path exists

diff --git a/core/src/test/kotlin/fr/sncf/osrd/pathfinding/PathfindingV2Test.kt b/core/src/test/kotlin/fr/sncf/osrd/pathfinding/PathfindingV2Test.kt
@@ -134,7 +134,20 @@ class PathfindingV2Test : ApiTest() {
             parsed.incompatibleConstraints ==
                 IncompatibleConstraints(
                     incompatibleElectrificationRanges =
-                        listOf(RangeValue(Pathfinding.Range(Offset.zero(), Offset.zero()), "elec")),
+                        listOf(
+                            RangeValue(
+                                Pathfinding.Range(Offset.zero(), Offset(125.meters)),
+                                "elec"
+                            ),
+                            RangeValue(
+                                Pathfinding.Range(Offset(125.meters), Offset(10175.meters)),
+                                "elec"
+                            ),
+                            RangeValue(
+                                Pathfinding.Range(Offset(10175.meters), Offset(10250.meters)),
+                                "elec"
+                            )
+                        ),
                     incompatibleGaugeRanges = listOf(),
                     incompatibleSignalingSystemRanges = listOf()
                 )