mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-13 06:33:45 +08:00
feat: mapbox navigation helpers
This commit is contained in:
discountchubbs
@@ -21,11 +21,12 @@ env:
|
||||
PYTHONWARNINGS: error
|
||||
BASE_IMAGE: sunnypilot-base
|
||||
AZURE_TOKEN: ${{ secrets.AZURE_COMMADATACI_OPENPILOTCI_TOKEN }}
|
||||
MAPBOX_TOKEN_CI: ${{ secrets.MAPBOX_TOKEN_CI }}
|
||||
|
||||
DOCKER_LOGIN: docker login ghcr.io -u ${{ github.actor }} -p ${{ secrets.GITHUB_TOKEN }}
|
||||
BUILD: release/ci/docker_build_sp.sh base
|
||||
|
||||
RUN: docker run --shm-size 2G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e CI=1 -e PYTHONWARNINGS=error -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v $GITHUB_WORKSPACE/.ci_cache/scons_cache:/tmp/scons_cache -v $GITHUB_WORKSPACE/.ci_cache/comma_download_cache:/tmp/comma_download_cache -v $GITHUB_WORKSPACE/.ci_cache/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/bash -c
|
||||
RUN: docker run --shm-size 2G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e CI=1 -e PYTHONWARNINGS=error -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -e MAPBOX_TOKEN_CI=$MAPBOX_TOKEN_CI -v $GITHUB_WORKSPACE/.ci_cache/scons_cache:/tmp/scons_cache -v $GITHUB_WORKSPACE/.ci_cache/comma_download_cache:/tmp/comma_download_cache -v $GITHUB_WORKSPACE/.ci_cache/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/bash -c
|
||||
|
||||
PYTEST: pytest --continue-on-collection-errors --durations=0 -n logical
|
||||
|
||||
|
||||
@@ -186,6 +186,11 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"ModelManager_LastSyncTime", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
|
||||
{"ModelManager_ModelsCache", {PERSISTENT | BACKUP, JSON}},
|
||||
|
||||
// Navigation params
|
||||
{"MapboxToken", {PERSISTENT | BACKUP, STRING}},
|
||||
{"MapboxSettings", {CLEAR_ON_MANAGER_START, JSON}},
|
||||
{"MapboxRoute", {CLEAR_ON_MANAGER_START, STRING}},
|
||||
|
||||
// Neural Network Lateral Control
|
||||
{"NeuralNetworkLateralControl", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
# Navigation
|
||||
|
||||
Navigation daemon with Mapbox integration for semi-offline navigation. This module handles route planning, geocoding, and turn-by-turn instructions to support autonomous driving features.
|
||||
|
||||
- `navigation_helpers/`: Mapbox API integration and navigation instructions processing.
|
||||
@@ -0,0 +1,106 @@
|
||||
import requests
|
||||
from urllib.parse import quote
|
||||
|
||||
from openpilot.common.params import Params
|
||||
|
||||
|
||||
class MapboxIntegration:
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
|
||||
def get_public_token(self) -> str:
|
||||
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]:
|
||||
if 'latitude' and 'longitude' in postvars:
|
||||
self.nav_confirmed(postvars, current_lon, current_lat, bearing)
|
||||
return postvars, True
|
||||
|
||||
addr = postvars['place_name']
|
||||
if not addr:
|
||||
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}'
|
||||
try:
|
||||
response = requests.get(query, timeout=5)
|
||||
if response.status_code == 200:
|
||||
features = response.json()['features']
|
||||
if features:
|
||||
longitude, latitude = features[0]['geometry']['coordinates']
|
||||
postvars.update({'latitude': latitude, 'longitude': longitude, 'name': addr})
|
||||
self.nav_confirmed(postvars, current_lon, current_lat, bearing)
|
||||
return postvars, True
|
||||
except requests.RequestException:
|
||||
pass # Handle network errors without crashing service
|
||||
return postvars, False
|
||||
|
||||
def nav_confirmed(self, postvars, start_lon, start_lat, bearing=None) -> None:
|
||||
if not postvars:
|
||||
return
|
||||
|
||||
latitude = float(postvars['latitude'])
|
||||
longitude = float(postvars['longitude'])
|
||||
|
||||
data: dict = {'navData': {'current': {'latitude': latitude, 'longitude': longitude}, 'route': {}}}
|
||||
|
||||
token = self.get_public_token()
|
||||
route_data = self.generate_route(start_lon, start_lat, longitude, latitude, token, bearing)
|
||||
if route_data:
|
||||
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:
|
||||
if not token:
|
||||
return None
|
||||
|
||||
params = {
|
||||
'access_token': token,
|
||||
'geometries': 'geojson',
|
||||
'steps': 'true',
|
||||
'overview': 'full',
|
||||
'annotations': 'maxspeed',
|
||||
'alternatives': 'false',
|
||||
'banner_instructions': 'true',
|
||||
}
|
||||
if bearing is not None:
|
||||
params['bearings'] = f'{int((bearing + 360) % 360):.0f},90;'
|
||||
|
||||
try:
|
||||
response = requests.get(f'https://api.mapbox.com/directions/v5/mapbox/driving/{start_lon},{start_lat};{end_lon},{end_lat}', params=params, timeout=5)
|
||||
data = response.json() if response.status_code == 200 else {}
|
||||
except requests.RequestException:
|
||||
return None
|
||||
|
||||
routes = data['routes'] if data else None
|
||||
legs = routes[0]['legs'] if routes else None
|
||||
|
||||
if data.get('code') != 'Ok' or not routes or not legs:
|
||||
return None
|
||||
|
||||
route = routes[0]
|
||||
leg = legs[0]
|
||||
|
||||
steps = [
|
||||
{
|
||||
'maneuver': step['maneuver']['type'],
|
||||
'instruction': step['maneuver']['instruction'],
|
||||
'distance': step['distance'],
|
||||
'duration': step['duration'],
|
||||
'location': {'longitude': step['maneuver']['location'][0], 'latitude': step['maneuver']['location'][1]},
|
||||
'modifier': step['maneuver'].get('modifier', 'none'),
|
||||
'bannerInstructions': step['bannerInstructions'],
|
||||
}
|
||||
for step in leg['steps']
|
||||
]
|
||||
|
||||
maxspeed = [{'speed': item['speed'], 'unit': item['unit']} for item in leg['annotation']['maxspeed'] if 'speed' in item]
|
||||
|
||||
return {
|
||||
'steps': steps,
|
||||
'totalDistance': route['distance'],
|
||||
'totalDuration': route['duration'],
|
||||
'geometry': [{'longitude': coord[0], 'latitude': coord[1]} for coord in route['geometry']['coordinates']],
|
||||
'maxspeed': maxspeed,
|
||||
}
|
||||
@@ -0,0 +1,138 @@
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.sunnypilot.navd.helpers import Coordinate, string_to_direction
|
||||
|
||||
|
||||
class NavigationInstructions:
|
||||
def __init__(self):
|
||||
self.coord = Coordinate(0, 0)
|
||||
self.params = Params()
|
||||
self._cached_route = None
|
||||
self._route_loaded = False
|
||||
self._no_route = False
|
||||
|
||||
def get_route_progress(self, current_lat, current_lon) -> dict | None:
|
||||
'''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
|
||||
|
||||
self.coord.latitude = current_lat
|
||||
self.coord.longitude = current_lon
|
||||
|
||||
# Find closest point on the route polyline
|
||||
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
|
||||
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
|
||||
|
||||
# Next turn is the next step after current
|
||||
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
|
||||
|
||||
distance_to_end_of_step = max(0, current_step['distance'] - (closest_cumulative - current_step['cumulative_distance'])) if current_step else None
|
||||
|
||||
# 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 :])
|
||||
|
||||
all_maneuvers: list = []
|
||||
max_maneuvers = 2
|
||||
for idx in range(current_step_idx, min(current_step_idx + max_maneuvers, len(route['steps']))):
|
||||
step = route['steps'][idx]
|
||||
if idx == current_step_idx:
|
||||
distance = distance_to_end_of_step
|
||||
else:
|
||||
distance = step['cumulative_distance'] - closest_cumulative
|
||||
all_maneuvers.append({'distance': distance, 'type': step['maneuver'], 'modifier': step['modifier']})
|
||||
|
||||
return {
|
||||
'distance_from_route': min_distance,
|
||||
'route_position_cumulative': closest_cumulative,
|
||||
'current_step': current_step,
|
||||
'next_turn': next_turn,
|
||||
'distance_to_next_turn': next_turn_distance,
|
||||
'route_progress_percent': (closest_cumulative / max(1, route['total_distance'])) * 100,
|
||||
'current_maxspeed': current_maxspeed,
|
||||
'distance_to_end_of_step': distance_to_end_of_step,
|
||||
'total_distance_remaining': total_distance_remaining,
|
||||
'total_time_remaining': total_time_remaining,
|
||||
'all_maneuvers': all_maneuvers,
|
||||
'current_step_idx': current_step_idx,
|
||||
}
|
||||
|
||||
def get_current_route(self):
|
||||
if self._route_loaded and self._cached_route is not None:
|
||||
return self._cached_route
|
||||
if self._no_route:
|
||||
return None
|
||||
|
||||
param_value = self.params.get('MapboxSettings')
|
||||
route = param_value['navData']['route'] if param_value else None
|
||||
if not route:
|
||||
self._no_route = True
|
||||
return None
|
||||
steps = []
|
||||
|
||||
geometry = [Coordinate(coord['latitude'], coord['longitude']) for coord in route['geometry']]
|
||||
cumulative_distances = [0.0]
|
||||
cumulative_distances.extend(cumulative_distances[-1] + geometry[step - 1].distance_to(geometry[step]) for step in range(1, len(geometry)))
|
||||
maxspeed = [(speed['speed'], speed['unit']) for speed in route['maxspeed']]
|
||||
steps = []
|
||||
for step in route['steps']:
|
||||
location = Coordinate(step['location']['latitude'], step['location']['longitude'])
|
||||
closest_idx = min(range(len(geometry)), key=lambda i: location.distance_to(geometry[i]))
|
||||
steps.append({
|
||||
'bannerInstructions': step['bannerInstructions'],
|
||||
'distance': step['distance'],
|
||||
'duration': step['duration'],
|
||||
'maneuver': step['maneuver'],
|
||||
'location': location,
|
||||
'cumulative_distance': cumulative_distances[closest_idx],
|
||||
'maxspeed': maxspeed[closest_idx] if closest_idx < len(maxspeed) else None,
|
||||
'modifier': string_to_direction(step['modifier']),
|
||||
})
|
||||
self._cached_route = {
|
||||
'steps': steps,
|
||||
'total_distance': route['totalDistance'],
|
||||
'total_duration': route['totalDuration'],
|
||||
'geometry': geometry,
|
||||
'cumulative_distances': cumulative_distances,
|
||||
'maxspeed': maxspeed,
|
||||
}
|
||||
self._route_loaded = True
|
||||
return self._cached_route
|
||||
|
||||
def clear_route_cache(self):
|
||||
self._cached_route = None
|
||||
self._route_loaded = False
|
||||
self._no_route = False
|
||||
|
||||
def get_upcoming_turn_from_progress(self, progress, current_lat, current_lon) -> str:
|
||||
if progress and progress['next_turn']:
|
||||
self.coord.latitude = current_lat
|
||||
self.coord.longitude = current_lon
|
||||
distance = self.coord.distance_to(progress['next_turn']['location'])
|
||||
if distance <= 100:
|
||||
modifier = progress['next_turn']['modifier']
|
||||
if modifier:
|
||||
return str(modifier)
|
||||
return 'none'
|
||||
|
||||
def get_current_speed_limit_from_progress(self, progress, is_metric: bool) -> int:
|
||||
if progress and progress['current_maxspeed']:
|
||||
speed, _ = progress['current_maxspeed']
|
||||
if is_metric:
|
||||
return int(speed)
|
||||
else:
|
||||
return int(round(speed * CV.KPH_TO_MPH))
|
||||
return 0
|
||||
@@ -0,0 +1,94 @@
|
||||
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:
|
||||
@classmethod
|
||||
def setup_class(cls):
|
||||
cls.mapbox = MapboxIntegration()
|
||||
cls.nav = NavigationInstructions()
|
||||
|
||||
token = os.environ.get('MAPBOX_TOKEN_CI')
|
||||
if token:
|
||||
cls.mapbox.params.put('MapboxToken', token)
|
||||
|
||||
# setup route
|
||||
cls.current_lon, cls.current_lat = -119.17557, 34.23305
|
||||
cls.mapbox.params.put('MapboxRoute', '740 E Ventura Blvd. Camarillo, CA')
|
||||
cls.postvars = {"place_name": cls.mapbox.params.get('MapboxRoute')}
|
||||
cls.postvars, cls.valid_addr = cls.mapbox.set_destination(cls.postvars, cls.current_lon, cls.current_lat)
|
||||
assert cls.valid_addr
|
||||
cls.route = cls.nav.get_current_route()
|
||||
assert cls.route is not None
|
||||
assert len(cls.route['steps']) > 0
|
||||
|
||||
def test_set_destination(self):
|
||||
settings = self.mapbox.params.get('MapboxSettings')
|
||||
assert settings is not None
|
||||
dest_lat = settings['navData']['current']['latitude']
|
||||
dest_lon = settings['navData']['current']['longitude']
|
||||
assert dest_lat == self.postvars["latitude"] and dest_lon == self.postvars["longitude"]
|
||||
|
||||
def test_get_route(self):
|
||||
assert 'steps' in self.route
|
||||
assert 'geometry' in self.route
|
||||
assert 'maxspeed' in self.route
|
||||
assert 'total_distance' in self.route
|
||||
assert 'total_duration' in self.route
|
||||
assert len(self.route['steps']) > 0
|
||||
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
|
||||
|
||||
def test_upcoming_turn_detection(self):
|
||||
progress = self.nav.get_route_progress(self.current_lat, self.current_lon)
|
||||
upcoming = self.nav.get_upcoming_turn_from_progress(progress, self.current_lat, self.current_lon)
|
||||
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.0008 # 80 ish meters before 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"
|
||||
|
||||
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
|
||||
assert 'route_progress_percent' in progress
|
||||
assert 'current_maxspeed' in progress
|
||||
assert 'total_distance_remaining' in progress
|
||||
assert 'total_time_remaining' in progress
|
||||
assert 'all_maneuvers' in progress
|
||||
assert progress['distance_from_route'] >= 0
|
||||
assert 0 <= progress['route_progress_percent'] <= 100
|
||||
assert progress['total_distance_remaining'] >= 0
|
||||
assert progress['total_time_remaining'] >= 0
|
||||
assert isinstance(progress['all_maneuvers'], list)
|
||||
|
||||
# Test speed limit extraction
|
||||
speed_limit_metric = self.nav.get_current_speed_limit_from_progress(progress, True)
|
||||
speed_limit_imperial = self.nav.get_current_speed_limit_from_progress(progress, False)
|
||||
assert isinstance(speed_limit_metric, int)
|
||||
assert isinstance(speed_limit_imperial, int)
|
||||
expected_metric = int(progress['current_maxspeed'][0])
|
||||
expected_imperial = int(round(progress['current_maxspeed'][0] * CV.KPH_TO_MPH))
|
||||
assert speed_limit_metric == expected_metric
|
||||
assert speed_limit_imperial == expected_imperial
|
||||
Reference in New Issue
Block a user