wip

selfdrive/navd/navd.py

@@ -18,8 +18,6 @@ from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
                                     parse_banner_instructions)
 from openpilot.common.swaglog import cloudlog
 
-from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
-
 REROUTE_DISTANCE = 25
 MANEUVER_TRANSITION_THRESHOLD = 10
 REROUTE_COUNTER_MIN = 3
@@ -55,7 +53,6 @@ class RouteEngine:
     if "MAPBOX_TOKEN" in os.environ:
       self.mapbox_token = os.environ["MAPBOX_TOKEN"]
       self.mapbox_host = "https://api.mapbox.com"
-      self.params.put("MapboxPublicKey", self.mapbox_token)
     elif self.params.get_int("PrimeType") == 0:
       self.mapbox_token = self.params.get("MapboxPublicKey", encoding='utf8')
       self.mapbox_host = "https://api.mapbox.com"
@@ -77,10 +74,6 @@ class RouteEngine:
     self.update_frogpilot_params()
 
   def update(self):
-    # Update FrogPilot parameters
-    if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
-      self.update_frogpilot_params()
-
     self.sm.update(0)
 
     if self.sm.updated["managerState"]:
@@ -98,6 +91,10 @@ class RouteEngine:
     except Exception:
       cloudlog.exception("navd.failed_to_compute")
 
+    # Update FrogPilot parameters
+    if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
+      self.update_frogpilot_params()
+
   def update_location(self):
     location = self.sm['liveLocationKalman']
     self.gps_ok = location.gpsOK
@@ -179,6 +176,7 @@ class RouteEngine:
 
       r = resp.json()
       r1 = resp.json()
+
       # Function to remove specified keys recursively unnessary for display
       def remove_keys(obj, keys_to_remove):
         if isinstance(obj, list):
@@ -187,13 +185,16 @@ class RouteEngine:
           return {key: remove_keys(value, keys_to_remove) for key, value in obj.items() if key not in keys_to_remove}
         else:
           return obj
+
       keys_to_remove = ['geometry', 'annotation', 'incidents', 'intersections', 'components', 'sub', 'waypoints']
       self.r2 = remove_keys(r1, keys_to_remove)
       self.r3 = {}
+
       # Add items for display under "routes"
       if 'routes' in self.r2 and len(self.r2['routes']) > 0:
         first_route = self.r2['routes'][0]
         nav_destination_json = self.params.get('NavDestination')
+
         try:
           nav_destination_data = json.loads(nav_destination_json)
           place_name = nav_destination_data.get('place_name', 'Default Place Name')
@@ -203,7 +204,8 @@ class RouteEngine:
           self.r3['uuid'] = self.r2['uuid']
         except json.JSONDecodeError as e:
           print(f"Error decoding JSON: {e}")
-	  # Save slim json as file
+
+      # Save slim json as file
       with open('navdirections.json', 'w') as json_file:
         json.dump(self.r2, json_file, indent=4)
       with open('CurrentStep.json', 'w') as json_file:
@@ -217,6 +219,7 @@ class RouteEngine:
         if self.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:
@@ -265,11 +268,7 @@ class RouteEngine:
 
     if self.step_idx is None:
       msg.valid = False
-      SpeedLimitController.nav_speed_limit = 0
-      SpeedLimitController.write_nav_state()
-
-      if SpeedLimitController.desired_speed_limit != 0:
-        msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit
+      self.params_memory.put_float("NavSpeedLimit", 0)
       self.pm.send('navInstruction', msg)
       return
 
@@ -344,14 +343,9 @@ class RouteEngine:
 
     if ('maxspeed' in closest.annotations) and self.localizer_valid:
       msg.navInstruction.speedLimit = closest.annotations['maxspeed']
-      SpeedLimitController.nav_speed_limit = closest.annotations['maxspeed']
-      SpeedLimitController.write_nav_state()
+      self.params_memory.put_float("NavSpeedLimit", closest.annotations['maxspeed'])
     if not self.localizer_valid or ('maxspeed' not in closest.annotations):
-      SpeedLimitController.nav_speed_limit = 0
-      SpeedLimitController.write_nav_state()
-
-    if SpeedLimitController.desired_speed_limit != 0:
-      msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit
+      self.params_memory.put_float("NavSpeedLimit", 0)
 
     # Speed limit sign type
     if 'speedLimitSign' in step:
@@ -367,6 +361,7 @@ class RouteEngine:
       if self.step_idx + 1 < len(self.route):
         self.step_idx += 1
         self.reset_recompute_limits()
+
         # Update the 'CurrentStep' value in the JSON
         if 'routes' in self.r2 and len(self.r2['routes']) > 0:
           self.r3['CurrentStep'] = self.step_idx
@@ -382,10 +377,11 @@ class RouteEngine:
           self.params.remove("NavDestination")
           self.clear_route()
 
-    # 5-10 Seconds to stop condition based on v_ego or minimum of 25 meters
+    # 5-10 Seconds to stop condition based on the current speed or minimum of 25 meters
     if self.conditional_navigation:
       v_ego = self.sm['carState'].vEgo
       seconds_to_stop = interp(v_ego, [0, 22.3, 44.7], [5, 10, 10])
+
       # Determine the location of the closest upcoming stopSign or trafficLight
       closest_condition_indices = [idx for idx in self.stop_signal if idx >= closest_idx]
       if closest_condition_indices: