From 76a9bc01433f8f8546957f536be2479a6514770f Mon Sep 17 00:00:00 2001 From: Eloi Charpentier Date: Thu, 7 Dec 2023 15:16:23 +0100 Subject: [PATCH] core: fix pathfinding waypoints at track transitions --- .../osrd/sim_infra/impl/PathPropertiesImpl.kt | 5 +- .../java/fr/sncf/osrd/geom/LineString.java | 1 - .../pathfinding/PathfindingBlocksEndpoint.kt | 30 +-------- .../pathfinding/PathfindingResultConverter.kt | 34 +++++++--- .../response/PathWaypointResult.kt | 1 + .../sncf/osrd/pathfinding/PathfindingTest.kt | 64 +++++++++++++++++-- 6 files changed, 89 insertions(+), 46 deletions(-) diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt index 6cbecd85aa6..7bb0c2c2694 100644 --- a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt +++ b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt @@ -255,14 +255,13 @@ fun buildChunkPath( var mutBeginOffset = pathBeginOffset var mutEndOffset = pathEndOffset for (dirChunkId in chunks) { - if (totalBlocksLength >= pathEndOffset) + if (totalBlocksLength > pathEndOffset) break val length = infra.getTrackChunkLength(dirChunkId.value) val blockEndOffset = totalBlocksLength + length.distance // if the block ends before the path starts, it can be safely skipped - // If a block ends where the path starts, it can be skipped too - if (pathBeginOffset >= blockEndOffset) { + if (pathBeginOffset > blockEndOffset) { mutBeginOffset -= length.distance mutEndOffset -= length.distance } else { diff --git a/core/osrd-geom/src/main/java/fr/sncf/osrd/geom/LineString.java b/core/osrd-geom/src/main/java/fr/sncf/osrd/geom/LineString.java index 7e98893715d..0384a29173b 100644 --- a/core/osrd-geom/src/main/java/fr/sncf/osrd/geom/LineString.java +++ b/core/osrd-geom/src/main/java/fr/sncf/osrd/geom/LineString.java @@ -206,7 +206,6 @@ public Point interpolateNormalized(double distance) { public LineString slice(double begin, double end) { assert begin >= 0 && begin <= 1; assert end >= 0 && end <= 1; - assert Double.compare(begin, end) != 0; if (begin > end) return slice(end, begin).reverse(); diff --git a/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingBlocksEndpoint.kt b/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingBlocksEndpoint.kt index bb022990b25..b853eff98fa 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingBlocksEndpoint.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingBlocksEndpoint.kt @@ -96,7 +96,7 @@ private fun getRouteDirTracks(rawInfra: RawSignalingInfra, routeId: RouteId): Di return res } -private fun validatePathfindingResult( +fun validatePathfindingResult( res: PathfindingResult, reqWaypoints: Array>, rawInfra: RawSignalingInfra ) { @@ -112,7 +112,6 @@ private fun validatePathfindingResult( .flatMap { route: RJSRoutePath? -> route!!.trackSections.stream() } .toList() assertPathTracksAreComplete(tracksOnPath, rawInfra) - assertRequiredWaypointsOnPathTracks(reqWaypoints, tracksOnPath, res.pathWaypoints) } private fun assertPathRoutesAreAdjacent(routeTracks: DirStaticIdxList, rawInfra: RawSignalingInfra) { @@ -173,33 +172,6 @@ private fun assertPathTracksAreComplete(tracksOnPath: List>, - tracksOnPath: List, - pathWaypoints: List -) { - // Checks that at least one waypoint of each step is on the path - assert(Arrays.stream(reqWaypoints).allMatch { step: Array? -> - Arrays.stream(step) - .anyMatch { waypoint: PathfindingWaypoint -> - tracksOnPath.stream() - .anyMatch { trackOnPath: RJSDirectionalTrackRange -> isWaypointOnTrack(waypoint, trackOnPath) } - } - }) { "The path does not contain one of the wanted steps" } - for (waypoint in pathWaypoints) { - val loc = waypoint.location - assert(tracksOnPath.stream() - .filter { range: RJSDirectionalTrackRange -> range.trackSectionID == loc.trackSection } - .anyMatch { range: RJSDirectionalTrackRange -> - ((loc.offset > range.begin || TrainPhysicsIntegrator.arePositionsEqual(loc.offset, range.begin)) - && (loc.offset < range.end || TrainPhysicsIntegrator.arePositionsEqual( - loc.offset, - range.end - ))) - }) { "A waypoint isn't included in the track path" } - } -} - private fun isWaypointOnTrack(waypoint: PathfindingWaypoint, track: RJSDirectionalTrackRange): Boolean { return (track.trackSectionID == waypoint.trackSection && track.direction == waypoint.direction && (track.begin <= waypoint.offset || abs(track.begin - waypoint.offset) < 1e-3) diff --git a/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingResultConverter.kt b/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingResultConverter.kt index 4aaf1ede37c..cb023755dd9 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingResultConverter.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingResultConverter.kt @@ -81,7 +81,7 @@ private fun makeUserDefinedWaypoints( for (waypoint in userDefinedWaypointsPerBlock.getOrDefault(blockRange.edge, ArrayList())) { if (blockRange.start <= waypoint && waypoint <= blockRange.end) { val pathOffset = lengthPrevBlocks + waypoint.distance - startFirstRange.distance - res.add(makePendingWaypoint(infra, path, false, pathOffset, null)) + res.add(makePendingUserDefinedWaypoint(infra, path, pathOffset)) } } lengthPrevBlocks += blockInfra.getBlockLength(blockRange.edge).distance @@ -96,19 +96,35 @@ fun makeOperationalPoints( ): Collection { val res = ArrayList() for ((opId, offset) in path.getOperationalPointParts()) { - val opName = infra.getOperationalPointPartName(opId) - res.add(makePendingWaypoint(infra, path, true, offset, opName)) + res.add(makePendingOPWaypoint(infra, offset, opId)) } return res } +/** Creates a pending waypoint from an operational point part */ +private fun makePendingOPWaypoint( + infra: RawSignalingInfra, + pathOffset: Offset, + opId: OperationalPointPartId +): PathWaypointResult { + val partChunk = infra.getOperationalPointPartChunk(opId) + val partChunkOffset = infra.getOperationalPointPartChunkOffset(opId) + val opName = infra.getOperationalPointPartName(opId) + val trackId = infra.getTrackFromChunk(partChunk) + val trackOffset = partChunkOffset + infra.getTrackChunkOffset(partChunk).distance + val trackName = infra.getTrackSectionName(trackId) + val location = PathWaypointLocation( + trackName, + trackOffset.distance.meters + ) + return PathWaypointResult(location, pathOffset.distance.meters, true, opName) +} + /** Creates a pending waypoint from a path and its offset */ -private fun makePendingWaypoint( +private fun makePendingUserDefinedWaypoint( infra: RawSignalingInfra, path: PathProperties, - suggestion: Boolean, - pathOffset: Offset, - opName: String? + pathOffset: Offset ): PathWaypointResult { val (trackId, offset) = path.getTrackLocationAtOffset(pathOffset) val trackName = infra.getTrackSectionName(trackId) @@ -116,7 +132,7 @@ private fun makePendingWaypoint( trackName, offset.distance.meters ) - return PathWaypointResult(location, pathOffset.distance.meters, suggestion, opName) + return PathWaypointResult(location, pathOffset.distance.meters, false, null) } /** Sorts the waypoints on the path. When waypoints overlap, the user-defined one is kept. */ @@ -259,7 +275,7 @@ private fun makeRJSTrackRanges( val dirEndOfRouteRange = dirTrackChunkOffset + routeEndOffset.distance - chunkStartPathOffset.distance val dirRangeStartOnTrack = Offset.max(dirTrackChunkOffset, dirStartOfRouteRange) val dirRangeEndOnTrack = Offset.min(dirTrackChunkOffset + chunkLength.distance, dirEndOfRouteRange) - if (dirRangeStartOnTrack < dirRangeEndOnTrack) { + if (dirRangeStartOnTrack <= dirRangeEndOnTrack) { val trackName = infra.getTrackSectionName(trackId) val direction = if (dirChunkId.direction === Direction.INCREASING) diff --git a/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/response/PathWaypointResult.kt b/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/response/PathWaypointResult.kt index 41d0af29be1..3ece527a01f 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/response/PathWaypointResult.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/response/PathWaypointResult.kt @@ -46,6 +46,7 @@ class PathWaypointResult( return pathOffset = other.pathOffset id = other.id + location = other.location } override fun equals(other: Any?): Boolean { diff --git a/core/src/test/kotlin/fr/sncf/osrd/pathfinding/PathfindingTest.kt b/core/src/test/kotlin/fr/sncf/osrd/pathfinding/PathfindingTest.kt index da0dead0e88..6ff2c3af472 100644 --- a/core/src/test/kotlin/fr/sncf/osrd/pathfinding/PathfindingTest.kt +++ b/core/src/test/kotlin/fr/sncf/osrd/pathfinding/PathfindingTest.kt @@ -2,6 +2,7 @@ package fr.sncf.osrd.pathfinding import fr.sncf.osrd.api.ApiTest import fr.sncf.osrd.api.pathfinding.PathfindingBlocksEndpoint +import fr.sncf.osrd.api.pathfinding.convertPathfindingResult import fr.sncf.osrd.api.pathfinding.request.PathfindingRequest import fr.sncf.osrd.api.pathfinding.request.PathfindingWaypoint import fr.sncf.osrd.api.pathfinding.response.CurveChartPointResult @@ -10,18 +11,18 @@ import fr.sncf.osrd.api.pathfinding.response.PathWaypointResult.PathWaypointLoca import fr.sncf.osrd.api.pathfinding.response.PathfindingResult import fr.sncf.osrd.api.pathfinding.response.SlopeChartPointResult import fr.sncf.osrd.api.pathfinding.runPathfinding +import fr.sncf.osrd.api.pathfinding.validatePathfindingResult import fr.sncf.osrd.cli.StandaloneSimulationCommand import fr.sncf.osrd.railjson.schema.common.graph.ApplicableDirection import fr.sncf.osrd.railjson.schema.common.graph.EdgeDirection +import fr.sncf.osrd.railjson.schema.infra.RJSOperationalPoint import fr.sncf.osrd.railjson.schema.infra.RJSRoutePath import fr.sncf.osrd.railjson.schema.infra.RJSTrackSection -import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSApplicableDirectionsTrackRange -import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSCatenary -import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSDirectionalTrackRange -import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSLoadingGaugeLimit +import fr.sncf.osrd.railjson.schema.infra.trackranges.* import fr.sncf.osrd.railjson.schema.rollingstock.RJSLoadingGaugeType import fr.sncf.osrd.reporting.exceptions.ErrorType import fr.sncf.osrd.reporting.exceptions.OSRDError +import fr.sncf.osrd.reporting.warnings.DiagnosticRecorderImpl import fr.sncf.osrd.train.TestTrains import fr.sncf.osrd.utils.Helpers import fr.sncf.osrd.utils.moshi.MoshiUtils @@ -43,6 +44,7 @@ import java.util.stream.Collectors import java.util.stream.Stream import kotlin.math.max import kotlin.math.min +import kotlin.test.assertEquals @TestInstance(TestInstance.Lifecycle.PER_CLASS) class PathfindingTest : ApiTest() { @@ -624,6 +626,60 @@ class PathfindingTest : ApiTest() { ) } + @Test + fun pathStartingAtTrackEdge() { + /* + foo_a foo_to_bar bar_a + ------>|----------->|------> + ^ ^ + new_op_1 new_op_2 + */ + val waypointStart = PathfindingWaypoint("ne.micro.foo_a", 200.0, EdgeDirection.START_TO_STOP) + val waypointEnd = PathfindingWaypoint("ne.micro.bar_a", 0.0, EdgeDirection.START_TO_STOP) + val waypoints = Array(2) { Array(1) { waypointStart } } + waypoints[1][0] = waypointEnd + val rjsInfra = Helpers.getExampleInfra("tiny_infra/infra.json") + rjsInfra.operationalPoints.add( + RJSOperationalPoint( + "new_op_1", listOf( + RJSOperationalPointPart("ne.micro.foo_a", 200.0) + ) + ) + ) + rjsInfra.operationalPoints.add( + RJSOperationalPoint( + "new_op_2", listOf( + RJSOperationalPointPart("ne.micro.bar_a", 0.0) + ) + ) + ) + val infra = Helpers.fullInfraFromRJS(rjsInfra) + + val path = runPathfinding( + infra, + waypoints, + listOf(TestTrains.REALISTIC_FAST_TRAIN) + ) + val res = convertPathfindingResult( + infra.blockInfra, infra.rawInfra, + path, DiagnosticRecorderImpl(true) + ) + validatePathfindingResult(res, waypoints, infra.rawInfra) + assertEquals( + listOf( + PathWaypointResult( + PathWaypointLocation("ne.micro.foo_a", 200.0), + 0.0, false, "new_op_1" + ), + PathWaypointResult( + PathWaypointLocation("ne.micro.bar_a", 0.0), + 10_000.0, false, "new_op_2" + ), + ), + res.pathWaypoints + ) + } + companion object { private const val SIGNALING_TYPE = "BAL3" private fun makeBidirectionalEndPoint(point: PathfindingWaypoint): Array {