[wip] core: draft PathfindingV2

core/src/main/kotlin/fr/sncf/osrd/pathfinding/PathfindingV2.kt

@@ -0,0 +1,106 @@
+package fr.sncf.osrd.pathfinding
+
+import fr.sncf.osrd.api.FullInfra
+import fr.sncf.osrd.graph.AStarHeuristic
+import fr.sncf.osrd.pathfinding.constraints.ConstraintCombiner
+import fr.sncf.osrd.reporting.exceptions.ErrorType
+import fr.sncf.osrd.reporting.exceptions.OSRDError
+import fr.sncf.osrd.sim_infra.api.Block
+import fr.sncf.osrd.sim_infra.api.BlockId
+import fr.sncf.osrd.stdcm.infra_exploration.InfraExplorer
+import fr.sncf.osrd.train.RollingStock
+import fr.sncf.osrd.utils.indexing.StaticIdx
+import fr.sncf.osrd.utils.units.Offset
+import java.util.ArrayList
+import java.util.PriorityQueue
+
+/** Contains all the results of a pathfinding */
+data class ResultV2(
+    val ranges: List<EdgeRangeV2>, // Full path as edge ranges
+    val waypoints: List<EdgeLocationV2>,
+)
+
+/** A location on a range, made of edge + offset. Used for the input of the pathfinding */
+data class EdgeLocationV2(val edge: BlockId, val offset: Offset<Block>)
+
+/** A range, made of edge + start and end offsets on the edge. Used to provide a cost function. */
+// TODO: Remove the use for the output of the pathfinding?
+data class EdgeRangeV2(
+    val edge: BlockId,
+    val start: Offset<Block>,
+    val end: Offset<Block>,
+)
+data class PathfindingNodeV2( // Instance used to explore the infra
+    val infraExplorer: InfraExplorer,
+    // Priority queue weight (could be different from totalDistance to allow for A*)
+    // TODO: split in 2 parts (one for EstablishedCost, the other for HeuristicAnticipatedCost)
+    val weight: Double,
+) : Comparable<PathfindingNodeV2> {
+    override fun compareTo(other: PathfindingNodeV2): Int {
+        if (weight != other.weight) return weight.compareTo(other.weight)
+        return 0
+    }
+}
+
+class PathfindingV2() {
+
+    /** Step priority queue */
+    private val queue = PriorityQueue<PathfindingNodeV2>()
+
+    /**
+     * Functions to call to get estimate of the remaining distance. We have a list of function for
+     * each step. These functions take the edge and the offset and returns a distance.
+     */
+    // TODO: make it a simple hard-coded fn(currentInfraExplorer, targets),
+    //  with shortest crow-fly distances between targets computed once for all
+    //  then just compute distance to next target and and the remaining targets' distances
+    private var estimateRemainingDistance: List<AStarHeuristic<BlockId, Block>>? = ArrayList()
+
+    /**
+     * Function to call to get the cost of a range. Defaults to distances. The heuristic unit *must*
+     * match.
+     */
+    private var rangeCost: (EdgeRangeV2) -> Double =
+        { range: EdgeRangeV2 ->
+            (range.end - range.start).millimeters.toDouble()
+        }
+
+    /** Timeout, in seconds, to avoid infinite loop when no path can be found. */
+    private var timeout = TIMEOUT
+
+    /** Sets the functor used to define the cost of an edge range */
+    fun setRangeCost(
+        f: (EdgeRangeV2) -> Double
+    ): PathfindingV2 {
+        rangeCost = f
+        return this
+    }
+
+    /** Sets the pathfinding's timeout */
+    fun setTimeout(timeout: Double?): PathfindingV2 {
+        if (timeout != null) this.timeout = timeout
+        return this
+    }
+
+    fun runPathfinding(
+        targets: List<Collection<EdgeLocationV2>>,
+        fullInfra: FullInfra,
+        rollingStock: RollingStock,
+        speedLimitTag: String?,
+        constraints: ConstraintCombiner<StaticIdx<Block>, Block>,
+    ): ResultV2? {
+        return null
+    }
+
+    /** Checks that required parameters are set, sets the optional ones to their default values */
+    private fun checkParameters(targets: List<Collection<EdgeLocationV2>>) {
+        assert(estimateRemainingDistance != null)
+        if (targets.size < 2)
+            throw OSRDError(ErrorType.InvalidSTDCMInputs)
+                .withContext("cause", "Not enough steps have been set to find a path")
+    }
+
+    companion object {
+        const val TIMEOUT = 180.0
+    }
+}

core/src/test/java/fr/sncf/osrd/utils/graph/PathfindingTests.kt

@@ -3,7 +3,9 @@ package fr.sncf.osrd.utils.graph
 import com.google.common.graph.NetworkBuilder
 import fr.sncf.osrd.graph.Graph
 import fr.sncf.osrd.graph.NetworkGraphAdapter
+import fr.sncf.osrd.pathfinding.EdgeLocationV2
 import fr.sncf.osrd.pathfinding.Pathfinding
+import fr.sncf.osrd.pathfinding.PathfindingV2
 import fr.sncf.osrd.pathfinding.Pathfinding.EdgeLocation
 import fr.sncf.osrd.pathfinding.Pathfinding.EdgeRange
 import fr.sncf.osrd.pathfinding.Pathfinding.Result
@@ -756,27 +758,20 @@ class PathfindingTests {
         val mrspBuilder =
             CachedBlockMRSPBuilder(infra.fullInfra().rawInfra, infra.fullInfra().blockInfra, null)
         val waypoints =
-            arrayListOf<Collection<EdgeLocation<BlockId, Block>>>(
-                listOf(EdgeLocation(slow, Offset.zero()), EdgeLocation(fast, Offset.zero())),
-                listOf(EdgeLocation(secondBlock, Offset.zero())),
+            arrayListOf<Collection<EdgeLocationV2<BlockId>>>(
+                listOf(EdgeLocationV2(slow, Offset.zero()), EdgeLocationV2(fast, Offset.zero())),
+                listOf(EdgeLocationV2(secondBlock, Offset.zero())),
             )
         val res =
-            Pathfinding(PathfindingGraph())
-                .setEdgeToLength { it.length }
+            PathfindingV2(Graph<PathfindingV2.PathfindingNode, PathfindingV2.PathfindingNode, Block>())
                 .setRangeCost { range ->
                     val start = mrspBuilder.getBlockTime(range.edge.block, range.start)
                     val end = mrspBuilder.getBlockTime(range.edge.block, range.end)
                     val res = end - start
                     return@setRangeCost res
                 }
                 .runPathfinding(
-                    getStartLocations(
-                        infra.fullInfra().rawInfra,
-                        infra.fullInfra().blockInfra,
-                        waypoints,
-                        listOf(),
-                    ),
-                    getTargetsOnEdges(waypoints),
+                    waypoints
                 )
         Assertions.assertEquals(
             arrayListOf(