diff --git a/common/params_keys.h b/common/params_keys.h index 7157629f14..3bce8eb2cd 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -188,11 +188,12 @@ inline static std::unordered_map keys = { {"ModelManager_ModelsCache", {PERSISTENT | BACKUP, JSON}}, // Navigation params + {"AllowNavigation", {PERSISTENT | BACKUP, BOOL, "0"}}, {"MapboxToken", {PERSISTENT | BACKUP, STRING}}, {"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/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/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 356d412d81..6459c785cb 100644 --- a/sunnypilot/navd/navigation_helpers/nav_instructions.py +++ b/sunnypilot/navd/navigation_helpers/nav_instructions.py @@ -43,7 +43,7 @@ class NavigationInstructions: distance_to_end_of_step = max(0, current_step['distance'] - (closest_cumulative - current_step['cumulative_distance'])) all_maneuvers: list = [] - max_maneuvers = 2 + max_maneuvers = 3 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: @@ -128,3 +128,11 @@ class NavigationInstructions: else: return int(round(speed * CV.KPH_TO_MPH)) return 0 + + @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'): + return True + return False 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 57c10bc040..ec2d43c4e7 100755 --- a/sunnypilot/navd/navigationd.py +++ b/sunnypilot/navd/navigationd.py @@ -12,6 +12,7 @@ from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper from openpilot.common.swaglog import cloudlog +from openpilot.sunnypilot.navd.constants import NAV_CV 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 @@ -31,6 +32,7 @@ class Navigationd: self.destination: str | None = None self.new_destination: str = '' + self.allow_navigation: bool = False self.recompute_allowed: bool = False self.allow_recompute: bool = False self.reroute_counter: int = 0 @@ -46,6 +48,7 @@ class Navigationd: if self.last_position is not None: self.frame += 1 if self.frame % 9 == 0: + self.allow_navigation = self.params.get('AllowNavigation', return_default=True) 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) @@ -62,6 +65,7 @@ class Navigationd: self.destination = self.new_destination self.nav_instructions.clear_route_cache() self.route = self.nav_instructions.get_current_route() + self.cancel_route_counter = 0 self.reroute_counter = 0 if self.cancel_route_counter == 30: @@ -74,12 +78,12 @@ class Navigationd: 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 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): 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) + arrived = self.nav_instructions.arrived_at_destination(progress) if progress['current_step']: parsed = parse_banner_instructions(progress['current_step']['bannerInstructions'], progress['distance_to_end_of_step']) @@ -90,10 +94,11 @@ class Navigationd: large_distance = progress['distance_from_route'] > 100 if large_distance: - if progress['distance_from_route'] > 200: - self.cancel_route_counter += 1 + self.cancel_route_counter = self.cancel_route_counter + 1 if progress['distance_from_route'] > NAV_CV.QUARTER_MILE else 0 if self.recompute_allowed: self.reroute_counter += 1 + elif arrived: + self.cancel_route_counter += 1 else: self.cancel_route_counter = 0 self.reroute_counter = 0 @@ -102,6 +107,11 @@ class Navigationd: if self.route: if progress['current_step_idx'] == len(self.route['steps']) - 1: self.allow_recompute = False + else: + banner_instructions = '' + progress = None + nav_data = {} + self.valid = False return banner_instructions, progress, nav_data 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