kouros/navigation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

RouteCalculator

Browse Source
This commit is contained in:
Dimitris
2026-05-07 10:34:27 +02:00
parent 8b612d5b80
commit 72b3185280
8 changed files with 79 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/build.gradle.kts
+2 -2
View File
@@ -17,8 +17,8 @@ android {
applicationId = "com.kouros.navigation"
minSdk = 33
targetSdk = 37
versionCode = 108
versionName = "0.3.0.108"
versionCode = 109
versionName = "0.3.0.109"
base.archivesName = "navi-$versionName"
testInstrumentationRunner = "androidx.test.runner.AndroidJUnitRunner"
}
common/car/src/main/java/com/kouros/navigation/car/DeviceLocationManager.kt
+2 -2
View File
@@ -107,8 +107,8 @@ class DeviceLocationManager(
val lastLocation = locationManager.getLastKnownLocation(LocationManager.GPS_PROVIDER)
if (lastLocation != null) {
if (setIndividualLocation) {
onInitialLocation(lastLocation)
onLocationUpdate(lastLocation)
onInitialLocation(homeVogelhart)
onLocationUpdate(homeVogelhart)
} else {
onInitialLocation(lastLocation)
onLocationUpdate(lastLocation)
common/car/src/main/java/com/kouros/navigation/car/NavigationSession.kt
+1
View File
@@ -485,6 +485,7 @@ class NavigationSession : CarSession(), NavigationListener, NavigationObserverCa
val distance = location.distanceTo(snappedLocation)
when {
distance > MAXIMAL_ROUTE_DEVIATION -> {
Log.d(TAG, "Maximal route deviation")
stopNavigation()
navigationScreen.calculateNewRoute(routeModel.navState.destination)
return false
common/car/src/main/java/com/kouros/navigation/car/navigation/Simulation.kt
+4 -7
View File
@@ -28,8 +28,8 @@ class Simulation {
) {
if (routeModel.navState.route.isRouteValid()) {
if (BuildConfig.DEBUG) {
gpxSimulation(routeModel, lifecycleScope, updateLocation)
//currentSimulation(routeModel, lifecycleScope, updateLocation)
//gpxSimulation(routeModel, lifecycleScope, updateLocation)
currentSimulation(routeModel, lifecycleScope, updateLocation)
} else {
currentSimulation(routeModel, lifecycleScope, updateLocation)
}
@@ -46,7 +46,7 @@ class Simulation {
if (points.isEmpty()) return
simulationJob?.cancel()
var lastLocation = Location(LocationManager.FUSED_PROVIDER)
var curBearing = 0f
var curBearing: Float
simulationJob = lifecycleScope.launch {
for ((index, point) in points.withIndex()) {
if (index >= 0) {
@@ -90,7 +90,7 @@ class Simulation {
simulationJob?.cancel()
simulationJob =lifecycleScope.launch() {
var lastLocation = Location(LocationManager.FUSED_PROVIDER)
var curBearing = 0f
val curBearing = 0f
val parser = GPXParser()
val parsedGpx: Gpx? =
parser.parse(route.byteInputStream())
@@ -118,9 +118,6 @@ class Simulation {
// Update your app's state as if a real GPS update occurred
updateLocation(fakeLocation)
// Wait before moving to the next point (e.g., every 1 second)
if (duration > 100) {
// delay(duration / 4)
}
delay(200)
lastTime = p.time
lastLocation = fakeLocation
common/data/src/main/java/com/kouros/navigation/data/Data.kt
+3 -2
View File
@@ -16,6 +16,7 @@
package com.kouros.navigation.data
import android.location.Location
import android.net.Uri
import com.google.gson.annotations.Expose
import com.kouros.navigation.data.route.Lane
@@ -28,7 +29,7 @@ data class Category(
val name: String,
)
data class StepMatch(val stepIndex: Int, val waypointIndex: Int, val location: Location)
data class Places(
val places: List<Place>,
@@ -144,7 +145,7 @@ object Constants {
const val NEAREST_LOCATION_DISTANCE = 10F
const val MAXIMUM_LOCATION_DISTANCE = 100000F
const val MAXIMUM_LOCATION_DISTANCE = Float.MAX_VALUE
const val TRAFFIC_UPDATE = 300
common/data/src/main/java/com/kouros/navigation/model/RouteCalculator.kt
+53 -29
View File
@@ -3,60 +3,84 @@ package com.kouros.navigation.model
import android.location.Location
import android.util.Log
import androidx.car.app.navigation.model.Step
import com.kouros.navigation.data.ApplicationConfig
import com.kouros.navigation.data.Constants.MAXIMUM_LOCATION_DISTANCE
import com.kouros.navigation.data.Constants.NEAREST_LOCATION_DISTANCE
import com.kouros.navigation.data.Constants.SPEED_UPDATE_DISTANCE
import com.kouros.navigation.data.Constants.TAG
import com.kouros.navigation.utils.bearingPositive
import com.kouros.navigation.data.StepMatch
import com.kouros.navigation.data.osrm.Waypoints
import com.kouros.navigation.utils.location
import java.util.concurrent.TimeUnit
import kotlin.math.absoluteValue
class RouteCalculator(var routeModel: RouteModel) {
var bestMatch: StepMatch? = null
var lastSpeedLocation: Location = location(0.0, 0.0)
var lastSpeedIndex: Int = 0
fun findStep(location: Location) {
val route = routeModel.navState.route
val steps = routeModel.curLeg.steps
val startIndex = route.currentStepIndex
// Windowed search for performance (current + 2 steps)
//val endIndex = (startIndex + 2).coerceAtMost(steps.size - 1)
val endIndex = steps.size - 1
var nearestDistance = MAXIMUM_LOCATION_DISTANCE
var count = 0
var lastDistance = 0F
var increaseDistance = 0
for ((index, step) in routeModel.curLeg.steps.withIndex()) {
count++
var distance = 0F
if (index >= routeModel.navState.route.currentStepIndex) {
for ((wayIndex, waypoint) in step.maneuver.waypoints.withIndex()) {
count++
if (wayIndex >= step.waypointIndex) {
distance = location.distanceTo(waypoint)
if (distance < nearestDistance ) {
var searchCount = 0
for (i in startIndex..endIndex) {
val step = steps[i]
val waypoints = step.maneuver.waypoints
val startWayIndex = if (i == startIndex) step.waypointIndex else 0
var lastDistance = Float.MAX_VALUE
var increaseCount = 0
for (j in startWayIndex until waypoints.size) {
searchCount++
val waypoint = waypoints[j]
val distance = location.distanceTo(waypoint)
if (distance < nearestDistance) {
nearestDistance = distance
routeModel.navState.route.currentStepIndex = step.index
step.waypointIndex = wayIndex
step.wayPointLocation = waypoint
}
}
if (stopSearch(nearestDistance, distance) || increaseDistance > 20) {
break
bestMatch = StepMatch(step.index, j, waypoint)
}
// Track if we are getting further away
if (distance > lastDistance) {
increaseDistance++
increaseCount++
} else {
increaseCount = 0
}
// Absolute early exit: found a close point and now moved far away
if (stopSearch(nearestDistance, distance, increaseCount)) {
bestMatch?.let { match ->
route.currentStepIndex = match.stepIndex
steps[match.stepIndex].waypointIndex = match.waypointIndex
steps[match.stepIndex].wayPointLocation = match.location
}
Log.d(TAG, "FindStep optimized: count=$searchCount, dist=$nearestDistance")
return
}
lastDistance = distance
}
}
if (stopSearch(nearestDistance, distance) || increaseDistance > 20) {
break
bestMatch?.let { match ->
route.currentStepIndex = match.stepIndex
steps[match.stepIndex].waypointIndex = match.waypointIndex
steps[match.stepIndex].wayPointLocation = match.location
}
}
Log.d(TAG, "FindStep $nearestDistance $count $increaseDistance")
Log.d(TAG, "FindStep not optimized: count=$searchCount, dist=$nearestDistance")
}
private fun stopSearch(nearestDistance: Float, distance: Float): Boolean {
return nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 10
private fun stopSearch(nearestDistance: Float, distance: Float, increaseCount: Int): Boolean {
return (nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 10)
|| increaseCount > 10
}
fun travelLeftTime(): Double {
@@ -92,7 +116,7 @@ class RouteCalculator(var routeModel: RouteModel) {
fun leftStepDistance(): Double {
val step = routeModel.route.currentStep()
var leftDistance = 0F
if (step.waypointIndex < step.maneuver.waypoints.size -1) {
if (step.waypointIndex < step.maneuver.waypoints.size - 1) {
leftDistance = step.maneuver.leftDistance[step.waypointIndex]
val waypointLocation = step.maneuver.waypoints[step.waypointIndex]
if (routeModel.navState.lastLocation.latitude != 0.0) {
common/data/src/test/java/com/kouros/navigation/model/RouteCalculatorTest.kt
+7 -5
View File
@@ -7,6 +7,7 @@ import com.kouros.navigation.data.route.Maneuver
import com.kouros.navigation.data.route.Routes
import com.kouros.navigation.data.route.Step
import com.kouros.navigation.data.route.Summary
import com.kouros.navigation.utils.location
import org.junit.Assert.assertEquals
import org.junit.Assert.assertTrue
import org.junit.Before
@@ -44,7 +45,7 @@ class RouteCalculatorTest {
waypointIndex: Int = 0,
): Step {
val waypoints = (0 until numWaypoints).map { i ->
listOf(11.0 + index * 0.01 + i * 0.001, 48.0)
location(11.0 + index * 0.01 + i * 0.001, 48.0)
}
return Step(
index = index,
@@ -127,13 +128,14 @@ class RouteCalculatorTest {
)
val mockLocation: Location = mock()
// step0/wp0: 500F, step0/wp1: 5F — 5F < NEAREST_LOCATION_DISTANCE (10F) → break
whenever(mockLocation.distanceTo(any())).thenReturn(500F, 5F)
// step0/wp0: 500F, step0/wp1: 5F, step1/wp0: 200F
// 5F < NEAREST_LOCATION_DISTANCE (10F) AND 200F > 10 * 10F → break
whenever(mockLocation.distanceTo(any())).thenReturn(500F, 5F, 200F)
routeCalculator.findStep(mockLocation)
// step1 and step2 are never evaluated
verify(mockLocation, times(2)).distanceTo(any())
// should stop after the 3rd call (step1/wp0 triggers stopSearch)
verify(mockLocation, times(3)).distanceTo(any())
assertEquals(0, routeModel.navState.route.currentStepIndex)
}
common/data/src/test/java/com/kouros/navigation/model/RouteModelTest.kt
+2 -1
View File
@@ -9,6 +9,7 @@ import com.kouros.navigation.data.route.Routes
import com.kouros.navigation.data.route.Step
import com.kouros.navigation.data.route.Summary
import com.kouros.navigation.utils.GeoUtils.createPointCollection
import com.kouros.navigation.utils.location
import org.junit.Before
import org.junit.Test
import org.maplibre.geojson.FeatureCollection
@@ -39,7 +40,7 @@ class RouteModelTest {
waypointIndex: Int = 0,
): Step {
val waypoints = (0 until numWaypoints).map { i ->
listOf(11.0 + index * 0.01 + i * 0.001, 48.0)
location(11.0 + index * 0.01 + i * 0.001, 48.0)
}
return Step(
index = index,