merge origin/nav-events into nav-commacon

common/params_keys.h

@@ -193,7 +193,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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

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

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

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

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',

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

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

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'):

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

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):

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
 

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