diff --git a/src/stores/TurnNavigationStore.ts b/src/stores/TurnNavigationStore.ts index c5d1ea8d..836c4a93 100644 --- a/src/stores/TurnNavigationStore.ts +++ b/src/stores/TurnNavigationStore.ts @@ -233,7 +233,7 @@ export default class TurnNavigationStore extends Store const coordinate = action.coordinate let path = state.activePath - let instrInfo = getCurrentInstruction(path.instructions, coordinate) + let instrInfo = getCurrentInstruction(path.instructions, coordinate, action.heading) // skip waypoint if close to it and next is available (either activePath has via points or initialPath) let skipWaypoint = TurnNavigationStore.skipWaypoint( @@ -250,7 +250,7 @@ export default class TurnNavigationStore extends Store ) { // switch back to original path and skip the current waypoint path = state.initialPath - instrInfo = getCurrentInstruction(path.instructions, coordinate) + instrInfo = getCurrentInstruction(path.instructions, coordinate, action.heading) skipWaypoint = TurnNavigationStore.skipWaypoint( state.instruction.distanceToWaypoint, TurnNavigationStore.getWaypoint(path, instrInfo.nextWaypointIndex), @@ -401,7 +401,7 @@ export default class TurnNavigationStore extends Store const path = action.path // ensure that path and instruction are synced - const instr = getCurrentInstruction(path.instructions, state.coordinate) + const instr = getCurrentInstruction(path.instructions, state.coordinate, state.heading) // current location is still not close if (instr.index < 0) { diff --git a/src/turnNavigation/GeoMethods.ts b/src/turnNavigation/GeoMethods.ts index 320ab168..c6c20c2b 100644 --- a/src/turnNavigation/GeoMethods.ts +++ b/src/turnNavigation/GeoMethods.ts @@ -31,7 +31,8 @@ export function getCurrentDetails(path: Path, pillarPoint: Coordinate, details: export function getCurrentInstruction( instructions: Instruction[], - location: Coordinate + location: Coordinate, + heading: undefined | number ): { index: number timeToTurn: number @@ -43,13 +44,9 @@ export function getCurrentInstruction( distanceToWaypoint: number nextWaypointIndex: number } { - let instructionIndex = -1 - let distanceToRoute = Number.MAX_VALUE - // TODO do we need to calculate the more precise route distance or is the current straight-line distance sufficient? - let distanceToTurn = -1 - let nextWaypointIndex = 0 let waypointIndex = 0 - let pillarPointOnRoute = { lat: 0, lng: 0 } + const result = new InstructionResult() + const resultWithHeadingFilter = new InstructionResult() for (let instrIdx = 0; instrIdx < instructions.length; instrIdx++) { const sign = instructions[instrIdx].sign @@ -59,41 +56,56 @@ export function getCurrentInstruction( for (let pIdx = 0; pIdx < points.length; pIdx++) { const p: number[] = points[pIdx] let snapped = { lat: p[1], lng: p[0] } + const last: number[] = points[points.length - 1] let dist = calcDist(snapped, location) - // calculate the snapped point, TODO use first point of next instruction for "next" if last point of current instruction + let headingMatches = false + // calculate the snapped point + // TODO use first point of next instruction for "next" if last point of current instruction if (pIdx + 1 < points.length) { const next: number[] = points[pIdx + 1] if (validEdgeDistance(location.lat, location.lng, p[1], p[0], next[1], next[0])) { snapped = calcCrossingPointToEdge(location.lat, location.lng, p[1], p[0], next[1], next[0]) dist = Math.min(dist, calcDist(snapped, location)) } + + if (heading) { + // TODO reject point based on heading if a similar close point is available + const tmpHeading = toDegrees(toNorthBased(calcOrientation(p[1], p[0], next[1], next[0]))) + headingMatches = Math.abs(heading - tmpHeading) < 40 + } } - if (dist < distanceToRoute) { - distanceToRoute = dist + const set = (res: InstructionResult) => { + res.distanceToRoute = dist // use next instruction or finish - instructionIndex = instrIdx + 1 < instructions.length ? instrIdx + 1 : instrIdx - const last: number[] = points[points.length - 1] - distanceToTurn = Math.round(calcDist({ lat: last[1], lng: last[0] }, snapped)) - nextWaypointIndex = waypointIndex + 1 - pillarPointOnRoute = { lat: p[1], lng: p[0] } + res.index = instrIdx + 1 < instructions.length ? instrIdx + 1 : instrIdx + res.distanceToTurn = Math.round(calcDist({ lat: last[1], lng: last[0] }, snapped)) + res.nextWaypointIndex = waypointIndex + 1 + res.pillarPointOnRoute = { lat: p[1], lng: p[0] } } + + if (dist < result.distanceToRoute) set(result) + if (dist < resultWithHeadingFilter.distanceToRoute && headingMatches) set(resultWithHeadingFilter) } } + const finalResult = + resultWithHeadingFilter.index >= 0 && resultWithHeadingFilter.distanceToRoute < 20 + ? resultWithHeadingFilter + : result let distanceToWaypoint = -1 let timeToTurn = 0 let timeToEnd = 0 - let distanceToEnd = distanceToTurn - if (instructionIndex >= 0) { - if (instructionIndex > 0) { + let distanceToEnd = finalResult.distanceToTurn + if (finalResult.index >= 0) { + if (finalResult.index > 0) { // proportional estimate the time to the next instruction, TODO use time from path details instead - let prevInstr = instructions[instructionIndex - 1] + let prevInstr = instructions[finalResult.index - 1] timeToTurn = prevInstr.distance > 0 ? prevInstr.time * (distanceToEnd / prevInstr.distance) : 0 } timeToEnd = timeToTurn - distanceToEnd = distanceToTurn - for (let instrIdx = instructionIndex; instrIdx < instructions.length; instrIdx++) { + distanceToEnd = finalResult.distanceToTurn + for (let instrIdx = finalResult.index; instrIdx < instructions.length; instrIdx++) { timeToEnd += instructions[instrIdx].time distanceToEnd += instructions[instrIdx].distance @@ -105,18 +117,23 @@ export function getCurrentInstruction( } return { - index: instructionIndex, + ...finalResult, timeToTurn, - distanceToTurn, - distanceToRoute, - pillarPointOnRoute, timeToEnd, distanceToEnd, distanceToWaypoint, - nextWaypointIndex, } } +class InstructionResult { + index = -1 + distanceToRoute = Number.MAX_VALUE + // TODO do we need to calculate the more precise route distance or is the current straight-line distance sufficient? + distanceToTurn = -1 + nextWaypointIndex = 0 + pillarPointOnRoute = { lat: 0, lng: 0 } +} + /** * Calculates the great-circle distance between two points on Earth given the latitudes and longitudes * assuming that Earth is a sphere with radius 6371km. The result is returned in meters. diff --git a/test/stores/TurnNavigationStore.test.ts b/test/stores/TurnNavigationStore.test.ts index 361d55a4..c22dbe2d 100644 --- a/test/stores/TurnNavigationStore.test.ts +++ b/test/stores/TurnNavigationStore.test.ts @@ -25,6 +25,8 @@ let reroute2 = toRoutingResult(require('../turnNavigation/reroute2.json')) let announceBug = toRoutingResult(require('../turnNavigation/announce-bug-original.json')) let announceBugReroute = toRoutingResult(require('../turnNavigation/announce-bug-reroute.json')) +let loopBug = toRoutingResult(require('../turnNavigation/loop-bug.json')) + function toRoutingResult(rawResult: RawResult): RoutingResult { return { ...rawResult, @@ -311,6 +313,49 @@ describe('TurnNavigationStore', () => { expect(speech.getTexts()).toEqual(['Links halten', 'reroute', 'Scharf links abbiegen']) }) + + it('do not announce too old instruction for loops', async () => { + const api = new LocalApi() + const speech = new DummySpeech() + const store = createStore(api, speech) + Dispatcher.dispatch(new SetVehicleProfile({ name: 'car' })) + Dispatcher.dispatch(new SetSelectedPath(loopBug.paths[0])) + Dispatcher.dispatch(new TurnNavigationSettingsUpdate({ soundEnabled: true } as TNSettingsState)) + Dispatcher.dispatch(new LocationUpdate({ lng: 11.97108, lat: 50.352875 }, true, 16, 135)) + expect(store.state.activePath).toEqual(loopBug.paths[0]) + Dispatcher.dispatch(new LocationUpdate({ lng: 11.972844, lat: 50.350855 }, true, 16, 180)) + + // GPS location is closer to incorrect (underlying) motorway than to bridge. + // Due to heading prefer the slightly more distant bridge + Dispatcher.dispatch(new LocationUpdate({ lng: 11.972071, lat: 50.351871 }, true, 16, 45)) + + expect(speech.getTexts()).toEqual([ + 'Keep right and take B 173 toward Hof-Zentrum, Feilitzsch, Trogen', + 'Turn right onto B 173', + 'Arrive at destination', + ]) + }) + + it('do not announce future instruction for loops', async () => { + const api = new LocalApi() + const speech = new DummySpeech() + const store = createStore(api, speech) + Dispatcher.dispatch(new SetVehicleProfile({ name: 'car' })) + Dispatcher.dispatch(new SetSelectedPath(loopBug.paths[0])) + Dispatcher.dispatch(new TurnNavigationSettingsUpdate({ soundEnabled: true } as TNSettingsState)) + Dispatcher.dispatch(new LocationUpdate({ lng: 11.97108, lat: 50.352875 }, true, 16, 135)) + expect(store.state.activePath).toEqual(loopBug.paths[0]) + + // GPS location is closer to bridge than to correct motorway -> with heading still enforces motorway + Dispatcher.dispatch(new LocationUpdate({ lng: 11.97213, lat: 50.351902 }, true, 16, 135)) + // trigger announcements to turn right + Dispatcher.dispatch(new LocationUpdate({ lng: 11.972865, lat: 50.350855 }, true, 16, 180)) + + expect(speech.getTexts()).toEqual([ + 'Keep right and take B 173 toward Hof-Zentrum, Feilitzsch, Trogen', + 'Turn right onto B 173', + ]) + }) }) function createStore(api: Api, speech = new DummySpeech()) { diff --git a/test/turnNavigation/GeoMethods.test.ts b/test/turnNavigation/GeoMethods.test.ts index 6360801a..73407e4c 100644 --- a/test/turnNavigation/GeoMethods.test.ts +++ b/test/turnNavigation/GeoMethods.test.ts @@ -11,10 +11,14 @@ describe('calculate instruction', () => { // http://localhost:3000/?point=51.437233%2C14.246489&point=51.435514%2C14.239923&profile=car it('second instruction should not be "right turn"', () => { let path = ApiImpl.decodeResult(responseHoyerswerda1, true)[0] - const { index, distanceToTurn, timeToEnd, distanceToEnd } = getCurrentInstruction(path.instructions, { - lat: 51.435029, - lng: 14.243259, - }) + const { index, distanceToTurn, timeToEnd, distanceToEnd } = getCurrentInstruction( + path.instructions, + { + lat: 51.435029, + lng: 14.243259, + }, + undefined + ) expect(distanceToTurn).toEqual(236) expect(distanceToEnd).toEqual(236) @@ -25,10 +29,14 @@ describe('calculate instruction', () => { it('remaining time should be correct', () => { let path = ApiImpl.decodeResult(responseHoyerswerda1, true)[0] - const { timeToEnd, distanceToEnd } = getCurrentInstruction(path.instructions, { - lat: 51.439291, - lng: 14.245254, - }) + const { timeToEnd, distanceToEnd } = getCurrentInstruction( + path.instructions, + { + lat: 51.439291, + lng: 14.245254, + }, + undefined + ) expect(Math.round(timeToEnd / 1000)).toEqual(101) expect(Math.round(distanceToEnd)).toEqual(578) @@ -37,32 +45,61 @@ describe('calculate instruction', () => { it('nextWaypointIndex should be correct', () => { let path = ApiImpl.decodeResult(responseHoyerswerda2, true)[0] { - const { nextWaypointIndex } = getCurrentInstruction(path.instructions, { - lat: 51.434672, - lng: 14.267248, - }) + const { nextWaypointIndex } = getCurrentInstruction( + path.instructions, + { + lat: 51.434672, + lng: 14.267248, + }, + undefined + ) expect(nextWaypointIndex).toEqual(1) } // points that could return both indices return the first // TODO include heading to differentiate! { - const { nextWaypointIndex } = getCurrentInstruction(path.instructions, { - lat: 51.434491, - lng: 14.268535, - }) + const { nextWaypointIndex } = getCurrentInstruction( + path.instructions, + { + lat: 51.434491, + lng: 14.268535, + }, + undefined + ) expect(nextWaypointIndex).toEqual(1) } { - const { nextWaypointIndex } = getCurrentInstruction(path.instructions, { - lat: 51.433247, - lng: 14.267763, - }) + const { nextWaypointIndex } = getCurrentInstruction( + path.instructions, + { + lat: 51.433247, + lng: 14.267763, + }, + undefined + ) expect(nextWaypointIndex).toEqual(2) } }) + it('pick instruction depending on heading for same location', () => { + let path = ApiImpl.decodeResult(responseHoyerswerda2, true)[0] + const location = { lat: 51.434356, lng: 14.267697 } + { + const { index } = getCurrentInstruction(path.instructions, location, 170) + expect(path.instructions[index].text).toEqual('Turn left onto Franz-Liszt-Straße') + } + { + const { index } = getCurrentInstruction(path.instructions, location, 240) + expect(path.instructions[index].text).toEqual('Turn left onto Bautzener Allee') + } + { + const { index } = getCurrentInstruction(path.instructions, location, 80) + expect(path.instructions[index].text).toEqual('Waypoint 1') + } + }) + it('calc angle', () => { // downwards+west expect(Math.round(toDegrees(calcOrientation(51.439146, 14.245258, 51.438908, 14.245931)))).toEqual(-30) diff --git a/test/turnNavigation/loop-bug.json b/test/turnNavigation/loop-bug.json new file mode 100644 index 00000000..73e47f14 --- /dev/null +++ b/test/turnNavigation/loop-bug.json @@ -0,0 +1,216 @@ +{ + "orig_url": "https://graphhopper.com/maps/?point=50.352875%2C11.97108&point=50.352734%2C11.973135&profile=car", + "hints": { + "visited_nodes.sum": 16, + "visited_nodes.average": 16.0 + }, + "info": { + "copyrights": [ + "GraphHopper", + "OpenStreetMap contributors" + ], + "took": 1 + }, + "paths": [ + { + "distance": 700.504, + "weight": 40.471842, + "time": 29963, + "transfers": 0, + "points_encoded": true, + "bbox": [ + 11.970981, + 50.350402, + 11.973162, + 50.352882 + ], + "points": "opirHibahA{ukBjCeCrElAsA`OrAgAbHl@m@bDZMzAZ?tA\\HxATVrAZb@FJ`AkG?t@_GM|@cIq@vB}XmAmArAgBgBlLu@s@pHm@s@bHkAwA~@g@}@R", + "instructions": [ + { + "street_ref": "A 93", + "distance": 143.594, + "heading": 148.84, + "sign": 0, + "interval": [ + 0, + 2 + ], + "text": "Continue onto A 93", + "time": 4307, + "street_name": "" + }, + { + "distance": 286.368, + "sign": 7, + "interval": [ + 2, + 13 + ], + "text": "Keep right and take B 173 toward Hof-Zentrum, Feilitzsch, Trogen", + "time": 11471, + "street_destination": "Hof-Zentrum, Feilitzsch, Trogen", + "street_destination_ref": "B 173", + "street_name": "" + }, + { + "street_ref": "B 173", + "distance": 270.542, + "sign": 2, + "interval": [ + 13, + 19 + ], + "text": "Turn right onto B 173", + "time": 14185, + "street_name": "" + }, + { + "distance": 0.0, + "sign": 4, + "last_heading": 43.109344505643136, + "interval": [ + 19, + 19 + ], + "text": "Arrive at destination", + "time": 0, + "street_name": "" + } + ], + "legs": [], + "details": { + "country": [ + [ + 0, + 19, + "DEU" + ] + ], + "surface": [ + [ + 0, + 1, + "missing" + ], + [ + 1, + 14, + "asphalt" + ], + [ + 14, + 15, + "missing" + ], + [ + 15, + 19, + "asphalt" + ] + ], + "road_environment": [ + [ + 0, + 15, + "road" + ], + [ + 15, + 17, + "bridge" + ], + [ + 17, + 19, + "road" + ] + ], + "road_access": [ + [ + 0, + 19, + "yes" + ] + ], + "road_class": [ + [ + 0, + 13, + "motorway" + ], + [ + 13, + 19, + "primary" + ] + ], + "max_speed": [ + [ + 0, + 2, + 150.0 + ], + [ + 2, + 13, + null + ], + [ + 13, + 14, + 100.0 + ], + [ + 14, + 19, + 70.0 + ] + ], + "average_speed": [ + [ + 0, + 2, + 120.0 + ], + [ + 2, + 9, + 86.0 + ], + [ + 9, + 13, + 96.0 + ], + [ + 13, + 14, + 100.0 + ], + [ + 14, + 19, + 64.0 + ] + ], + "toll": [ + [ + 0, + 19, + "missing" + ] + ], + "track_type": [ + [ + 0, + 19, + "missing" + ] + ] + }, + "ascend": 8.39404296875, + "descend": 13.693023681640625, + "snapped_waypoints": "opirHibahA{ukB`@}Kb`@" + } + ] +} \ No newline at end of file