diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 85b7c61e5a..3e6279e476 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -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 diff --git a/common/params_keys.h b/common/params_keys.h index dd58462a95..ec749648c3 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -186,6 +186,11 @@ inline static std::unordered_map 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"}}, diff --git a/sunnypilot/navd/README.md b/sunnypilot/navd/README.md new file mode 100644 index 0000000000..9cedf83345 --- /dev/null +++ b/sunnypilot/navd/README.md @@ -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. diff --git a/sunnypilot/navd/__init__.py b/sunnypilot/navd/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/navd/navigation_helpers/__init__.py b/sunnypilot/navd/navigation_helpers/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/navd/navigation_helpers/mapbox_integration.py b/sunnypilot/navd/navigation_helpers/mapbox_integration.py new file mode 100644 index 0000000000..3a97c2e5d8 --- /dev/null +++ b/sunnypilot/navd/navigation_helpers/mapbox_integration.py @@ -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, + } diff --git a/sunnypilot/navd/navigation_helpers/nav_instructions.py b/sunnypilot/navd/navigation_helpers/nav_instructions.py new file mode 100644 index 0000000000..c274bb9dae --- /dev/null +++ b/sunnypilot/navd/navigation_helpers/nav_instructions.py @@ -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 diff --git a/sunnypilot/navd/navigation_helpers/tests/__init__.py b/sunnypilot/navd/navigation_helpers/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py b/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py new file mode 100644 index 0000000000..ac5ebed241 --- /dev/null +++ b/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py @@ -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