mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-23 16:32:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

some clean up for production

Browse Source
This commit is contained in:
discountchubbs
2025-10-16 15:43:34 -07:00
parent 3481702715
commit 5bd9549bd1
3 changed files with 26 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/navd/navigation_helpers/mapbox_integration.py
+6 -5
View File
@@ -9,7 +9,7 @@ class MapboxIntegration:
self.params = Params()
def get_public_token(self) -> str:
token = str(self.params.get('MapboxToken', return_default=True))
token: str = self.params.get('MapboxToken', return_default=True)
return token
def set_destination(self, postvars, current_lon, current_lat, bearing=None) -> tuple[dict, bool]:
@@ -22,9 +22,9 @@ class MapboxIntegration:
return postvars, False
token = self.get_public_token()
query = f'https://api.mapbox.com/geocoding/v5/mapbox.places/{quote(addr)}.json?access_token={token}&limit=1&proximity={current_lon},{current_lat}'
url = f'https://api.mapbox.com/geocoding/v5/mapbox.places/{quote(addr)}.json?access_token={token}&limit=1&proximity={current_lon},{current_lat}'
try:
response = requests.get(query, timeout=5)
response = requests.get(url, timeout=5)
if response.status_code == 200:
features = response.json()['features']
if features:
@@ -33,7 +33,7 @@ class MapboxIntegration:
self.nav_confirmed(postvars, current_lon, current_lat, bearing)
return postvars, True
except requests.RequestException:
pass # Handle network errors without crashing service
pass # Broad exception to handle network errors like no internet without crashing navd process.
return postvars, False
def nav_confirmed(self, postvars, start_lon, start_lat, bearing=None) -> None:
@@ -51,7 +51,8 @@ class MapboxIntegration:
data['navData']['route'] = route_data
self.params.put('MapboxSettings', data)
def generate_route(self, start_lon, start_lat, end_lon, end_lat, token, bearing=None) -> dict | None:
@staticmethod
def generate_route(start_lon, start_lat, end_lon, end_lat, token, bearing=None) -> dict | None:
if not token:
return None
sunnypilot/navd/navigation_helpers/nav_instructions.py
+14 -16
View File
@@ -1,5 +1,6 @@
from openpilot.common.params import Params
from openpilot.common.constants import CV
from openpilot.common.params import Params
from openpilot.sunnypilot.navd.helpers import Coordinate, string_to_direction
@@ -12,7 +13,7 @@ class NavigationInstructions:
self._no_route = False
def get_route_progress(self, current_lat, current_lon) -> dict | None:
'''Get current position on route and progress information'''
"""Get current position on route and progress information"""
route = self.get_current_route()
if not route or not route['geometry'] or not route['steps']:
return None
@@ -20,30 +21,28 @@ class NavigationInstructions:
self.coord.latitude = current_lat
self.coord.longitude = current_lon
# Find closest point on the route polyline
# Find the closest point on the route relative to self
closest_idx, min_distance = min(((idx, self.coord.distance_to(coord)) for idx, coord in enumerate(route['geometry'])), key=lambda x: x[1])
closest_cumulative = route['cumulative_distances'][closest_idx]
# Find the current step idx: the highest idx where the step location cumulative <= closest_cumulative
# Find the current step index, which is the HIGHEST idx where the step location cumulative less/equal closest cumulative
current_step_idx = max((idx for idx, step in enumerate(route['steps']) if step['cumulative_distance'] <= closest_cumulative), default=-1)
current_step = route['steps'][current_step_idx if current_step_idx >= 0 else 0] if route['steps'] else None
current_step = route['steps'][current_step_idx if current_step_idx >= 0 else 0]
# Next turn is the next step after current
# The next turn is the next step relative to our cumulative index
next_turn_idx = current_step_idx + 1
next_turn = route['steps'][next_turn_idx] if 0 <= next_turn_idx < len(route['steps']) else None
next_turn_distance = max(0, next_turn['cumulative_distance'] - closest_cumulative) if next_turn else None
current_maxspeed = current_step['maxspeed'] if current_step else None
current_maxspeed = current_step['maxspeed']
distance_to_end_of_step = max(0, current_step['distance'] - (closest_cumulative - current_step['cumulative_distance'])) if current_step else None
distance_to_end_of_step = max(0, current_step['distance'] - (closest_cumulative - current_step['cumulative_distance']))
# Calculate total remaining distance and time
total_distance_remaining: float = max(0, route['total_distance'] - closest_cumulative)
total_time_remaining: float = 0.0
if current_step:
progress_in_step = (closest_cumulative - current_step['cumulative_distance']) / current_step['distance']
time_left_in_step = (1 - progress_in_step) * current_step['duration']
total_time_remaining = time_left_in_step + sum(step['duration'] for step in route['steps'][current_step_idx + 1 :])
progress_in_step = (closest_cumulative - current_step['cumulative_distance']) / current_step['distance']
time_left_in_step = (1 - progress_in_step) * current_step['duration']
total_time_remaining: float = time_left_in_step + sum(step['duration'] for step in route['steps'][current_step_idx + 1 :])
all_maneuvers: list = []
max_maneuvers = 2
@@ -97,7 +96,7 @@ class NavigationInstructions:
'maneuver': step['maneuver'],
'location': location,
'cumulative_distance': cumulative_distances[closest_idx],
'maxspeed': maxspeed[closest_idx] if closest_idx < len(maxspeed) else None,
'maxspeed': maxspeed[min(closest_idx, len(maxspeed) - 1)],
'modifier': string_to_direction(step['modifier']),
})
self._cached_route = {
@@ -123,8 +122,7 @@ class NavigationInstructions:
distance = self.coord.distance_to(progress['next_turn']['location'])
if distance <= 100:
modifier = progress['next_turn']['modifier']
if modifier:
return str(modifier)
return str(modifier)
return 'none'
def get_current_speed_limit_from_progress(self, progress, is_metric: bool) -> int:
sunnypilot/navd/navigation_helpers/tests/test_mapbox.py
+6 -9
View File
@@ -1,7 +1,9 @@
import os
from openpilot.common.constants import CV
from openpilot.sunnypilot.navd.navigation_helpers.mapbox_integration import MapboxIntegration
from openpilot.sunnypilot.navd.navigation_helpers.nav_instructions import NavigationInstructions
from openpilot.common.constants import CV
import os
class TestMapbox:
@@ -41,10 +43,6 @@ class TestMapbox:
assert len(self.route['geometry']) > 0
assert len(self.route['maxspeed']) > 0
maxspeed = [(speed, unit) for speed, unit in self.route['maxspeed'] if speed > 0]
print(f"Maxspeed: {maxspeed}")
modifiers = [step['modifier'] for step in self.route['steps']]
print(f"Modifiers: {modifiers}")
if self.route and 'steps' in self.route:
for step in self.route['steps']:
assert 'modifier' in step
@@ -58,17 +56,16 @@ class TestMapbox:
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.0008 # 80 ish meters before turn
close_lat = turn_lat - 0.0008 # 80 ish meters before the turn
if progress and progress.get('next_turn'):
expected_turn = progress['next_turn']['modifier']
upcoming_close = self.nav.get_upcoming_turn_from_progress(progress, close_lat, turn_lon)
if expected_turn:
assert upcoming_close == expected_turn == 'right', f"Should detect '{expected_turn}' turn when close to next turn location"
assert upcoming_close == expected_turn == 'right', "Should be a right turn upcoming"
def test_route_progress_tracking(self):
# Test route progress tracking
progress = self.nav.get_route_progress(self.current_lat, self.current_lon)
print(f"Route progress: {progress}")
assert progress is not None
assert 'distance_from_route' in progress
assert 'next_turn' in progress