From 33017c92ac145dca603b1982bf221c2534082d0a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pierre-Etienne=20Bougu=C3=A9?= Date: Tue, 16 Dec 2025 11:11:14 +0100 Subject: [PATCH] [wip] core: draft PathfindingV2 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Pierre-Etienne Bougué --- .../fr/sncf/osrd/pathfinding/PathfindingV2.kt | 106 ++++++++++++++++++ .../sncf/osrd/utils/graph/PathfindingTests.kt | 15 +-- 2 files changed, 109 insertions(+), 12 deletions(-) create mode 100644 core/src/main/kotlin/fr/sncf/osrd/pathfinding/PathfindingV2.kt diff --git a/core/src/main/kotlin/fr/sncf/osrd/pathfinding/PathfindingV2.kt b/core/src/main/kotlin/fr/sncf/osrd/pathfinding/PathfindingV2.kt new file mode 100644 index 00000000000..7ace4f1bb00 --- /dev/null +++ b/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, // Full path as edge ranges + val waypoints: List, +) + +/** 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) + +/** 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, + val end: Offset, +) +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 { + 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() + + /** + * 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>? = 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>, + fullInfra: FullInfra, + rollingStock: RollingStock, + speedLimitTag: String?, + constraints: ConstraintCombiner, Block>, + ): ResultV2? { + return null + } + + /** Checks that required parameters are set, sets the optional ones to their default values */ + private fun checkParameters(targets: List>) { + 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 + } +} diff --git a/core/src/test/java/fr/sncf/osrd/utils/graph/PathfindingTests.kt b/core/src/test/java/fr/sncf/osrd/utils/graph/PathfindingTests.kt index 3a780183966..38231280527 100644 --- a/core/src/test/java/fr/sncf/osrd/utils/graph/PathfindingTests.kt +++ b/core/src/test/java/fr/sncf/osrd/utils/graph/PathfindingTests.kt @@ -5,12 +5,10 @@ import fr.sncf.osrd.graph.Graph import fr.sncf.osrd.graph.NetworkGraphAdapter import fr.sncf.osrd.pathfinding.BlockLocation 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 -import fr.sncf.osrd.pathfinding.PathfindingGraph -import fr.sncf.osrd.pathfinding.getStartLocations -import fr.sncf.osrd.pathfinding.getTargetsOnEdges import fr.sncf.osrd.reporting.exceptions.ErrorType import fr.sncf.osrd.reporting.exceptions.OSRDError import fr.sncf.osrd.sim_infra.api.Block @@ -761,8 +759,7 @@ class PathfindingTests { listOf(BlockLocation(secondBlock, Offset.zero())), ) val res = - Pathfinding(PathfindingGraph()) - .setEdgeToLength { it.length } + PathfindingV2(Graph()) .setRangeCost { range -> val start = mrspBuilder.getBlockTime(range.edge.block, range.start) val end = mrspBuilder.getBlockTime(range.edge.block, range.end) @@ -770,13 +767,7 @@ class PathfindingTests { return@setRangeCost res } .runPathfinding( - getStartLocations( - infra.fullInfra().rawInfra, - infra.fullInfra().blockInfra, - waypoints, - listOf(), - ), - getTargetsOnEdges(waypoints), + waypoints ) Assertions.assertEquals( arrayListOf(