mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-29 12:53:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

v_ego

Browse Source
This commit is contained in:
discountchubbs
2025-11-10 08:56:49 -08:00
parent 2d1f3833e4
commit 3a82a0797a
2 changed files with 12 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/navd/navigation_helpers/nav_instructions.py
+9 -2
View File
@@ -4,6 +4,8 @@ Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of oth
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import numpy as np
from openpilot.common.constants import CV
from openpilot.common.params import Params
@@ -109,12 +111,17 @@ class NavigationInstructions:
self._route_loaded = False
self._no_route = False
def get_upcoming_turn_from_progress(self, progress, current_lat, current_lon) -> str:
def get_upcoming_turn_from_progress(self, progress, current_lat, current_lon, v_ego: float) -> str:
if progress and progress['next_turn']:
speed_breakpoints: list = [0, 5, 10, 15, 20, 25, 30, 35, 40]
distance_breakpoints: list = [20, 25, 30, 45, 60, 75, 90, 105, 120]
distance_interp = np.interp(v_ego, speed_breakpoints, distance_breakpoints)
self.coord.latitude = current_lat
self.coord.longitude = current_lon
distance = self.coord.distance_to(progress['next_turn']['location'])
if distance <= 30.0:
if distance <= distance_interp:
modifier = progress['next_turn']['modifier']
return str(modifier)
return 'none'
sunnypilot/navd/navigation_helpers/tests/test_mapbox.py
+3 -3
View File
@@ -54,17 +54,17 @@ class TestMapbox:
assert 'modifier' in step
def test_upcoming_turn_detection(self):
upcoming = self.nav.get_upcoming_turn_from_progress(self.progress, self.current_lat, self.current_lon)
upcoming = self.nav.get_upcoming_turn_from_progress(self.progress, self.current_lat, self.current_lon, v_ego=40.0)
assert isinstance(upcoming, str)
assert upcoming == 'none'
if self.route['steps']:
turn_lat = self.route['steps'][1]['location'].latitude
turn_lon = self.route['steps'][1]['location'].longitude
close_lat = turn_lat - 0.00025 # slightly before the turn
close_lat = turn_lat - 0.000175 # slightly before the turn
if self.progress and self.progress.get('next_turn'):
expected_turn = self.progress['next_turn']['modifier']
upcoming_close = self.nav.get_upcoming_turn_from_progress(self.progress, close_lat, turn_lon)
upcoming_close = self.nav.get_upcoming_turn_from_progress(self.progress, close_lat, turn_lon, v_ego=0.0)
if expected_turn:
assert upcoming_close == expected_turn == 'right', "Should be a right turn upcoming"