From d58bbda789f8f3e1c1f80d7e7b2b0e050718b687 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Wed, 5 Nov 2025 19:28:38 -0800 Subject: [PATCH] merge origin/nav-events into nav-commacon --- common/params_keys.h | 2 +- selfdrive/controls/lib/desire_helper.py | 3 +- .../qt/offroad/settings/navigation_panel.cc | 2 +- sunnypilot/navd/constants.py | 7 +-- sunnypilot/navd/event_builder.py | 6 +-- .../navigation_desires/navigation_desires.py | 9 +--- .../tests/test_navigation_desires.py | 5 +- .../navigation_helpers/nav_instructions.py | 3 +- .../navigation_helpers/tests/test_mapbox.py | 46 ++++++++++--------- sunnypilot/navd/navigationd.py | 1 - sunnypilot/navd/tests/test_event_builder.py | 2 +- sunnypilot/navd/tests/test_navigationd.py | 2 +- 12 files changed, 43 insertions(+), 45 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index d0dc97f070..3bce8eb2cd 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -193,7 +193,7 @@ inline static std::unordered_map keys = { {"MapboxSettings", {CLEAR_ON_MANAGER_START, JSON}}, {"MapboxRoute", {PERSISTENT, STRING}}, {"MapboxRecompute", {PERSISTENT | BACKUP, BOOL, "0"}}, - {"NavAllowed", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"NavDesiresAllowed", {PERSISTENT | BACKUP, BOOL, "0"}}, {"NavEvents", {PERSISTENT | BACKUP, BOOL, "0"}}, // Neural Network Lateral Control diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 7b4b20e009..e0293e5ded 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -61,7 +61,6 @@ class DesireHelper: def update(self, carstate, lateral_active, lane_change_prob): self.alc.update_params() self.lane_turn_controller.update_params() - self.navigation_desires.update(carstate, lateral_active) v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN @@ -147,6 +146,6 @@ class DesireHelper: self.alc.update_state() - nav_desire = self.navigation_desires.get_desire() + nav_desire = self.navigation_desires.update(carstate, lateral_active) if nav_desire != log.Desire.none and (self.desire == log.Desire.none or self.desire in (log.Desire.turnLeft, log.Desire.turnRight)): self.desire = nav_desire diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/navigation_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/navigation_panel.cc index 27d89e07ce..6ddf65cb67 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/navigation_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/navigation_panel.cc @@ -49,7 +49,7 @@ NavigationPanel::NavigationPanel(QWidget* parent) : QWidget(parent) { list->addItem(mapbox_recompute); // Nav Allowed - nav_allowed = new ParamControlSP("NavAllowed", tr("Navigation Allowed"), tr("Allow navigation features"), "", this); + nav_allowed = new ParamControlSP("NavDesiresAllowed", tr("Navigation Allowed"), tr("Allow navigation features"), "", this); list->addItem(nav_allowed); // Nav Events diff --git a/sunnypilot/navd/constants.py b/sunnypilot/navd/constants.py index e7daa8da7a..716aa7df1a 100644 --- a/sunnypilot/navd/constants.py +++ b/sunnypilot/navd/constants.py @@ -5,11 +5,12 @@ 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. """ + class NAV_CV: """ These distances are expected in meters format and convert to desired format """ SHORT_DISTANCE_METERS = 200.0 QUARTER_MILE = 402.336 POINT_ONE_MILE = 160.9344 - METERS_TO_KILO = 1000 # divide m by this - METERS_TO_MILE = 1609.344 # divide m by this - METERS_TO_FEET = 3.280839895 # multiply m by this + METERS_TO_KILO = 1000 # divide n by this + METERS_TO_MILE = 1609.344 # divide n by this + METERS_TO_FEET = 3.280839895 # multiply n by this diff --git a/sunnypilot/navd/event_builder.py b/sunnypilot/navd/event_builder.py index 79360876a7..4415f9ce55 100644 --- a/sunnypilot/navd/event_builder.py +++ b/sunnypilot/navd/event_builder.py @@ -19,7 +19,7 @@ class EventBuilder: @staticmethod def _build_banner_message(metric: bool, nav_msg): - m = nav_msg.allManeuvers[-1] + m = nav_msg.allManeuvers[1] if len(nav_msg.allManeuvers) > 1 else nav_msg.allManeuvers[0] banner = m.instruction if metric: @@ -45,8 +45,8 @@ class EventBuilder: @staticmethod def _get_turning_message(upcoming_turn): turn_messages = { - 'left': 'Turning Left. Make sure to nudge the wheel!', - 'right': 'Turning Right. Make sure to nudge the wheel!', + 'left': 'Turning Left, Make sure to nudge the wheel', + 'right': 'Turning Right, Make sure to nudge the wheel', 'slightLeft': 'Keeping Left', 'slightRight': 'Keeping Right', 'sharpLeft': 'Sharp Left Turn', diff --git a/sunnypilot/navd/navigation_desires/navigation_desires.py b/sunnypilot/navd/navigation_desires/navigation_desires.py index 51f4325843..e4a6d9a52c 100644 --- a/sunnypilot/navd/navigation_desires/navigation_desires.py +++ b/sunnypilot/navd/navigation_desires/navigation_desires.py @@ -22,9 +22,9 @@ class NavigationDesires: def update_params(self): self.param_counter += 1 if self.param_counter % 60 == 0: # every 3 seconds at 20hz - self.nav_allowed = self._params.get("NavAllowed", return_default=True) + self.nav_allowed = self._params.get("NavDesiresAllowed", return_default=True) - def update(self, CS: car.CarState, lateral_active: bool): + def update(self, CS: car.CarState, lateral_active: bool) -> log.Desire: self.update_params() self.sm.update(0) nav_msg = self.sm['navigationd'] @@ -41,9 +41,4 @@ class NavigationDesires: elif (upcoming == 'right' and CS.steeringPressed and CS.steeringTorque < 0 and not CS.leftBlinker and not CS.rightBlindspot and CS.vEgo < self._turn_speed_limit): self.desire = log.Desire.turnRight - - # Note to reviewers: This change to require steering pressed (nudge basically) is intentional to prevent unwanted turns - # for those who don't have blind spot detection. - - def get_desire(self) -> log.Desire: return self.desire diff --git a/sunnypilot/navd/navigation_desires/tests/test_navigation_desires.py b/sunnypilot/navd/navigation_desires/tests/test_navigation_desires.py index c2150558fe..5cfd34c271 100644 --- a/sunnypilot/navd/navigation_desires/tests/test_navigation_desires.py +++ b/sunnypilot/navd/navigation_desires/tests/test_navigation_desires.py @@ -46,7 +46,7 @@ def make_nav_msg(valid=False, upcoming='none'): def params_setter(allowed: bool): params = Params() - params.put("NavAllowed", allowed) + params.put("NavDesiresAllowed", allowed) @pytest.fixture def mock_submaster(mocker): @@ -75,8 +75,7 @@ def test_update(mock_submaster, mocker): mock_submaster.__getitem__ = mocker.Mock(return_value=make_nav_msg(valid=True, upcoming='left')) nav_desires = NavigationDesires() - nav_desires.update(make_car(leftBlinker=True, steeringPressed=True, steeringTorque=1), True) - assert nav_desires.get_desire() == log.Desire.turnLeft + assert nav_desires.update(make_car(leftBlinker=True, steeringPressed=True, steeringTorque=1), True) == log.Desire.turnLeft params_setter(False) nav_desires.param_counter = 59 diff --git a/sunnypilot/navd/navigation_helpers/nav_instructions.py b/sunnypilot/navd/navigation_helpers/nav_instructions.py index 13c2044604..6459c785cb 100644 --- a/sunnypilot/navd/navigation_helpers/nav_instructions.py +++ b/sunnypilot/navd/navigation_helpers/nav_instructions.py @@ -129,7 +129,8 @@ class NavigationInstructions: return int(round(speed * CV.KPH_TO_MPH)) return 0 - def arrived_at_destination(self, progress) -> bool: + @staticmethod + def arrived_at_destination(progress) -> bool: if progress['all_maneuvers'][0]['type'] == 'arrive': return True elif progress['all_maneuvers'][0]['instruction'].startswith('Your destination'): diff --git a/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py b/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py index e0f2895131..2c025bd407 100644 --- a/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py +++ b/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py @@ -27,12 +27,11 @@ class TestMapbox: 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 + cls.progress = cls.nav.get_route_progress(cls.current_lat, cls.current_lon) def test_set_destination(self): + assert self.valid_addr settings = self.mapbox.params.get('MapboxSettings') assert settings is not None dest_lat = settings['navData']['current']['latitude'] @@ -40,6 +39,7 @@ class TestMapbox: assert dest_lat == self.postvars["latitude"] and dest_lon == self.postvars["longitude"] def test_get_route(self): + assert self.route is not None assert 'steps' in self.route assert 'geometry' in self.route assert 'maxspeed' in self.route @@ -54,8 +54,7 @@ class TestMapbox: 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) + upcoming = self.nav.get_upcoming_turn_from_progress(self.progress, self.current_lat, self.current_lon) assert isinstance(upcoming, str) assert upcoming == 'none' @@ -63,28 +62,33 @@ class TestMapbox: 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 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 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) if expected_turn: assert upcoming_close == expected_turn == 'right', "Should be a right turn upcoming" def test_route_progress_tracking(self): - progress = self.nav.get_route_progress(self.current_lat, self.current_lon) - assert progress is not None - assert 'distance_from_route' in progress - assert 'next_turn' in progress - assert 'current_maxspeed' in progress - assert 'all_maneuvers' in progress - assert 'distance_to_end_of_step' in progress - assert progress['distance_from_route'] >= 0 - assert isinstance(progress['all_maneuvers'], list) + assert self.progress is not None + assert 'distance_from_route' in self.progress + assert 'next_turn' in self.progress + assert 'current_maxspeed' in self.progress + assert 'all_maneuvers' in self.progress + assert 'distance_to_end_of_step' in self.progress + assert self.progress['distance_from_route'] >= 0 + assert isinstance(self.progress['all_maneuvers'], list) - 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) + def test_speed_limit_handling(self): + speed_limit_metric = self.nav.get_current_speed_limit_from_progress(self.progress, True) + speed_limit_imperial = self.nav.get_current_speed_limit_from_progress(self.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)) + expected_metric = int(self.progress['current_maxspeed'][0]) + expected_imperial = int(round(self.progress['current_maxspeed'][0] * CV.KPH_TO_MPH)) assert speed_limit_metric == expected_metric assert speed_limit_imperial == expected_imperial + + def test_arrival_detection(self): + is_arrived = self.nav.arrived_at_destination(self.progress) + assert isinstance(is_arrived, bool) + assert not is_arrived diff --git a/sunnypilot/navd/navigationd.py b/sunnypilot/navd/navigationd.py index ca7466f545..ec2d43c4e7 100755 --- a/sunnypilot/navd/navigationd.py +++ b/sunnypilot/navd/navigationd.py @@ -78,7 +78,6 @@ class Navigationd: def _update_navigation(self) -> tuple[str, dict | None, dict]: banner_instructions: str = '' - progress: dict | None = None nav_data: dict = {} if self.allow_navigation and self.last_position is not None: if progress := self.nav_instructions.get_route_progress(self.last_position.latitude, self.last_position.longitude): diff --git a/sunnypilot/navd/tests/test_event_builder.py b/sunnypilot/navd/tests/test_event_builder.py index 9eff8cd0aa..852b02a742 100644 --- a/sunnypilot/navd/tests/test_event_builder.py +++ b/sunnypilot/navd/tests/test_event_builder.py @@ -76,7 +76,7 @@ class TestEventBuilder: events = EventBuilder.build_navigation_events(MockSM(nav_msg)) expected = [{ 'name': custom.OnroadEventSP.EventName.navigationBanner, - 'message': 'Turning Left. Make sure to nudge the wheel!', + 'message': 'Turning Left, Make sure to nudge the wheel', }] assert events == expected diff --git a/sunnypilot/navd/tests/test_navigationd.py b/sunnypilot/navd/tests/test_navigationd.py index c9d9f5d75b..641d39491a 100644 --- a/sunnypilot/navd/tests/test_navigationd.py +++ b/sunnypilot/navd/tests/test_navigationd.py @@ -9,7 +9,7 @@ import pytest import cereal.messaging as messaging -from sunnypilot.navd.navigationd import Navigationd +from openpilot.sunnypilot.navd.navigationd import Navigationd from openpilot.sunnypilot.navd.helpers import Coordinate