Controls - Conditional Experimental Mode - Navigation Based

Switch to 'Experimental Mode' based on navigation data. (i.e. Intersections, stop signs, upcoming turns, etc.).

Co-Authored-By: mike8643 <98910897+mike8643@users.noreply.github.com>

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -29,6 +29,11 @@ class ConditionalExperimentalMode:
       self.status_value = 7 if tracking_lead else 8
       return True
 
+    approaching_maneuver = modelData.navEnabled and (frogpilotNavigation.approachingIntersection or frogpilotNavigation.approachingTurn)
+    if frogpilot_toggles.conditional_navigation and approaching_maneuver and (frogpilot_toggles.conditional_navigation_lead or not tracking_lead):
+      self.status_value = 10 if frogpilotNavigation.approachingIntersection else 11
+      return True
+
     if frogpilot_toggles.conditional_curves and self.curve_detected and (frogpilot_toggles.conditional_curves_lead or not tracking_lead):
       self.status_value = 12
       return True

selfdrive/navd/navd.py

@@ -9,6 +9,7 @@ import requests
 import cereal.messaging as messaging
 from cereal import log
 from openpilot.common.api import Api
+from openpilot.common.numpy_fast import interp
 from openpilot.common.params import Params
 from openpilot.common.realtime import Ratekeeper
 from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
@@ -66,6 +67,11 @@ class RouteEngine:
     # FrogPilot variables
     self.frogpilot_toggles = FrogPilotVariables.toggles
 
+    self.stop_coord = []
+    self.stop_signal = []
+
+    self.approaching_intersection = False
+    self.approaching_turn = False
     self.update_toggles = False
 
   def update(self):
@@ -217,6 +223,17 @@ class RouteEngine:
         self.route = r['routes'][0]['legs'][0]['steps']
         self.route_geometry = []
 
+        # Iterate through the steps in self.route to find "stop_sign" and "traffic_light"
+        if self.frogpilot_toggles.conditional_navigation_intersections:
+          self.stop_signal = []
+          self.stop_coord = []
+
+          for step in self.route:
+            for intersection in step["intersections"]:
+              if "stop_sign" in intersection or "traffic_signal" in intersection:
+                self.stop_signal.append(intersection["geometry_index"])
+                self.stop_coord.append(Coordinate.from_mapbox_tuple(intersection["location"]))
+
         maxspeed_idx = 0
         maxspeeds = r['routes'][0]['legs'][0]['annotation']['maxspeed']
 
@@ -364,9 +381,31 @@ class RouteEngine:
           self.params.remove("NavDestination")
           self.clear_route()
 
+    if self.frogpilot_toggles.conditional_navigation:
+      v_ego = self.sm['carState'].vEgo
+      seconds_to_stop = interp(v_ego, [0, 22.5, 45], [5, 10, 10])
+
+      closest_condition_indices = [idx for idx in self.stop_signal if idx >= closest_idx]
+      if closest_condition_indices:
+        closest_condition_index = min(closest_condition_indices, key=lambda idx: abs(closest_idx - idx))
+        index = self.stop_signal.index(closest_condition_index)
+
+        distance_to_condition = self.last_position.distance_to(self.stop_coord[index])
+        self.approaching_intersection = self.frogpilot_toggles.conditional_navigation_intersections and distance_to_condition < max((seconds_to_stop * v_ego), 25)
+      else:
+        self.approaching_intersection = False
+
+      self.approaching_turn = self.frogpilot_toggles.conditional_navigation_turns and distance_to_maneuver_along_geometry < max((seconds_to_stop * v_ego), 25)
+    else:
+      self.approaching_intersection = False
+      self.approaching_turn = False
+
     frogpilot_plan_send = messaging.new_message('frogpilotNavigation')
     frogpilotNavigation = frogpilot_plan_send.frogpilotNavigation
 
+    frogpilotNavigation.approachingIntersection = self.approaching_intersection
+    frogpilotNavigation.approachingTurn = self.approaching_turn
+
     self.pm.send('frogpilotNavigation', frogpilot_plan_send)
 
   def send_route(self):
@@ -420,7 +459,7 @@ class RouteEngine:
 
 def main():
   pm = messaging.PubMaster(['navInstruction', 'navRoute', 'frogpilotNavigation'])
-  sm = messaging.SubMaster(['liveLocationKalman', 'managerState'])
+  sm = messaging.SubMaster(['carState', 'liveLocationKalman', 'managerState'])
 
   rk = Ratekeeper(1.0)
   route_engine = RouteEngine(sm, pm)