Skip to content

Commit 40ddf7d

Browse files
ecktereckter
committed
core: fix pathfinding waypoints at track transitions
1 parent 746bfcc commit 40ddf7d

File tree

6 files changed

+89
-46
lines changed

6 files changed

+89
-46
lines changed

‎core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt

+2-3
Original file line numberDiff line numberDiff line change
@@ -255,14 +255,13 @@ fun buildChunkPath(
255255
var mutBeginOffset = pathBeginOffset
256256
var mutEndOffset = pathEndOffset
257257
for (dirChunkId in chunks) {
258-
if (totalBlocksLength >= pathEndOffset)
258+
if (totalBlocksLength > pathEndOffset)
259259
break
260260
val length = infra.getTrackChunkLength(dirChunkId.value)
261261
val blockEndOffset = totalBlocksLength + length.distance
262262

263263
// if the block ends before the path starts, it can be safely skipped
264-
// If a block ends where the path starts, it can be skipped too
265-
if (pathBeginOffset >= blockEndOffset) {
264+
if (pathBeginOffset > blockEndOffset) {
266265
mutBeginOffset -= length.distance
267266
mutEndOffset -= length.distance
268267
} else {

‎core/osrd-geom/src/main/java/fr/sncf/osrd/geom/LineString.java

-1
Original file line numberDiff line numberDiff line change
@@ -206,7 +206,6 @@ public Point interpolateNormalized(double distance) {
206206
public LineString slice(double begin, double end) {
207207
assert begin >= 0 && begin <= 1;
208208
assert end >= 0 && end <= 1;
209-
assert Double.compare(begin, end) != 0;
210209

211210
if (begin > end)
212211
return slice(end, begin).reverse();

‎core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingBlocksEndpoint.kt

+1-29
Original file line numberDiff line numberDiff line change
@@ -96,7 +96,7 @@ private fun getRouteDirTracks(rawInfra: RawSignalingInfra, routeId: RouteId): Di
9696
return res
9797
}
9898

99-
private fun validatePathfindingResult(
99+
fun validatePathfindingResult(
100100
res: PathfindingResult, reqWaypoints: Array<Array<PathfindingWaypoint>>,
101101
rawInfra: RawSignalingInfra
102102
) {
@@ -112,7 +112,6 @@ private fun validatePathfindingResult(
112112
.flatMap { route: RJSRoutePath? -> route!!.trackSections.stream() }
113113
.toList()
114114
assertPathTracksAreComplete(tracksOnPath, rawInfra)
115-
assertRequiredWaypointsOnPathTracks(reqWaypoints, tracksOnPath, res.pathWaypoints)
116115
}
117116

118117
private fun assertPathRoutesAreAdjacent(routeTracks: DirStaticIdxList<TrackSection>, rawInfra: RawSignalingInfra) {
@@ -173,33 +172,6 @@ private fun assertPathTracksAreComplete(tracksOnPath: List<RJSDirectionalTrackRa
173172
}
174173
}
175174

176-
private fun assertRequiredWaypointsOnPathTracks(
177-
reqWaypoints: Array<Array<PathfindingWaypoint>>,
178-
tracksOnPath: List<RJSDirectionalTrackRange>,
179-
pathWaypoints: List<PathWaypointResult>
180-
) {
181-
// Checks that at least one waypoint of each step is on the path
182-
assert(Arrays.stream(reqWaypoints).allMatch { step: Array<PathfindingWaypoint>? ->
183-
Arrays.stream(step)
184-
.anyMatch { waypoint: PathfindingWaypoint ->
185-
tracksOnPath.stream()
186-
.anyMatch { trackOnPath: RJSDirectionalTrackRange -> isWaypointOnTrack(waypoint, trackOnPath) }
187-
}
188-
}) { "The path does not contain one of the wanted steps" }
189-
for (waypoint in pathWaypoints) {
190-
val loc = waypoint.location
191-
assert(tracksOnPath.stream()
192-
.filter { range: RJSDirectionalTrackRange -> range.trackSectionID == loc.trackSection }
193-
.anyMatch { range: RJSDirectionalTrackRange ->
194-
((loc.offset > range.begin || TrainPhysicsIntegrator.arePositionsEqual(loc.offset, range.begin))
195-
&& (loc.offset < range.end || TrainPhysicsIntegrator.arePositionsEqual(
196-
loc.offset,
197-
range.end
198-
)))
199-
}) { "A waypoint isn't included in the track path" }
200-
}
201-
}
202-
203175
private fun isWaypointOnTrack(waypoint: PathfindingWaypoint, track: RJSDirectionalTrackRange): Boolean {
204176
return (track.trackSectionID == waypoint.trackSection && track.direction == waypoint.direction
205177
&& (track.begin <= waypoint.offset || abs(track.begin - waypoint.offset) < 1e-3)

‎core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingResultConverter.kt

+25-9
Original file line numberDiff line numberDiff line change
@@ -81,7 +81,7 @@ private fun makeUserDefinedWaypoints(
8181
for (waypoint in userDefinedWaypointsPerBlock.getOrDefault(blockRange.edge, ArrayList())) {
8282
if (blockRange.start <= waypoint && waypoint <= blockRange.end) {
8383
val pathOffset = lengthPrevBlocks + waypoint.distance - startFirstRange.distance
84-
res.add(makePendingWaypoint(infra, path, false, pathOffset, null))
84+
res.add(makePendingUserDefinedWaypoint(infra, path, pathOffset))
8585
}
8686
}
8787
lengthPrevBlocks += blockInfra.getBlockLength(blockRange.edge).distance
@@ -96,27 +96,43 @@ fun makeOperationalPoints(
9696
): Collection<PathWaypointResult> {
9797
val res = ArrayList<PathWaypointResult>()
9898
for ((opId, offset) in path.getOperationalPointParts()) {
99-
val opName = infra.getOperationalPointPartName(opId)
100-
res.add(makePendingWaypoint(infra, path, true, offset, opName))
99+
res.add(makePendingOPWaypoint(infra, offset, opId))
101100
}
102101
return res
103102
}
104103

104+
/** Creates a pending waypoint from an operational point part */
105+
private fun makePendingOPWaypoint(
106+
infra: RawSignalingInfra,
107+
pathOffset: Offset<Path>,
108+
opId: OperationalPointPartId
109+
): PathWaypointResult {
110+
val partChunk = infra.getOperationalPointPartChunk(opId)
111+
val partChunkOffset = infra.getOperationalPointPartChunkOffset(opId)
112+
val opName = infra.getOperationalPointPartName(opId)
113+
val trackId = infra.getTrackFromChunk(partChunk)
114+
val trackOffset = partChunkOffset + infra.getTrackChunkOffset(partChunk).distance
115+
val trackName = infra.getTrackSectionName(trackId)
116+
val location = PathWaypointLocation(
117+
trackName,
118+
trackOffset.distance.meters
119+
)
120+
return PathWaypointResult(location, pathOffset.distance.meters, true, opName)
121+
}
122+
105123
/** Creates a pending waypoint from a path and its offset */
106-
private fun makePendingWaypoint(
124+
private fun makePendingUserDefinedWaypoint(
107125
infra: RawSignalingInfra,
108126
path: PathProperties,
109-
suggestion: Boolean,
110-
pathOffset: Offset<Path>,
111-
opName: String?
127+
pathOffset: Offset<Path>
112128
): PathWaypointResult {
113129
val (trackId, offset) = path.getTrackLocationAtOffset(pathOffset)
114130
val trackName = infra.getTrackSectionName(trackId)
115131
val location = PathWaypointLocation(
116132
trackName,
117133
offset.distance.meters
118134
)
119-
return PathWaypointResult(location, pathOffset.distance.meters, suggestion, opName)
135+
return PathWaypointResult(location, pathOffset.distance.meters, false, null)
120136
}
121137

122138
/** Sorts the waypoints on the path. When waypoints overlap, the user-defined one is kept. */
@@ -259,7 +275,7 @@ private fun makeRJSTrackRanges(
259275
val dirEndOfRouteRange = dirTrackChunkOffset + routeEndOffset.distance - chunkStartPathOffset.distance
260276
val dirRangeStartOnTrack = Offset.max(dirTrackChunkOffset, dirStartOfRouteRange)
261277
val dirRangeEndOnTrack = Offset.min(dirTrackChunkOffset + chunkLength.distance, dirEndOfRouteRange)
262-
if (dirRangeStartOnTrack < dirRangeEndOnTrack) {
278+
if (dirRangeStartOnTrack <= dirRangeEndOnTrack) {
263279
val trackName = infra.getTrackSectionName(trackId)
264280
val direction =
265281
if (dirChunkId.direction === Direction.INCREASING)

‎core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/response/PathWaypointResult.kt

+1
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ class PathWaypointResult(
4646
return
4747
pathOffset = other.pathOffset
4848
id = other.id
49+
location = other.location
4950
}
5051

5152
override fun equals(other: Any?): Boolean {

‎core/src/test/kotlin/fr/sncf/osrd/pathfinding/PathfindingTest.kt

+60-4
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@ package fr.sncf.osrd.pathfinding
22

33
import fr.sncf.osrd.api.ApiTest
44
import fr.sncf.osrd.api.pathfinding.PathfindingBlocksEndpoint
5+
import fr.sncf.osrd.api.pathfinding.convertPathfindingResult
56
import fr.sncf.osrd.api.pathfinding.request.PathfindingRequest
67
import fr.sncf.osrd.api.pathfinding.request.PathfindingWaypoint
78
import fr.sncf.osrd.api.pathfinding.response.CurveChartPointResult
@@ -10,18 +11,18 @@ import fr.sncf.osrd.api.pathfinding.response.PathWaypointResult.PathWaypointLoca
1011
import fr.sncf.osrd.api.pathfinding.response.PathfindingResult
1112
import fr.sncf.osrd.api.pathfinding.response.SlopeChartPointResult
1213
import fr.sncf.osrd.api.pathfinding.runPathfinding
14+
import fr.sncf.osrd.api.pathfinding.validatePathfindingResult
1315
import fr.sncf.osrd.cli.StandaloneSimulationCommand
1416
import fr.sncf.osrd.railjson.schema.common.graph.ApplicableDirection
1517
import fr.sncf.osrd.railjson.schema.common.graph.EdgeDirection
18+
import fr.sncf.osrd.railjson.schema.infra.RJSOperationalPoint
1619
import fr.sncf.osrd.railjson.schema.infra.RJSRoutePath
1720
import fr.sncf.osrd.railjson.schema.infra.RJSTrackSection
18-
import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSApplicableDirectionsTrackRange
19-
import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSCatenary
20-
import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSDirectionalTrackRange
21-
import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSLoadingGaugeLimit
21+
import fr.sncf.osrd.railjson.schema.infra.trackranges.*
2222
import fr.sncf.osrd.railjson.schema.rollingstock.RJSLoadingGaugeType
2323
import fr.sncf.osrd.reporting.exceptions.ErrorType
2424
import fr.sncf.osrd.reporting.exceptions.OSRDError
25+
import fr.sncf.osrd.reporting.warnings.DiagnosticRecorderImpl
2526
import fr.sncf.osrd.train.TestTrains
2627
import fr.sncf.osrd.utils.Helpers
2728
import fr.sncf.osrd.utils.moshi.MoshiUtils
@@ -43,6 +44,7 @@ import java.util.stream.Collectors
4344
import java.util.stream.Stream
4445
import kotlin.math.max
4546
import kotlin.math.min
47+
import kotlin.test.assertEquals
4648

4749
@TestInstance(TestInstance.Lifecycle.PER_CLASS)
4850
class PathfindingTest : ApiTest() {
@@ -624,6 +626,60 @@ class PathfindingTest : ApiTest() {
624626
)
625627
}
626628

629+
@Test
630+
fun pathStartingAtTrackEdge() {
631+
/*
632+
foo_a foo_to_bar bar_a
633+
------>|----------->|------>
634+
^ ^
635+
new_op_1 new_op_2
636+
*/
637+
val waypointStart = PathfindingWaypoint("ne.micro.foo_a", 200.0, EdgeDirection.START_TO_STOP)
638+
val waypointEnd = PathfindingWaypoint("ne.micro.bar_a", 0.0, EdgeDirection.START_TO_STOP)
639+
val waypoints = Array(2) { Array(1) { waypointStart } }
640+
waypoints[1][0] = waypointEnd
641+
val rjsInfra = Helpers.getExampleInfra("tiny_infra/infra.json")
642+
rjsInfra.operationalPoints.add(
643+
RJSOperationalPoint(
644+
"new_op_1", listOf(
645+
RJSOperationalPointPart("ne.micro.foo_a", 200.0)
646+
)
647+
)
648+
)
649+
rjsInfra.operationalPoints.add(
650+
RJSOperationalPoint(
651+
"new_op_2", listOf(
652+
RJSOperationalPointPart("ne.micro.bar_a", 0.0)
653+
)
654+
)
655+
)
656+
val infra = Helpers.fullInfraFromRJS(rjsInfra)
657+
658+
val path = runPathfinding(
659+
infra,
660+
waypoints,
661+
listOf(TestTrains.REALISTIC_FAST_TRAIN)
662+
)
663+
val res = convertPathfindingResult(
664+
infra.blockInfra, infra.rawInfra,
665+
path, DiagnosticRecorderImpl(true)
666+
)
667+
validatePathfindingResult(res, waypoints, infra.rawInfra)
668+
assertEquals(
669+
listOf(
670+
PathWaypointResult(
671+
PathWaypointLocation("ne.micro.foo_a", 200.0),
672+
0.0, false, "new_op_1"
673+
),
674+
PathWaypointResult(
675+
PathWaypointLocation("ne.micro.bar_a", 0.0),
676+
10_000.0, false, "new_op_2"
677+
),
678+
),
679+
res.pathWaypoints
680+
)
681+
}
682+
627683
companion object {
628684
private const val SIGNALING_TYPE = "BAL3"
629685
private fun makeBidirectionalEndPoint(point: PathfindingWaypoint): Array<PathfindingWaypoint> {

0 commit comments

Comments
 (0)