core: add etcs svl logic

Signed-off-by: Erashin <[email protected]>

Author: Erashin

core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/etcs/Constants.kt

@@ -13,6 +13,12 @@ package fr.sncf.osrd.envelope_sim.etcs
  */
 const val qNvinhsmicperm = false
 
+/**
+ * National Default Value: permission to follow release speed at 40km/h near the EoA. See Subset
+ * 026: table in Appendix A.3.2.
+ */
+const val releaseSpeed = 40.0 / 3.6 // m/s
+
 /**
  * Estimated acceleration during tBerem, worst case scenario (aEst2 is between 0 and 0.4), expressed
  * in m/s². See Subset 026: §3.13.9.3.2.9.

core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/etcs/ETCSBrakingCurves.kt

@@ -47,57 +47,61 @@ fun addBrakingCurvesAtEOAs(
 ): Envelope {
     val sortedEndsOfAuthority = endsOfAuthority.sortedBy { it.offsetEOA }
     var beginPos = envelope.beginPos
-    val builder = OverlayEnvelopeBuilder.forward(envelope)
+    val maxSpeedEnvelope = envelope.maxSpeed
+    var envelopeWithEoaBrakingCurves = envelope
+    var builder = OverlayEnvelopeBuilder.forward(envelopeWithEoaBrakingCurves)
     for (endOfAuthority in sortedEndsOfAuthority) {
         val targetPosition = endOfAuthority.offsetEOA.distance.meters
         assert(targetPosition > 0.0)
         val targetSpeed = 0.0
-        val maxSpeedEnvelope = envelope.maxSpeed
-        val overhead =
-            Envelope.make(
-                EnvelopePart.generateTimes(
-                    listOf(EnvelopeProfile.CONSTANT_SPEED),
-                    doubleArrayOf(0.0, targetPosition),
-                    doubleArrayOf(maxSpeedEnvelope, maxSpeedEnvelope)
+        val fullIndicationCurveEoa =
+            computeSbdFullIndicationCurve(context, targetPosition, maxSpeedEnvelope)
+
+        if (endOfAuthority.offsetSVL != null) {
+            val targetPositionSvl = endOfAuthority.offsetSVL.distance.meters
+            val fullIndicationCurveSvl =
+                computeEbdFullIndicationCurve(
+                    context,
+                    targetPositionSvl,
+                    targetSpeed,
+                    maxSpeedEnvelope
                 )
-            )
-        val sbdCurve =
-            computeBrakingCurve(
-                context,
-                overhead,
-                targetPosition,
-                targetSpeed,
-                BrakingType.ETCS_SBD
-            )
-        assert(sbdCurve.beginPos >= 0 && sbdCurve.endPos == targetPosition)
-        assert(sbdCurve.endSpeed == targetSpeed)
-        val guiCurve =
-            computeBrakingCurve(
-                context,
-                overhead,
-                targetPosition,
-                targetSpeed,
-                BrakingType.ETCS_GUI
-            )
-        assert(guiCurve.beginPos >= 0.0 && guiCurve.endPos == targetPosition)
-        assert((guiCurve.beginSpeed == maxSpeedEnvelope || guiCurve.beginPos == 0.0))
-        assert(guiCurve.endSpeed == targetSpeed)
-
-        val fullIndicationCurve =
-            computeIndicationBrakingCurveFromRef(context, sbdCurve, BrakingCurveType.SBD, guiCurve)
-        assert(fullIndicationCurve.endPos == targetPosition)
-        assert(fullIndicationCurve.endSpeed == targetSpeed)
+            val indicationCurveSvl =
+                computeSvlIndicationCurve(
+                    fullIndicationCurveSvl,
+                    targetPosition,
+                    envelope,
+                    beginPos
+                )
+            if (indicationCurveSvl != null) {
+                assert(
+                    indicationCurveSvl.beginPos >= beginPos &&
+                        indicationCurveSvl.endPos == targetPosition
+                )
+                assert(
+                    indicationCurveSvl.beginSpeed <= maxSpeedEnvelope &&
+                        indicationCurveSvl.endSpeed >= releaseSpeed
+                )
+                builder.addPart(indicationCurveSvl)
+                envelopeWithEoaBrakingCurves = builder.build()
+                builder = OverlayEnvelopeBuilder.forward(envelopeWithEoaBrakingCurves)
+            }
+        }
 
         val indicationCurve =
-            keepBrakingCurveUnderOverlay(Envelope.make(fullIndicationCurve), envelope, beginPos)
-                ?: continue
+            keepBrakingCurveUnderOverlay(
+                Envelope.make(fullIndicationCurveEoa),
+                envelopeWithEoaBrakingCurves,
+                beginPos
+            ) ?: continue
         assert(indicationCurve.beginPos >= beginPos && indicationCurve.endPos == targetPosition)
         assert(
             indicationCurve.beginSpeed <= maxSpeedEnvelope &&
                 indicationCurve.endSpeed == targetSpeed
         )
-
         builder.addPart(indicationCurve)
+        envelopeWithEoaBrakingCurves = builder.build()
+        builder = OverlayEnvelopeBuilder.forward(envelopeWithEoaBrakingCurves)
 
         // We build EOAs along the path. We need to handle overlaps with the next EOA. To do so, we
         // shift the left position constraint, beginPos, to this EOA's target position.
@@ -114,58 +118,19 @@ fun addBrakingCurvesAtLOAs(
 ): Envelope {
     val sortedLimitsOfAuthority = limitsOfAuthority.sortedBy { it.offset }
     val beginPos = envelope.beginPos
+    val maxSpeedEnvelope = envelope.maxSpeed
     var envelopeWithLoaBrakingCurves = envelope
     var builder = OverlayEnvelopeBuilder.forward(envelopeWithLoaBrakingCurves)
 
-    val maxSpeedEnvelope = envelopeWithLoaBrakingCurves.maxSpeed
-    // Add maxBecDeltaSpeed to EBD curve overhead so it reaches a sufficiently high speed to
-    // guarantee that, after the speed translation, the corresponding EBI curve does intersect
-    // with envelope max speed.
-    val maxBecDeltaSpeed = maxBecDeltaSpeed()
-    val maxSpeedEbd = maxSpeedEnvelope + maxBecDeltaSpeed
-    val overhead =
-        Envelope.make(
-            EnvelopePart.generateTimes(
-                listOf(EnvelopeProfile.CONSTANT_SPEED),
-                doubleArrayOf(0.0, context.path.length),
-                doubleArrayOf(maxSpeedEbd, maxSpeedEbd)
-            )
-        )
-
     for (limitOfAuthority in sortedLimitsOfAuthority) {
         val targetPosition = limitOfAuthority.offset.distance.meters
         assert(targetPosition > 0.0)
         val targetSpeed = limitOfAuthority.speed
         assert(targetSpeed > 0.0)
-
-        val ebdCurve =
-            computeBrakingCurve(
-                context,
-                overhead,
-                targetPosition,
-                targetSpeed,
-                BrakingType.ETCS_EBD
-            )
-        assert(ebdCurve.beginPos >= 0.0 && ebdCurve.endPos >= targetPosition)
-        val guiCurve =
-            computeBrakingCurve(
-                context,
-                overhead,
-                targetPosition,
-                targetSpeed,
-                BrakingType.ETCS_GUI
-            )
-        assert(guiCurve.beginPos >= 0.0 && guiCurve.endPos == targetPosition)
-        assert((guiCurve.beginSpeed == maxSpeedEbd || guiCurve.beginPos == 0.0))
-
-        val ebiCurve = computeEbiBrakingCurveFromEbd(context, ebdCurve, targetSpeed)
-        assert(ebiCurve.endSpeed == targetSpeed)
-
         val fullIndicationCurve =
-            computeIndicationBrakingCurveFromRef(context, ebiCurve, BrakingCurveType.EBI, guiCurve)
+            computeEbdFullIndicationCurve(context, targetPosition, targetSpeed, maxSpeedEnvelope)
         val endOfIndicationCurve = fullIndicationCurve.endPos
         assert(endOfIndicationCurve <= targetPosition)
-        assert(fullIndicationCurve.endSpeed == targetSpeed)
 
         val fullIndCurveWithMaintain: Envelope
         if (endOfIndicationCurve < targetPosition) {
@@ -203,6 +168,123 @@ fun addBrakingCurvesAtLOAs(
     return envelopeWithLoaBrakingCurves
 }
 
+/** Compute SBD-based full indication curve. */
+private fun computeSbdFullIndicationCurve(
+    context: EnvelopeSimContext,
+    targetPosition: Double,
+    maxSpeedEnvelope: Double
+): EnvelopePart {
+    val targetSpeed = 0.0
+    val overhead =
+        Envelope.make(
+            EnvelopePart.generateTimes(
+                listOf(EnvelopeProfile.CONSTANT_SPEED),
+                doubleArrayOf(0.0, targetPosition),
+                doubleArrayOf(maxSpeedEnvelope, maxSpeedEnvelope)
+            )
+        )
+    val sbdCurve =
+        computeBrakingCurve(context, overhead, targetPosition, targetSpeed, BrakingType.ETCS_SBD)
+    assert(sbdCurve.beginPos >= 0 && sbdCurve.endPos == targetPosition)
+    assert(sbdCurve.endSpeed == targetSpeed)
+
+    val guiCurve =
+        computeBrakingCurve(context, overhead, targetPosition, targetSpeed, BrakingType.ETCS_GUI)
+    assert(guiCurve.beginPos >= 0.0 && guiCurve.endPos == targetPosition)
+    assert((guiCurve.beginSpeed == maxSpeedEnvelope || guiCurve.beginPos == 0.0))
+    assert(guiCurve.endSpeed == targetSpeed)
+
+    val fullIndicationCurve =
+        computeIndicationBrakingCurveFromRef(context, sbdCurve, BrakingCurveType.SBD, guiCurve)
+    assert(fullIndicationCurve.endPos == targetPosition)
+    assert(fullIndicationCurve.endSpeed == targetSpeed)
+    return fullIndicationCurve
+}
+
+/** Compute EBD-based full indication curve. */
+private fun computeEbdFullIndicationCurve(
+    context: EnvelopeSimContext,
+    targetPosition: Double,
+    targetSpeed: Double,
+    maxSpeedEnvelope: Double
+): EnvelopePart {
+    // Add maxBecDeltaSpeed to EBD curve overhead so it reaches a sufficiently high speed to
+    // guarantee that, after the speed translation, the corresponding EBI curve does intersect
+    // with envelope max speed.
+    val maxBecDeltaSpeed = maxBecDeltaSpeed()
+    val maxSpeedEbd = maxSpeedEnvelope + maxBecDeltaSpeed
+    val overhead =
+        Envelope.make(
+            EnvelopePart.generateTimes(
+                listOf(EnvelopeProfile.CONSTANT_SPEED),
+                doubleArrayOf(0.0, max(context.path.length, targetPosition)),
+                doubleArrayOf(maxSpeedEbd, maxSpeedEbd)
+            )
+        )
+
+    val ebdCurve =
+        computeBrakingCurve(context, overhead, targetPosition, targetSpeed, BrakingType.ETCS_EBD)
+    assert(ebdCurve.beginPos >= 0.0 && ebdCurve.endPos >= targetPosition)
+    // TODO: uncomment once ebd is not shifted anymore for an LoA
+    // assert((ebdCurve.beginSpeed == maxSpeedEbd || ebdCurve.beginPos == 0.0))
+
+    val guiCurve =
+        computeBrakingCurve(context, overhead, targetPosition, targetSpeed, BrakingType.ETCS_GUI)
+    assert(guiCurve.beginPos >= 0.0 && guiCurve.endPos == targetPosition)
+    assert((guiCurve.beginSpeed == maxSpeedEbd || guiCurve.beginPos == 0.0))
+
+    val ebiCurve = computeEbiBrakingCurveFromEbd(context, ebdCurve, targetSpeed)
+    assert(ebiCurve.endSpeed == targetSpeed)
+
+    val fullIndicationCurve =
+        computeIndicationBrakingCurveFromRef(context, ebiCurve, BrakingCurveType.EBI, guiCurve)
+    assert(fullIndicationCurve.endSpeed == targetSpeed)
+    return fullIndicationCurve
+}
+
+private fun computeSvlIndicationCurve(
+    fullIndicationCurveSvl: EnvelopePart,
+    targetPosition: Double,
+    overlay: Envelope,
+    beginPos: Double
+): EnvelopePart? {
+    // If no part of the indication curve is before the target position, ignore it.
+    if (fullIndicationCurveSvl.beginPos >= targetPosition) return null
+    val fullIndCurveSvlWithMaintain: Envelope
+    val releaseSpeedPosition =
+        if (fullIndicationCurveSvl.maxSpeed >= releaseSpeed)
+            fullIndicationCurveSvl.interpolatePosition(releaseSpeed)
+        else fullIndicationCurveSvl.beginPos
+    if (releaseSpeedPosition >= targetPosition) {
+        fullIndCurveSvlWithMaintain =
+            Envelope.make(
+                fullIndicationCurveSvl.slice(fullIndicationCurveSvl.beginPos, targetPosition)
+            )
+    } else {
+        val maintainReleaseSpeedCurve =
+            EnvelopePart.generateTimes(
+                listOf(EnvelopeProfile.CONSTANT_SPEED),
+                doubleArrayOf(releaseSpeedPosition, targetPosition),
+                doubleArrayOf(releaseSpeed, releaseSpeed)
+            )
+        fullIndCurveSvlWithMaintain =
+            if (releaseSpeedPosition == fullIndicationCurveSvl.beginPos) {
+                Envelope.make(maintainReleaseSpeedCurve)
+            } else {
+                Envelope.make(
+                    fullIndicationCurveSvl.sliceWithSpeeds(
+                        fullIndicationCurveSvl.beginPos,
+                        fullIndicationCurveSvl.beginSpeed,
+                        releaseSpeedPosition,
+                        releaseSpeed
+                    ),
+                    maintainReleaseSpeedCurve
+                )
+            }
+    }
+    return keepBrakingCurveUnderOverlay(fullIndCurveSvlWithMaintain, overlay, beginPos)
+}
+
 /** Compute braking curve: used to compute EBD, SBD or GUI. */
 private fun computeBrakingCurve(
     context: EnvelopeSimContext,
@@ -217,10 +299,10 @@ private fun computeBrakingCurve(
             brakingType == BrakingType.ETCS_GUI
     )
     // If the stopPosition is after the end of the path, the input is invalid except if it is an
-    // SVL, i.e. the target speed is 0 and the curve to compute is an EBD.
+    // SVL, i.e. the target speed is 0 and the curve to compute is not an SBD.
     if (
         (targetPosition > context.path.length &&
-            !(targetSpeed == 0.0 && brakingType == BrakingType.ETCS_EBD))
+            (targetSpeed != 0.0 || brakingType == BrakingType.ETCS_SBD))
     )
         throw RuntimeException(
             String.format(
@@ -282,15 +364,21 @@ private fun computeEbiBrakingCurveFromEbd(
     targetSpeed: Double
 ): EnvelopePart {
     val pointCount = ebdCurve.pointCount()
-    val newPositions = DoubleArray(pointCount)
-    val newSpeeds = DoubleArray(pointCount)
-    for (i in 0 until ebdCurve.pointCount()) {
+    var newPositions = DoubleArray(pointCount)
+    var newSpeeds = DoubleArray(pointCount)
+    for (i in 0 until pointCount) {
         val speed = ebdCurve.getPointSpeed(i)
         val becParams = computeBecParams(context, ebdCurve, speed, targetSpeed)
         val newPos = ebdCurve.getPointPos(i) - becParams.dBec
         val newSpeed = speed - becParams.deltaBecSpeed
         newPositions[i] = newPos
-        newSpeeds[i] = newSpeed
+        // TODO: unneeded for now: interpolate to not approximate position at 0 m/s.
+        newSpeeds[i] = max(newSpeed, 0.0)
+        if (newSpeed <= 0.0 && i < pointCount - 1) {
+            newPositions = newPositions.dropLast(pointCount - 1 - i).toDoubleArray()
+            newSpeeds = newSpeeds.dropLast(pointCount - 1 - i).toDoubleArray()
+            break
+        }
     }
 
     val fullBrakingCurve =
@@ -353,12 +441,25 @@ private fun keepBrakingCurveUnderOverlay(
     overlay: Envelope,
     beginPos: Double
 ): EnvelopePart? {
-    if (fullBrakingCurve.endPos <= beginPos) {
+    if (fullBrakingCurve.beginPos >= overlay.endPos || fullBrakingCurve.endPos <= beginPos) {
         etcsBrakingCurvesLogger.warn(
-            "The position-range of the ETCS braking curve ending at ($beginPos, ${fullBrakingCurve.endSpeed}) does not intersect with the overlay envelope's position-range."
+            "The position-range of the ETCS braking curve starting at (${fullBrakingCurve.beginPos}, ${fullBrakingCurve.beginSpeed}) and ending at (${fullBrakingCurve.endPos}, ${fullBrakingCurve.endSpeed}) does not intersect with the overlay envelope's position-range."
         )
         return null
     }
+    if (
+        fullBrakingCurve.minSpeed >
+            Envelope.make(
+                    *overlay.slice(
+                        max(fullBrakingCurve.beginPos, beginPos),
+                        min(fullBrakingCurve.endPos, overlay.endPos)
+                    )
+                )
+                .maxSpeed
+    ) {
+        // The full braking curve is above the overlay envelope: nothing to do here.
+        return null
+    }
 
     // Remove duplicate point part transitions: the last point of the previous array is the first
     // point of the next array. Otherwise, we would be adding two following identical points with
@@ -383,11 +484,23 @@ private fun keepBrakingCurveUnderOverlay(
     val overlayBuilder =
         ConstrainedEnvelopePartBuilder(
             partBuilder,
-            PositionConstraint(beginPos, overlay.endPos),
+            PositionConstraint(max(beginPos, fullBrakingCurve.beginPos), overlay.endPos),
             EnvelopeConstraint(overlay, EnvelopePartConstraintType.CEILING)
         )
-    overlayBuilder.initEnvelopePart(positions[nbPoints - 1], speeds[nbPoints - 1], -1.0)
-    for (i in nbPoints - 2 downTo 0) {
+    // Find the last point of the braking curve which is located below the overlay envelope, and
+    // init the overlay builder there.
+    var lastIndex = nbPoints - 1
+    while (
+        speeds[lastIndex] >
+            overlay
+                .get(overlay.findRightDir(positions[lastIndex], -1.0))
+                .interpolateSpeed(positions[lastIndex])
+    ) {
+        lastIndex--
+        if (lastIndex == 0) return null
+    }
+    overlayBuilder.initEnvelopePart(positions[lastIndex], speeds[lastIndex], -1.0)
+    for (i in lastIndex - 1 downTo 0) {
         if (!overlayBuilder.addStep(positions[i], speeds[i], timeDeltas[i])) break
     }
     return partBuilder.build()