From 8c335926285e88cde36cd2ad190ace0d84d13f7e Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sat, 18 Oct 2025 18:18:12 -0700 Subject: [PATCH 1/2] Revert "feat: navigationd" bc it was supposed to be a branch lol This reverts commit 3bbb33f6bd985239600ded2564f87ecb16adcaa6. --- cereal/custom.capnp | 21 +--- cereal/log.capnp | 2 +- cereal/services.py | 1 - common/params_keys.h | 1 - sunnypilot/navd/README.md | 1 - sunnypilot/navd/navigationd.py | 140 ---------------------- sunnypilot/navd/tests/__init__.py | 0 sunnypilot/navd/tests/test_navigationd.py | 58 --------- system/manager/process_config.py | 3 - 9 files changed, 2 insertions(+), 225 deletions(-) delete mode 100755 sunnypilot/navd/navigationd.py delete mode 100644 sunnypilot/navd/tests/__init__.py delete mode 100644 sunnypilot/navd/tests/test_navigationd.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 794ce58dc2..5c0a004fa6 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -454,26 +454,7 @@ struct ModelDataV2SP @0xa1680744031fdb2d { } } -struct Navigationd @0xcb9fd56c7057593a { - timestamp @0 :UInt64; - upcomingTurn @1 :Text; - currentSpeedLimit @2 :UInt64; - bannerInstructions @3 :Text; - distanceToNextTurn @4 :Float64; - routeProgressPercent @5 :Float64; - distanceFromRoute @6 :Float64; - routePositionCumulative @7 :Float64; - distanceToEndOfStep @8 :Float64; - totalDistanceRemaining @9 :Float64; - totalTimeRemaining @10 :Float64; - allManeuvers @11 :List(Maneuver); - valid @12 :Bool; - - struct Maneuver { - distance @0 :Float64; - type @1 :Text; - modifier @2 :Text; - } +struct CustomReserved10 @0xcb9fd56c7057593a { } struct CustomReserved11 @0xc2243c65e0340384 { diff --git a/cereal/log.capnp b/cereal/log.capnp index e07ee31f8f..8925bca8ae 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2632,7 +2632,7 @@ struct Event { carStateSP @114 :Custom.CarStateSP; liveMapDataSP @115 :Custom.LiveMapDataSP; modelDataV2SP @116 :Custom.ModelDataV2SP; - navigationd @136 :Custom.Navigationd; + customReserved10 @136 :Custom.CustomReserved10; customReserved11 @137 :Custom.CustomReserved11; customReserved12 @138 :Custom.CustomReserved12; customReserved13 @139 :Custom.CustomReserved13; diff --git a/cereal/services.py b/cereal/services.py index b6871f0e0f..67548bc79e 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -89,7 +89,6 @@ _services: dict[str, tuple] = { "carStateSP": (True, 100., 10), "liveMapDataSP": (True, 1., 1), "modelDataV2SP": (True, 20.), - "navigationd": (True, 3.), "liveLocationKalman": (True, 20.), # debug diff --git a/common/params_keys.h b/common/params_keys.h index df5e1b6440..ec749648c3 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -190,7 +190,6 @@ inline static std::unordered_map keys = { {"MapboxToken", {PERSISTENT | BACKUP, STRING}}, {"MapboxSettings", {CLEAR_ON_MANAGER_START, JSON}}, {"MapboxRoute", {CLEAR_ON_MANAGER_START, STRING}}, - {"MapboxRecompute", {PERSISTENT | BACKUP, BOOL, "0"}}, // Neural Network Lateral Control {"NeuralNetworkLateralControl", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/sunnypilot/navd/README.md b/sunnypilot/navd/README.md index 73fbb0eb66..9cedf83345 100644 --- a/sunnypilot/navd/README.md +++ b/sunnypilot/navd/README.md @@ -3,4 +3,3 @@ 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. -- `navigationd`: Navigation service which uses mapbox integration to generate a route and keep it up to date. This service runs at three hz, using keep time to ensure the while loop only updates three times a second rather than every time sm updates, which in this case would be twenty hz (LLK). \ No newline at end of file diff --git a/sunnypilot/navd/navigationd.py b/sunnypilot/navd/navigationd.py deleted file mode 100755 index f9b3f89a19..0000000000 --- a/sunnypilot/navd/navigationd.py +++ /dev/null @@ -1,140 +0,0 @@ -import math -import time - -import cereal.messaging as messaging -from cereal import custom -from openpilot.common.params import Params -from openpilot.common.realtime import Ratekeeper -from openpilot.common.swaglog import cloudlog - -from openpilot.sunnypilot.navd.helpers import Coordinate, parse_banner_instructions -from openpilot.sunnypilot.navd.navigation_helpers.mapbox_integration import MapboxIntegration -from openpilot.sunnypilot.navd.navigation_helpers.nav_instructions import NavigationInstructions - - -class Navigationd: - def __init__(self): - self.params = Params() - self.mapbox = MapboxIntegration() - self.nav_instructions = NavigationInstructions() - - self.sm = messaging.SubMaster(['liveLocationKalman']) - self.pm = messaging.PubMaster(['navigationd']) - self.rk = Ratekeeper(3) # 3 Hz - - self.route = None - self.destination: str | None = None - self.new_destination: str = '' - - self.recompute_allowed: bool = False - self.allow_recompute: bool = False - self.reroute_counter: int = 0 - - self.frame: int = -1 - self.last_position: Coordinate | None = None - self.last_bearing: float | None = None - self.is_metric: bool = False - self.valid: bool = False - - def _update_params(self): - if self.last_position is not None: - self.frame += 1 - if self.frame % 9 == 0: - self.is_metric = self.params.get('IsMetric', return_default=True) - self.new_destination = self.params.get('MapboxRoute') - self.recompute_allowed = self.params.get('MapboxRecompute', return_default=True) - - self.allow_recompute: bool = (self.new_destination != self.destination and self.new_destination != '') or ( - self.recompute_allowed and self.reroute_counter > 9 and self.route - ) - - if self.allow_recompute: - postvars = {'place_name': self.new_destination} - postvars, valid_addr = self.mapbox.set_destination(postvars, self.last_position.longitude, self.last_position.latitude, self.last_bearing) - cloudlog.debug(f'Set new destination to: {self.new_destination}, valid: {valid_addr}') - if valid_addr: - self.destination = self.new_destination - self.nav_instructions.clear_route_cache() - self.route = self.nav_instructions.get_current_route() - self.reroute_counter = 0 - - self.valid = self.route is not None - - def _update_navigation(self) -> tuple[str, dict | None, dict]: - banner_instructions: str = '' - progress: dict | None = None - nav_data: dict = {} - if self.last_position is not None: - if progress := self.nav_instructions.get_route_progress(self.last_position.latitude, self.last_position.longitude): - nav_data['upcoming_turn'] = self.nav_instructions.get_upcoming_turn_from_progress(progress, self.last_position.latitude, self.last_position.longitude) - nav_data['current_speed_limit'] = self.nav_instructions.get_current_speed_limit_from_progress(progress, self.is_metric) - - if progress['current_step']: - parsed = parse_banner_instructions(progress['current_step']['bannerInstructions'], progress['distance_to_end_of_step']) - if parsed: - banner_instructions = parsed['maneuverPrimaryText'] - - nav_data['distance_to_next_turn'] = progress['distance_to_next_turn'] - nav_data['distance_to_end_of_step'] = progress['distance_to_end_of_step'] - nav_data['route_progress_percent'] = progress['route_progress_percent'] - nav_data['distance_from_route'] = progress['distance_from_route'] - nav_data['route_position_cumulative'] = progress['route_position_cumulative'] - - # Don't recompute in last segment to prevent reroute loops - if self.route: - if progress['current_step_idx'] == len(self.route['steps']) - 1: - self.allow_recompute = False - - if self.recompute_allowed: - self.reroute_counter += 1 if nav_data['distance_from_route'] > 25 else 0 - - return banner_instructions, progress, nav_data - - def _build_navigation_message(self, banner_instructions, progress, nav_data): - msg = custom.Navigationd.new_message() - msg.timestamp = int(time.monotonic() * 1000) - msg.upcomingTurn = nav_data.get('upcoming_turn', 'none') - msg.currentSpeedLimit = nav_data.get('current_speed_limit', 0) - msg.bannerInstructions = banner_instructions - msg.distanceToNextTurn = nav_data.get('distance_to_next_turn', 0.0) - msg.distanceToEndOfStep = nav_data.get('distance_to_end_of_step', 0.0) - msg.routeProgressPercent = nav_data.get('route_progress_percent', 0.0) - msg.distanceFromRoute = nav_data.get('distance_from_route', 0.0) - msg.routePositionCumulative = nav_data.get('route_position_cumulative', 0.0) - msg.totalDistanceRemaining = progress['total_distance_remaining'] if progress else 0.0 - msg.totalTimeRemaining = progress['total_time_remaining'] if progress else 0.0 - msg.valid = self.valid - - all_maneuvers = ( - [custom.Navigationd.Maneuver.new_message(distance=m['distance'], type=m['type'], modifier=m['modifier']) for m in progress['all_maneuvers']] - if progress - else [] - ) - msg.allManeuvers = all_maneuvers - - return msg - - def run(self): - cloudlog.warning('navigationd init') - - while True: - self.sm.update() - location = self.sm['liveLocationKalman'] - localizer_valid = location.positionGeodetic.valid if location else False - - if localizer_valid: - self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) - self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) - - self._update_params() - banner_instructions, progress, nav_data = self._update_navigation() - - msg = self._build_navigation_message(banner_instructions, progress, nav_data) - - self.pm.send('navigationd', msg) - self.rk.keep_time() - - -def main(): - nav = Navigationd() - nav.run() diff --git a/sunnypilot/navd/tests/__init__.py b/sunnypilot/navd/tests/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/navd/tests/test_navigationd.py b/sunnypilot/navd/tests/test_navigationd.py deleted file mode 100644 index 2d3e1c715c..0000000000 --- a/sunnypilot/navd/tests/test_navigationd.py +++ /dev/null @@ -1,58 +0,0 @@ -import cereal.messaging as messaging - -from sunnypilot.navd.navigationd import Navigationd -from openpilot.sunnypilot.navd.helpers import Coordinate - - -def test_init(): - nav = Navigationd() - assert nav.sm is not None - assert nav.pm is not None - assert nav.rk is not None - assert nav.route is None - assert nav.destination is None - assert nav.new_destination == '' - assert nav.frame == -1 - assert nav.last_position is None - assert not nav.is_metric - assert not nav.valid - -def test_update_params(): - nav = Navigationd() - nav.last_position = None - nav._update_params() - assert nav.frame == -1 - nav.last_position = Coordinate(latitude=37.0, longitude=128.0) - nav._update_params() - assert nav.frame == 0 # frame only updates when last position is set - -def test_update_navigation_no_position(): - nav = Navigationd() - nav.last_position = None - banner, progress, nav_data = nav._update_navigation() - assert banner == '' - assert progress is None - assert nav_data == {} - -def test_update_navigation(): - nav = Navigationd() - nav.last_position = Coordinate(latitude=37.0, longitude=128.0) - nav.route = {'580 Winchester dr, oxnard, CA': True} - banner, progress, nav_data = nav._update_navigation() - assert isinstance(banner, str) - assert not progress # no route was actually set - assert isinstance(nav_data, dict) - -def test_build_navigation_message(): - sm = messaging.SubMaster(['navigationd']) - nav = Navigationd() - msg = nav._build_navigation_message('', None, {}) - - nav.pm.send('navigationd', msg) - sm.update() - received_msg = sm['navigationd'] - - assert received_msg.bannerInstructions == msg.bannerInstructions - assert received_msg.totalDistanceRemaining == msg.totalDistanceRemaining - assert received_msg.totalTimeRemaining == msg.totalTimeRemaining - assert received_msg.valid == msg.valid diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 607de3eae7..c5f20e511b 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -180,9 +180,6 @@ procs += [ NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready), PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run), - # navigationd - PythonProcess("navigationd", "sunnypilot.navd.navigationd", only_onroad), - # locationd NativeProcess("locationd_llk", "sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad), ] From 07b8e7783d8eaa6d507bae3d42083e7b65810f5d Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 19 Oct 2025 19:47:21 -0700 Subject: [PATCH 2/2] Do i really need a readme --- sunnypilot/navd/navigation_helpers/nav_instructions.py | 4 ++-- sunnypilot/navd/navigation_helpers/tests/test_mapbox.py | 4 +--- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/sunnypilot/navd/navigation_helpers/nav_instructions.py b/sunnypilot/navd/navigation_helpers/nav_instructions.py index 888280f96e..a2e9148f41 100644 --- a/sunnypilot/navd/navigation_helpers/nav_instructions.py +++ b/sunnypilot/navd/navigation_helpers/nav_instructions.py @@ -13,7 +13,6 @@ 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""" route = self.get_current_route() if not route or not route['geometry'] or not route['steps']: return None @@ -125,7 +124,8 @@ class NavigationInstructions: return str(modifier) return 'none' - def get_current_speed_limit_from_progress(self, progress, is_metric: bool) -> int: + @staticmethod + def get_current_speed_limit_from_progress(progress, is_metric: bool) -> int: if progress and progress['current_maxspeed']: speed, _ = progress['current_maxspeed'] if is_metric: diff --git a/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py b/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py index cb1432a1ba..4ba70ac5bb 100644 --- a/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py +++ b/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py @@ -16,7 +16,7 @@ class TestMapbox: if token: cls.mapbox.params.put('MapboxToken', token) - # setup route + # route setup 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')} @@ -64,7 +64,6 @@ class TestMapbox: 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) assert progress is not None assert 'distance_from_route' in progress @@ -80,7 +79,6 @@ class TestMapbox: 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)