From dd9059a4fb303f8be9cc417006644e4b7f0f80fb Mon Sep 17 00:00:00 2001 From: Eloi Charpentier Date: Wed, 18 Sep 2024 15:44:04 +0200 Subject: [PATCH] core: stdcm: handle stops directly in infra explorer Signed-off-by: Eloi Charpentier --- .../fr/sncf/osrd/stdcm/graph/DelayManager.kt | 17 ++++ .../stdcm/graph/PostProcessingSimulation.kt | 20 +---- .../fr/sncf/osrd/stdcm/graph/STDCMEdge.kt | 12 ++- .../sncf/osrd/stdcm/graph/STDCMEdgeBuilder.kt | 20 +---- .../fr/sncf/osrd/stdcm/graph/TimeData.kt | 8 +- .../InfraExplorerWithEnvelope.kt | 47 +++++------ .../InfraExplorerWithEnvelopeImpl.kt | 77 ++++++++++++++++++- .../kotlin/fr/sncf/osrd/stdcm/StopTests.kt | 69 +++++++++++++++++ 8 files changed, 199 insertions(+), 71 deletions(-) diff --git a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/DelayManager.kt b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/DelayManager.kt index eff33383534..e72490f4d61 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/DelayManager.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/DelayManager.kt @@ -7,6 +7,7 @@ import fr.sncf.osrd.stdcm.preprocessing.interfaces.BlockAvailabilityInterface import fr.sncf.osrd.stdcm.preprocessing.interfaces.BlockAvailabilityInterface.Availability import fr.sncf.osrd.utils.units.Distance.Companion.fromMeters import fr.sncf.osrd.utils.units.Offset +import fr.sncf.osrd.utils.units.meters import java.util.* /** @@ -130,4 +131,20 @@ internal constructor( startTime ) } + + /** Return how much time we can add to the stop at the end of the explorer */ + fun getMaxAdditionalStopDuration( + explorerWithNewEnvelope: InfraExplorerWithEnvelope, + endTime: Double, + ): Double { + val availability = + blockAvailability.getAvailability( + explorerWithNewEnvelope, + explorerWithNewEnvelope.getSimulatedLength() - 10.meters, + explorerWithNewEnvelope.getSimulatedLength(), + endTime, + ) + if (availability is BlockAvailabilityInterface.Available) return availability.maximumDelay + return 0.0 + } } diff --git a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/PostProcessingSimulation.kt b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/PostProcessingSimulation.kt index 501aa1d1628..9c290d78d15 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/PostProcessingSimulation.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/PostProcessingSimulation.kt @@ -12,8 +12,6 @@ import fr.sncf.osrd.envelope_sim.allowances.utils.AllowanceValue import fr.sncf.osrd.railjson.schema.rollingstock.Comfort import fr.sncf.osrd.reporting.exceptions.ErrorType import fr.sncf.osrd.reporting.exceptions.OSRDError -import fr.sncf.osrd.standalone_sim.EnvelopeStopWrapper -import fr.sncf.osrd.stdcm.infra_exploration.withEnvelope import fr.sncf.osrd.stdcm.preprocessing.interfaces.BlockAvailabilityInterface import fr.sncf.osrd.train.RollingStock import fr.sncf.osrd.train.TrainStop @@ -88,11 +86,9 @@ fun buildFinalEnvelope( runSimulationWithFixedPoints(maxSpeedEnvelope, fixedPoints, context, isMareco) val conflictOffset = findConflictOffsets( - graph, newEnvelope, blockAvailability, edges, - stops, updatedTimeData, ) ?: return newEnvelope if (fixedPoints.any { it.offset == conflictOffset }) @@ -254,14 +250,11 @@ private fun getTimeOnEdges( * found, returns its offset. Otherwise, returns null. */ private fun findConflictOffsets( - graph: STDCMGraph, envelope: Envelope, blockAvailability: BlockAvailabilityInterface, edges: List, - stops: List, updatedTimeData: TimeData, ): Offset? { - val envelopeWithStops = EnvelopeStopWrapper(envelope, stops) val startOffset = edges[0].envelopeStartOffset val endOffset = startOffset + @@ -273,17 +266,12 @@ private fun findConflictOffsets( edges .last() .infraExplorer - .withEnvelope( - envelopeWithStops, - graph.fullInfra, - graph.rollingStock, - isSimulationComplete = true + .withNewEnvelope( + envelope, ) + .updateStopDurations(updatedTimeData) assert( - TrainPhysicsIntegrator.arePositionsEqual( - envelopeWithStops.endPos, - (endOffset - startOffset).meters - ) + TrainPhysicsIntegrator.arePositionsEqual(envelope.endPos, (endOffset - startOffset).meters) ) val availability = blockAvailability.getAvailability( diff --git a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMEdge.kt b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMEdge.kt index bd2af845dfd..55f34080c3c 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMEdge.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMEdge.kt @@ -55,7 +55,7 @@ data class STDCMEdge( return if (!endAtStop) { // We move on to the next block STDCMNode( - timeData.withAddedTime(totalTime, null), + timeData.withAddedTime(totalTime, null, null), endSpeed, infraExplorerWithNewEnvelope, this, @@ -71,8 +71,16 @@ data class STDCMEdge( val firstStopAfterIndex = graph.getFirstStopAfterIndex(waypointIndex)!! val stopDuration = firstStopAfterIndex.duration!! val locationOnEdge = envelopeStartOffset + length.distance + STDCMNode( - timeData.withAddedTime(totalTime, stopDuration), + timeData.withAddedTime( + totalTime, + stopDuration, + graph.delayManager.getMaxAdditionalStopDuration( + infraExplorerWithNewEnvelope, + timeData.earliestReachableTime + totalTime + ) + ), endSpeed, infraExplorerWithNewEnvelope, this, diff --git a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMEdgeBuilder.kt b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMEdgeBuilder.kt index 929a4953ddf..b7c57d7fb8c 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMEdgeBuilder.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMEdgeBuilder.kt @@ -1,13 +1,10 @@ package fr.sncf.osrd.stdcm.graph import fr.sncf.osrd.envelope.Envelope -import fr.sncf.osrd.envelope_sim.TrainPhysicsIntegrator.isTimeStrictlyPositive import fr.sncf.osrd.envelope_sim.allowances.LinearAllowance import fr.sncf.osrd.sim_infra.api.Block -import fr.sncf.osrd.standalone_sim.EnvelopeStopWrapper import fr.sncf.osrd.stdcm.infra_exploration.InfraExplorerWithEnvelope import fr.sncf.osrd.stdcm.preprocessing.interfaces.BlockAvailabilityInterface -import fr.sncf.osrd.train.TrainStop import fr.sncf.osrd.utils.units.Distance.Companion.fromMeters import fr.sncf.osrd.utils.units.Length import fr.sncf.osrd.utils.units.Offset @@ -121,21 +118,8 @@ internal constructor( if (envelope.endPos == 0.0) envelope else LinearAllowance.scaleEnvelope(envelope, speedRatio) val stopDuration = getEndStopDuration() - val envelopeWithStop = - if (stopDuration == null) scaledEnvelope - else - EnvelopeStopWrapper( - scaledEnvelope, - listOf( - TrainStop( - envelope.endPos, - stopDuration, - // TODO: forward and use onStopSignal param from request - isTimeStrictlyPositive(stopDuration) - ) - ) - ) - explorerWithNewEnvelope = infraExplorer.clone().addEnvelope(envelopeWithStop) + explorerWithNewEnvelope = infraExplorer.clone().addEnvelope(scaledEnvelope) + if (stopDuration != null) explorerWithNewEnvelope!!.addStop(stopDuration) } return explorerWithNewEnvelope } diff --git a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/TimeData.kt b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/TimeData.kt index c15b8694a50..bde625e7907 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/TimeData.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/TimeData.kt @@ -68,9 +68,12 @@ data class TimeData( fun withAddedTime( extraTravelTime: Double, extraStopTime: Double?, + maxAdditionalStopTime: Double? ): TimeData { var newStopData = stopTimeData var maxDepartureDelayingWithoutConflict = maxDepartureDelayingWithoutConflict + val nextEarliestReachableTime = + earliestReachableTime + extraTravelTime + (extraStopTime ?: 0.0) if (extraStopTime != null) { val stopDataCopy = newStopData.toMutableList() stopDataCopy.add( @@ -81,11 +84,10 @@ data class TimeData( ) ) newStopData = stopDataCopy - maxDepartureDelayingWithoutConflict = Double.POSITIVE_INFINITY + maxDepartureDelayingWithoutConflict = maxAdditionalStopTime!! } return copy( - earliestReachableTime = - earliestReachableTime + extraTravelTime + (extraStopTime ?: 0.0), + earliestReachableTime = nextEarliestReachableTime, totalRunningTime = totalRunningTime + extraTravelTime, stopTimeData = newStopData, maxDepartureDelayingWithoutConflict = maxDepartureDelayingWithoutConflict, diff --git a/core/src/main/kotlin/fr/sncf/osrd/stdcm/infra_exploration/InfraExplorerWithEnvelope.kt b/core/src/main/kotlin/fr/sncf/osrd/stdcm/infra_exploration/InfraExplorerWithEnvelope.kt index 2ef4d95d509..2989dcd58eb 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/stdcm/infra_exploration/InfraExplorerWithEnvelope.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/stdcm/infra_exploration/InfraExplorerWithEnvelope.kt @@ -3,8 +3,7 @@ package fr.sncf.osrd.stdcm.infra_exploration import fr.sncf.osrd.api.FullInfra import fr.sncf.osrd.conflicts.IncrementalRequirementEnvelopeAdapter import fr.sncf.osrd.conflicts.SpacingRequirementAutomaton -import fr.sncf.osrd.envelope.EnvelopeConcat -import fr.sncf.osrd.envelope.EnvelopeInterpolate +import fr.sncf.osrd.envelope.Envelope import fr.sncf.osrd.envelope.EnvelopeTimeInterpolate import fr.sncf.osrd.envelope_sim.PhysicsRollingStock import fr.sncf.osrd.graph.PathfindingConstraint @@ -12,6 +11,8 @@ import fr.sncf.osrd.graph.PathfindingEdgeLocationId import fr.sncf.osrd.sim_infra.api.Block import fr.sncf.osrd.sim_infra.api.Path import fr.sncf.osrd.standalone_sim.result.ResultTrain +import fr.sncf.osrd.stdcm.graph.TimeData +import fr.sncf.osrd.train.TrainStop import fr.sncf.osrd.utils.appendOnlyLinkedListOf import fr.sncf.osrd.utils.units.Length import fr.sncf.osrd.utils.units.Offset @@ -41,7 +42,21 @@ interface InfraExplorerWithEnvelope : InfraExplorer { fun getFullEnvelope(): EnvelopeTimeInterpolate /** Adds an envelope. This is done in-place. */ - fun addEnvelope(envelope: EnvelopeInterpolate): InfraExplorerWithEnvelope + fun addEnvelope(envelope: Envelope): InfraExplorerWithEnvelope + + /** + * Returns a copy with the given envelope, the previous one is ignored. This keeps stop data. + */ + fun withNewEnvelope(envelope: Envelope): InfraExplorerWithEnvelope + + /** Add a stop to the end of the last simulated envelope */ + fun addStop(stopDuration: Double) + + /** Just for debugging purposes. */ + fun getStops(): List + + /** Update the stop durations, following the updated time data */ + fun updateStopDurations(updatedTimeData: TimeData): InfraExplorerWithEnvelope /** * Calls `InterpolateDepartureFromClamp` on the underlying envelope, taking the travelled path @@ -99,29 +114,3 @@ fun initInfraExplorerWithEnvelope( ) } } - -/** Add an envelope to a simple InfraExplorer. */ -fun InfraExplorer.withEnvelope( - envelope: EnvelopeInterpolate, - fullInfra: FullInfra, - rollingStock: PhysicsRollingStock, - isSimulationComplete: Boolean = false, -): InfraExplorerWithEnvelope { - return InfraExplorerWithEnvelopeImpl( - this, - appendOnlyLinkedListOf(EnvelopeConcat.LocatedEnvelope(envelope, 0.0, 0.0)), - SpacingRequirementAutomaton( - fullInfra.rawInfra, - fullInfra.loadedSignalInfra, - fullInfra.blockInfra, - fullInfra.signalingSimulator, - IncrementalRequirementEnvelopeAdapter( - rollingStock, - EnvelopeConcat.from(listOf(envelope)), - isSimulationComplete - ), - getIncrementalPath(), - ), - rollingStock, - ) -} diff --git a/core/src/main/kotlin/fr/sncf/osrd/stdcm/infra_exploration/InfraExplorerWithEnvelopeImpl.kt b/core/src/main/kotlin/fr/sncf/osrd/stdcm/infra_exploration/InfraExplorerWithEnvelopeImpl.kt index fb1ce238fa4..395a60f4f43 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/stdcm/infra_exploration/InfraExplorerWithEnvelopeImpl.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/stdcm/infra_exploration/InfraExplorerWithEnvelopeImpl.kt @@ -3,14 +3,19 @@ package fr.sncf.osrd.stdcm.infra_exploration import fr.sncf.osrd.conflicts.IncrementalRequirementEnvelopeAdapter import fr.sncf.osrd.conflicts.SpacingRequirementAutomaton import fr.sncf.osrd.conflicts.SpacingRequirements +import fr.sncf.osrd.envelope.Envelope import fr.sncf.osrd.envelope.EnvelopeConcat import fr.sncf.osrd.envelope.EnvelopeConcat.LocatedEnvelope import fr.sncf.osrd.envelope.EnvelopeInterpolate import fr.sncf.osrd.envelope_sim.PhysicsRollingStock import fr.sncf.osrd.sim_infra.api.Path +import fr.sncf.osrd.standalone_sim.EnvelopeStopWrapper import fr.sncf.osrd.standalone_sim.result.ResultTrain.SpacingRequirement +import fr.sncf.osrd.stdcm.graph.TimeData import fr.sncf.osrd.stdcm.preprocessing.interfaces.BlockAvailabilityInterface +import fr.sncf.osrd.train.TrainStop import fr.sncf.osrd.utils.AppendOnlyLinkedList +import fr.sncf.osrd.utils.appendOnlyLinkedListOf import fr.sncf.osrd.utils.units.Distance import fr.sncf.osrd.utils.units.Length import fr.sncf.osrd.utils.units.Offset @@ -22,6 +27,7 @@ data class InfraExplorerWithEnvelopeImpl( private val envelopes: AppendOnlyLinkedList, private val spacingRequirementAutomaton: SpacingRequirementAutomaton, private val rollingStock: PhysicsRollingStock, + private var stops: MutableList = mutableListOf(), // Soft references tell the JVM that the values may be cleared when running out of memory private var spacingRequirementsCache: SoftReference>? = null, @@ -35,6 +41,7 @@ data class InfraExplorerWithEnvelopeImpl( envelopes.shallowCopy(), spacingRequirementAutomaton.clone(), rollingStock, + stops.toMutableList(), spacingRequirementsCache, ) } @@ -44,11 +51,12 @@ data class InfraExplorerWithEnvelopeImpl( val cached = envelopeCache?.get() if (cached != null) return cached val res = EnvelopeConcat.fromLocated(envelopes.toList()) - envelopeCache = SoftReference(res) - return res + val withStops = EnvelopeStopWrapper(res, stops) + envelopeCache = SoftReference(withStops) + return withStops } - override fun addEnvelope(envelope: EnvelopeInterpolate): InfraExplorerWithEnvelope { + override fun addEnvelope(envelope: Envelope): InfraExplorerWithEnvelope { var prevEndOffset = 0.0 var prevEndTime = 0.0 if (envelopes.isNotEmpty()) { @@ -62,6 +70,68 @@ data class InfraExplorerWithEnvelopeImpl( return this } + override fun withNewEnvelope(envelope: Envelope): InfraExplorerWithEnvelope { + return copy( + envelopes = + appendOnlyLinkedListOf( + LocatedEnvelope(envelope, 0.0, 0.0), + ), + spacingRequirementAutomaton = + SpacingRequirementAutomaton( + spacingRequirementAutomaton.rawInfra, + spacingRequirementAutomaton.loadedSignalInfra, + spacingRequirementAutomaton.blockInfra, + spacingRequirementAutomaton.simulator, + IncrementalRequirementEnvelopeAdapter( + rollingStock, + EnvelopeConcat.from(listOf(envelope)), + true + ), + getIncrementalPath(), + ), + spacingRequirementsCache = null, + envelopeCache = null, + ) + } + + override fun addStop(stopDuration: Double) { + val position = getFullEnvelope().endPos + // We tolerate duplicates and filter them + if (stops.isEmpty() || stops.last().position != position) { + stops.add( + TrainStop( + position, + stopDuration, + true, + ) + ) + envelopeCache = null + spacingRequirementsCache = null + } else { + assert(stops.isEmpty() || stops.last().duration == stopDuration) + } + } + + override fun getStops(): List { + return stops + } + + override fun updateStopDurations(updatedTimeData: TimeData): InfraExplorerWithEnvelope { + for ((i, stop) in stops.withIndex()) { + assert(i < updatedTimeData.stopTimeData.size) + val updatedStop = updatedTimeData.stopTimeData[i] + if (updatedStop.currentDuration != stop.duration) { + stops[i] = + TrainStop( + stops[i].position, + updatedStop.currentDuration, + stops[i].onStopSignal, + ) + } + } + return this + } + override fun interpolateDepartureFromClamp(pathOffset: Offset): Double { return getFullEnvelope() .interpolateDepartureFromClamp( @@ -133,6 +203,7 @@ data class InfraExplorerWithEnvelopeImpl( envelopes.shallowCopy(), spacingRequirementAutomaton.clone(), rollingStock, + stops.toMutableList(), spacingRequirementsCache ) } diff --git a/core/src/test/kotlin/fr/sncf/osrd/stdcm/StopTests.kt b/core/src/test/kotlin/fr/sncf/osrd/stdcm/StopTests.kt index ca352b3f1b6..eaa9fb24353 100644 --- a/core/src/test/kotlin/fr/sncf/osrd/stdcm/StopTests.kt +++ b/core/src/test/kotlin/fr/sncf/osrd/stdcm/StopTests.kt @@ -5,6 +5,7 @@ import fr.sncf.osrd.envelope_sim.TrainPhysicsIntegrator import fr.sncf.osrd.envelope_sim.allowances.utils.AllowanceValue import fr.sncf.osrd.graph.Pathfinding.EdgeLocation import fr.sncf.osrd.sim_infra.api.Block +import fr.sncf.osrd.sim_infra.api.BlockId import fr.sncf.osrd.stdcm.StandardAllowanceTests.Companion.checkAllowanceResult import fr.sncf.osrd.stdcm.StandardAllowanceTests.Companion.runWithAndWithoutAllowance import fr.sncf.osrd.stdcm.preprocessing.OccupancySegment @@ -627,6 +628,74 @@ class StopTests { assertEquals(15_000.0, arrivalTime, timeStep) } + /** + * Checks that we properly account for stop durations when looking for conflicts, with two + * stops. + */ + @Test + fun variableStopTimeWithConflicts() { + /* + a --> b --> c --> d --> e --> f + ^ ^ + stop stop + + space + ^ + f |############################ / ### + | / + e | / + | __(___)/ <-- stop + d | / + |################ / ############### + c | / + | __(___)/ <-- stop + | / + b | / + |#### /############################ + a |####/_############################_> time + + */ + val infra = DummyInfra() + val timeStep = 2.0 + val blocks = + listOf( + infra.addBlock("a", "b"), + infra.addBlock("b", "c"), + infra.addBlock("c", "d"), + infra.addBlock("d", "e"), + infra.addBlock("e", "f"), + ) + + val builder = ImmutableMultimap.builder() + builder.put(blocks[0], OccupancySegment(0.0, 1_000.0, 0.meters, 100.meters)) + builder.put( + blocks[0], + OccupancySegment(2_000.0, Double.POSITIVE_INFINITY, 0.meters, 100.meters) + ) + builder.put(blocks[2], OccupancySegment(0.0, 10_000.0, 0.meters, 100.meters)) + builder.put( + blocks[2], + OccupancySegment(12_000.0, Double.POSITIVE_INFINITY, 0.meters, 100.meters) + ) + builder.put(blocks[4], OccupancySegment(0.0, 20_000.0, 0.meters, 100.meters)) + builder.put( + blocks[4], + OccupancySegment(22_000.0, Double.POSITIVE_INFINITY, 0.meters, 100.meters) + ) + val occupancy = builder.build() + var res = + STDCMPathfindingBuilder() + .setInfra(infra.fullInfra()) + .setUnavailableTimes(occupancy) + .addStep(STDCMStep(setOf(EdgeLocation(blocks[0], Offset(0.meters))))) + .addStep(STDCMStep(setOf(EdgeLocation(blocks[1], Offset(50.meters))), 1.0, true)) + .addStep(STDCMStep(setOf(EdgeLocation(blocks[3], Offset(50.meters))), 1.0, true)) + .addStep(STDCMStep(setOf(EdgeLocation(blocks[4], Offset(100.meters))), 0.0, true)) + .setTimeStep(timeStep) + .run()!! + occupancyTest(res, occupancy) + } + companion object { /** Check that the train actually stops at the expected times and positions */ private fun checkStop(