core: fix "negative" routing requirements + add requirement margin for stop on closed signal (conflict detection)

core/src/main/java/fr/sncf/osrd/conflicts/SpacingResourceGenerator.kt

@@ -4,6 +4,7 @@ import fr.sncf.osrd.signaling.SignalingSimulator
 import fr.sncf.osrd.signaling.SignalingTrainState
 import fr.sncf.osrd.signaling.ZoneStatus
 import fr.sncf.osrd.sim_infra.api.*
+import fr.sncf.osrd.standalone_sim.CLOSED_SIGNAL_RESERVATION_MARGIN
 import fr.sncf.osrd.standalone_sim.result.ResultTrain.SpacingRequirement
 import fr.sncf.osrd.utils.indexing.mutableStaticIdxArrayListOf
 import fr.sncf.osrd.utils.units.Offset
@@ -416,8 +417,7 @@ class SpacingRequirementAutomaton(
 
         // TODO: use a lookup table
         fun findLastStopBeforeZone(): ProcessedStop? {
-            for (i in processedStops.size - 1 downTo 0) {
-                val stop = processedStops[i]
+            for (stop in processedStops.reversed()) {
                 if (stop.nextZoneIdx <= pendingRequirement.zoneIndex) {
                     return stop
                 }
@@ -432,7 +432,7 @@ class SpacingRequirementAutomaton(
             if (stopEndTime.isInfinite()) {
                 return null
             }
-            beginTime = maxOf(beginTime, stopEndTime)
+            beginTime = maxOf(beginTime, stopEndTime - CLOSED_SIGNAL_RESERVATION_MARGIN)
         }
 
         val departureTime = callbacks.departureTimeFromRange(zoneEntryOffset, zoneExitOffset)

core/src/main/java/fr/sncf/osrd/standalone_sim/ScheduleMetadataExtractor.kt

@@ -40,6 +40,9 @@ import mu.KotlinLogging
 
 private val logger = KotlinLogging.logger {}
 
+// Reserve clear track with a margin for the reaction time of the driver
+const val CLOSED_SIGNAL_RESERVATION_MARGIN = 20.0
+
 // the start offset is the distance from the start of the first block to the start location
 class PathOffsetBuilder(val startOffset: Distance) {
     fun toTravelledPath(offset: Offset<Path>): Offset<TravelledPath> {
@@ -363,39 +366,40 @@ fun routingRequirements(
             ) ?: return null
         val limitingBlock = blockPath[limitingSignalSpec.blockIndex]
         val signal = blockInfra.getBlockSignals(limitingBlock)[limitingSignalSpec.signalIndex]
-        val limitingSignalOffset =
+        val limitingSignalOffsetInBlock =
             blockInfra.getSignalsPositions(limitingBlock)[limitingSignalSpec.signalIndex].distance
 
-        val blockOffset = blockOffsets[limitingSignalSpec.blockIndex]
-        val sightDistance = rawInfra.getSignalSightDistance(rawInfra.getPhysicalSignal(signal))
+        val limitingBlockOffset = blockOffsets[limitingSignalSpec.blockIndex]
+        val signalSightDistance =
+            rawInfra.getSignalSightDistance(rawInfra.getPhysicalSignal(signal))
 
         // find the location at which establishing the route becomes necessary
-        var criticalPos = blockOffset + limitingSignalOffset - sightDistance
+        val criticalPos = limitingBlockOffset + limitingSignalOffsetInBlock - signalSightDistance
+        var criticalTime = envelope.interpolateArrivalAtClamp(criticalPos.distance.meters)
 
         // check if an arrival on stop signal is scheduled between the critical position and the
-        // entry signal of the route
+        // entry signal of the route (both position and time, as there is a time margin)
         // in this case, just move the critical position to just after the stop
         val entrySignalOffset =
-            blockOffset + blockInfra.getSignalsPositions(firstRouteBlock).first().distance
-        for (stopIdx in stops.size - 1 downTo 0) {
-            val stop = stops[stopIdx]
+            blockOffsets[routeStartBlockIndex] +
+                blockInfra.getSignalsPositions(firstRouteBlock).first().distance
+        for (stop in stops.reversed()) {
             val stopTravelledOffset = pathOffsetBuilder.toTravelledPath(stop.pathOffset)
             if (
                 stop.receptionSignal.isStopOnClosedSignal &&
-                    entrySignalOffset <= stopTravelledOffset
+                    stopTravelledOffset <= entrySignalOffset
             ) {
                 // stop duration is included in interpolateDepartureFromClamp()
-                criticalPos = stopTravelledOffset
-                break
-            }
-            if (stopTravelledOffset < criticalPos) {
+                val stopDepartureTime =
+                    envelope.interpolateDepartureFromClamp(stopTravelledOffset.distance.meters)
+                if (criticalTime < stopDepartureTime - CLOSED_SIGNAL_RESERVATION_MARGIN) {
+                    criticalTime = stopDepartureTime - CLOSED_SIGNAL_RESERVATION_MARGIN
+                }
                 break
             }
         }
 
-        // find last time when the train is at the critical location (including stop duration if at
-        // stop)
-        return envelope.interpolateDepartureFromClamp(criticalPos.distance.meters)
+        return maxOf(criticalTime, 0.0)
     }
 
     val res = mutableListOf<RoutingRequirement>()

core/src/test/java/fr/sncf/osrd/standalone_sim/ConflictDetectionTest.java

@@ -462,6 +462,102 @@ public void conflictDetectionForOvertakeInStation(
         assertFalse(conflicts.stream().anyMatch(conflict -> !conflict.workScheduleIds.isEmpty()));
     }
 
+    @ParameterizedTest
+    @CsvSource({
+        "OPEN, 213.458699", // first sight of the limiting signal for switch's zone
+        "STOP, 812.429826", // 20 s before the departure from the 10-min stop at Mid_West_station
+        "SHORT_SLIP_STOP, 812.429826",
+    })
+    public void resourceReservation(RJSReceptionSignal receptionSignal, double switchReqBegin) throws Exception {
+        var rjsInfra = Helpers.getExampleInfra("small_infra/infra.json");
+        var fullInfra = fullInfraFromRJS(rjsInfra);
+        var rawInfra = fullInfra.rawInfra();
+
+        var ta6 = getTrackSectionFromNameOrThrow("TA6", rawInfra);
+        var td0 = getTrackSectionFromNameOrThrow("TD0", rawInfra);
+
+        var chunkPathCenter = chunkPathFromRoutes(
+                rawInfra,
+                List.of("rt.DA0->DA5", "rt.DA5->DC5", "rt.DC5->DD2"),
+                makeTrackLocation(ta6, fromMeters(1000)), // start after DA3
+                makeTrackLocation(td0, fromMeters(24820)));
+        var pathPropsCenter = makePathProperties(rawInfra, chunkPathCenter, null);
+        var chunkPathNorth = chunkPathFromRoutes(
+                rawInfra,
+                List.of("rt.DA0->DA5", "rt.DA5->DC4", "rt.DC4->DD2"),
+                makeTrackLocation(ta6, fromMeters(1000)),
+                makeTrackLocation(td0, fromMeters(24820)));
+        var pathPropsNorth = makePathProperties(rawInfra, chunkPathNorth, null);
+
+        var stop = new TrainStop(9700, 600, receptionSignal);
+        var simResultCenterWithStop =
+                simpleSim(fullInfra, pathPropsCenter, chunkPathCenter, 0, Double.POSITIVE_INFINITY, List.of(stop));
+        var simResultNorthOvertaking =
+                simpleSim(fullInfra, pathPropsNorth, chunkPathNorth, 0, Double.POSITIVE_INFINITY, List.of());
+
+        var reqWithStop = convertRequirements(0L, 0.0, simResultCenterWithStop.train);
+        var directStartTime = 300; // 5 min after stopping train
+        var reqOvertaking = convertRequirements(1L, directStartTime, simResultNorthOvertaking.train);
+
+        // check that requirements are all ending after they begin
+        assertRequirementsPeriodsConsistency(reqOvertaking);
+        assertRequirementsPeriodsConsistency(reqWithStop);
+
+        // check spacing and routing requirements for the zone of the switch after (East) Mid_West_station
+        var switchZoneName = "zone.[DC4:INCREASING, DC5:INCREASING, DD0:DECREASING]";
+
+        // requirements for train with stop
+        var switchZoneExitTime = 844.837492;
+        var switchSpacingReqWithStop = reqWithStop.getSpacingRequirements().stream()
+                .filter(it -> it.zone.equals(switchZoneName))
+                .findFirst()
+                .get();
+        assertEquals(switchReqBegin, switchSpacingReqWithStop.beginTime);
+        assertEquals(switchZoneExitTime, switchSpacingReqWithStop.endTime);
+        var switchRouteReqWithStop = reqWithStop.getRoutingRequirements().stream()
+                .filter(it -> it.route.equals("rt.DC5->DD2"))
+                .findFirst()
+                .get();
+        var switchRouteCrossingZoneReqWithStop = switchRouteReqWithStop.zones.stream()
+                .filter(it -> it.zone.equals(switchZoneName))
+                .findFirst()
+                .get();
+        assertEquals(switchReqBegin, switchRouteReqWithStop.beginTime);
+        assertEquals(switchZoneExitTime, switchRouteCrossingZoneReqWithStop.endTime);
+
+        // requirements for overtaking train (no stop)
+        var overtakingSwitchZoneExitTime = 545.533259;
+        var overtakingSwitchLimitingSignalSight = 513.458699;
+        var overtakingSwitchSpacingReqWithStop = reqOvertaking.getSpacingRequirements().stream()
+                .filter(it -> it.zone.equals(switchZoneName))
+                .findFirst()
+                .get();
+        assertEquals(overtakingSwitchLimitingSignalSight, overtakingSwitchSpacingReqWithStop.beginTime);
+        assertEquals(overtakingSwitchZoneExitTime, overtakingSwitchSpacingReqWithStop.endTime);
+        var overtakingSwitchRouteReqWithStop = reqOvertaking.getRoutingRequirements().stream()
+                .filter(it -> it.route.equals("rt.DC4->DD2"))
+                .findFirst()
+                .get();
+        var overtakingSwitchRouteCrossingZoneReqWithStop = overtakingSwitchRouteReqWithStop.zones.stream()
+                .filter(it -> it.zone.equals(switchZoneName))
+                .findFirst()
+                .get();
+        assertEquals(overtakingSwitchLimitingSignalSight, overtakingSwitchRouteReqWithStop.beginTime);
+        assertEquals(overtakingSwitchZoneExitTime, overtakingSwitchRouteCrossingZoneReqWithStop.endTime);
+    }
+
+    private static void assertRequirementsPeriodsConsistency(TrainRequirements requirements) {
+        for (var spacingReq : requirements.getSpacingRequirements()) {
+            assert (spacingReq.beginTime <= spacingReq.endTime);
+        }
+        for (var routingReq : requirements.getRoutingRequirements()) {
+            var routingReqBegin = routingReq.beginTime;
+            for (var zoneReq : routingReq.zones) {
+                assert (routingReqBegin <= zoneReq.endTime);
+            }
+        }
+    }
+
     @ParameterizedTest
     @MethodSource("workScheduleArgs")
     public void testWorkSchedules(