Club Penguin 2.0

starpilot/controls/lib/speed_limit_controller.py

@@ -95,15 +95,6 @@ class SpeedLimitController:
     offset_map = OFFSET_MAP_METRIC if self.starpilot_toggles.is_metric else OFFSET_MAP_IMPERIAL
     return next((getattr(self.starpilot_toggles, offset) for low, high, offset in offset_map if low < self.target < high), 0)
 
-  def _read_next_map_speed_limit(self):
-    next_map_speed_limit = self.starpilot_planner.params_memory.get("NextMapSpeedLimit") or {}
-    if isinstance(next_map_speed_limit, (bytes, str)):
-      try:
-        next_map_speed_limit = json.loads(next_map_speed_limit)
-      except (TypeError, ValueError):
-        next_map_speed_limit = {}
-    return next_map_speed_limit if isinstance(next_map_speed_limit, dict) else {}
-
   @property
   def override_mode_enabled(self):
     if self.starpilot_toggles is None:
@@ -405,35 +396,15 @@ class SpeedLimitController:
   def update_map_speed_limit(self, v_ego, sm):
     next_speed_limit_distance = sm["mapdOut"].nextSpeedLimitDistance
 
-    if self.starpilot_toggles.speed_limit_filler:
-      next_map_speed_limit = self._read_next_map_speed_limit()
-      filler_map_speed_limit = self.starpilot_planner.params_memory.get_float("MapSpeedLimit")
-      filler_next_speed_limit = next_map_speed_limit.get("speedlimit", 0)
-
-      if filler_map_speed_limit > 0 or filler_next_speed_limit > 0:
-        self.map_speed_limit = filler_map_speed_limit
-        self.next_speed_limit = filler_next_speed_limit
-        next_speed_limit_distance = next_map_speed_limit.get("distance", 0)
-
-        next_latitude = next_map_speed_limit.get("latitude")
-        next_longitude = next_map_speed_limit.get("longitude")
-        if self.starpilot_planner.gps_valid and next_latitude is not None and next_longitude is not None:
-          current_latitude = self.starpilot_planner.gps_position.get("latitude")
-          current_longitude = self.starpilot_planner.gps_position.get("longitude")
-          next_speed_limit_distance = calculate_distance_to_point(current_latitude, current_longitude, next_latitude, next_longitude)
-      else:
-        self.map_speed_limit = 0
-        self.next_speed_limit = 0
+    way_sel = sm["mapdOut"].waySelectionType
+    if way_sel in (custom.WaySelectionType.current,
+                   custom.WaySelectionType.predicted,
+                   custom.WaySelectionType.extended):
+      self.map_speed_limit = sm["mapdOut"].speedLimit
+      self.next_speed_limit = sm["mapdOut"].nextSpeedLimit
     else:
-      way_sel = sm["mapdOut"].waySelectionType
-      if way_sel in (custom.WaySelectionType.current,
-                     custom.WaySelectionType.predicted,
-                     custom.WaySelectionType.extended):
-        self.map_speed_limit = sm["mapdOut"].speedLimit
-        self.next_speed_limit = sm["mapdOut"].nextSpeedLimit
-      else:
-        self.map_speed_limit = 0
-        self.next_speed_limit = 0
+      self.map_speed_limit = 0
+      self.next_speed_limit = 0
 
     if self.next_speed_limit > 0:
       if self.map_speed_limit < self.next_speed_limit:

starpilot/system/speed_limit_filler.py

@@ -2,6 +2,7 @@
 import json
 import math
 import requests
+import signal
 import time
 
 from collections import OrderedDict, deque
@@ -26,16 +27,13 @@ METERS_PER_DEG_LAT = 111_320
 PENDING_FLUSH_BATCH_SIZE = 1_000
 PENDING_FLUSH_INTERVAL_S = 60.0
 VETTING_INTERVAL_DAYS = 7
-ACTIVE_SEGMENT_BUFFER_METERS = 25
-MAX_BEARING_DELTA = 45
-MIN_LATERAL_MATCH_BUFFER = 12
 
 OVERPASS_API_URL = "https://overpass-api.de/api/interpreter"
 OVERPASS_STATUS_URL = "https://overpass-api.de/api/status"
 
 class MapSpeedLogger:
   def __init__(self):
-    self.params = Params(return_defaults=True)
+    self.params = Params()
     self.params_memory = Params(memory=True)
 
     self.cached_box = None
@@ -44,10 +42,9 @@ class MapSpeedLogger:
     self.cached_segments = {}
 
     self.dataset_additions = deque(maxlen=MAX_PENDING_ADDITIONS)
-    self.filtered_dataset = []
     self.last_dataset_flush = time.monotonic()
 
-    self.overpass_requests = self.params.get("OverpassRequests")
+    self.overpass_requests = self.get_json_param("OverpassRequests", {})
     self.overpass_requests.setdefault("day", datetime.now(timezone.utc).day)
     self.overpass_requests.setdefault("total_bytes", 0)
     self.overpass_requests.setdefault("total_requests", 0)
@@ -59,7 +56,6 @@ class MapSpeedLogger:
     self.gps_location_service = get_gps_location_service(self.params)
 
     self.sm = messaging.SubMaster(["deviceState", "starpilotCarState", "starpilotPlan", self.gps_location_service, "mapdOut", "modelV2"])
-    self.refresh_filtered_dataset()
 
   @property
   def can_make_overpass_request(self):
@@ -100,92 +96,20 @@ class MapSpeedLogger:
   def meters_to_deg_lon(meters, latitude):
     return meters / (METERS_PER_DEG_LAT * math.cos(latitude * CV.DEG_TO_RAD))
 
-  @staticmethod
-  def bearing_delta(bearing1, bearing2):
-    return abs((bearing1 - bearing2 + 180) % 360 - 180)
-
-  @staticmethod
-  def has_live_match_fields(entry):
-    required = {"bearing", "end_coordinates", "road_name", "road_width", "speed_limit", "start_coordinates"}
-    return required.issubset(entry.keys())
-
-  @staticmethod
-  def latlon_to_local_meters(origin_latitude, origin_longitude, latitude, longitude):
-    average_latitude = ((origin_latitude + latitude) / 2) * CV.DEG_TO_RAD
-    x = (longitude - origin_longitude) * METERS_PER_DEG_LAT * math.cos(average_latitude)
-    y = (latitude - origin_latitude) * METERS_PER_DEG_LAT
-    return x, y
-
-  def clear_live_speed_limits(self):
-    self.params_memory.remove("MapSpeedLimit")
-    self.params_memory.remove("NextMapSpeedLimit")
-
-  def refresh_filtered_dataset(self, filtered_dataset=None):
-    if filtered_dataset is None:
-      filtered_dataset = self.params.get("SpeedLimitsFiltered") or []
-
-    if filtered_dataset and not any(self.has_live_match_fields(entry) for entry in filtered_dataset):
-      dataset = self.cleanup_dataset(self.params.get("SpeedLimits"))
-      filtered_dataset = self.enrich_filtered_dataset(dataset, filtered_dataset)
-
-    self.filtered_dataset = [entry for entry in filtered_dataset if self.has_live_match_fields(entry)]
-
-  @staticmethod
-  def get_entry_identity(entry):
-    start_coordinates = entry.get("start_coordinates")
-    if not isinstance(start_coordinates, dict):
-      return None
-
-    try:
-      return (
-        bool(entry.get("incorrect_limit")),
-        round(float(start_coordinates["latitude"]), 6),
-        round(float(start_coordinates["longitude"]), 6),
-        str(entry.get("source", "")),
-        round(float(entry.get("speed_limit", 0)), 3),
-      )
-    except (KeyError, TypeError, ValueError):
-      return None
-
-  def enrich_filtered_dataset(self, dataset, filtered_dataset):
-    dataset_by_identity = {}
-    for entry in dataset:
-      if not self.has_live_match_fields(entry):
-        continue
-
-      identity = self.get_entry_identity(entry)
-      if identity is None or identity in dataset_by_identity:
-        continue
-
-      dataset_by_identity[identity] = entry
-
-    if not dataset_by_identity:
-      return filtered_dataset
-
-    filtered_entries = list(filtered_dataset)
-    updated = False
-    for index, entry in enumerate(filtered_entries):
-      if self.has_live_match_fields(entry):
-        continue
-
-      identity = self.get_entry_identity(entry)
-      matching_entry = dataset_by_identity.get(identity)
-      if matching_entry is None:
-        continue
-
-      filtered_entries[index] = {
-        **entry,
-        "bearing": matching_entry["bearing"],
-        "end_coordinates": matching_entry["end_coordinates"],
-        "road_name": matching_entry["road_name"],
-        "road_width": matching_entry["road_width"],
-      }
-      updated = True
-
-    if not updated:
-      return filtered_dataset
-
-    return self.cleanup_dataset(filtered_entries)
+  def get_json_param(self, key, default):
+    value = self.params.get(key)
+    if value is None:
+      return default
+    if isinstance(value, (dict, list)):
+      return value
+    if isinstance(value, (bytes, bytearray)):
+      value = value.decode("utf-8", errors="replace")
+    if isinstance(value, str):
+      try:
+        return json.loads(value)
+      except json.JSONDecodeError:
+        return default
+    return default
 
   def get_speed_limit_source(self):
     vision_speed_limit = self.params_memory.get_float("VisionSpeedLimit") if self.params.get_bool("VisionSpeedLimitDetection") else 0
@@ -199,15 +123,6 @@ class MapSpeedLogger:
         return speed_limit, source
     return None
 
-  def get_live_match_candidates(self, road_name, current_bearing):
-    return [
-      entry for entry in self.filtered_dataset
-      if self.has_live_match_fields(entry)
-      and entry["road_name"] == road_name
-      and entry["speed_limit"] > 0
-      and self.bearing_delta(entry["bearing"], current_bearing) <= MAX_BEARING_DELTA
-    ]
-
   def is_in_cached_box(self, latitude, longitude):
     if self.cached_box is None:
       return False
@@ -245,7 +160,7 @@ class MapSpeedLogger:
     if not should_flush:
       return
 
-    existing_dataset = self.params.get("SpeedLimits") or []
+    existing_dataset = self.get_json_param("SpeedLimits", [])
     existing_dataset.extend(self.dataset_additions)
 
     new_dataset = self.cleanup_dataset(existing_dataset)
@@ -254,74 +169,6 @@ class MapSpeedLogger:
     self.dataset_additions.clear()
     self.last_dataset_flush = now
 
-  def find_current_speed_limit_entry(self, latitude, longitude, road_name, current_bearing):
-    best_match = None
-    best_score = None
-
-    for entry in self.get_live_match_candidates(road_name, current_bearing):
-      start_latitude = entry["start_coordinates"]["latitude"]
-      start_longitude = entry["start_coordinates"]["longitude"]
-      end_latitude = entry["end_coordinates"]["latitude"]
-      end_longitude = entry["end_coordinates"]["longitude"]
-
-      segment_x, segment_y = self.latlon_to_local_meters(start_latitude, start_longitude, end_latitude, end_longitude)
-      segment_length_sq = segment_x ** 2 + segment_y ** 2
-      if segment_length_sq < 1:
-        continue
-
-      point_x, point_y = self.latlon_to_local_meters(start_latitude, start_longitude, latitude, longitude)
-      projection_ratio = (point_x * segment_x + point_y * segment_y) / segment_length_sq
-      segment_length = math.sqrt(segment_length_sq)
-      along_track = projection_ratio * segment_length
-
-      clamped_ratio = min(max(projection_ratio, 0.0), 1.0)
-      closest_x = segment_x * clamped_ratio
-      closest_y = segment_y * clamped_ratio
-      cross_track = math.hypot(point_x - closest_x, point_y - closest_y)
-
-      longitudinal_buffer = max(entry["speed_limit"] * 2, ACTIVE_SEGMENT_BUFFER_METERS)
-      lateral_buffer = max(entry["road_width"] * 2, MIN_LATERAL_MATCH_BUFFER)
-      if -longitudinal_buffer <= along_track <= segment_length + longitudinal_buffer and cross_track <= lateral_buffer:
-        score = (cross_track, abs(projection_ratio - 0.5))
-        if best_score is None or score < best_score:
-          best_match = entry
-          best_score = score
-
-    return best_match
-
-  def find_next_speed_limit_entry(self, latitude, longitude, road_name, current_bearing, current_entry=None):
-    best_match = None
-    best_score = None
-
-    heading_rad = current_bearing * CV.DEG_TO_RAD
-    heading_x = math.sin(heading_rad)
-    heading_y = math.cos(heading_rad)
-
-    current_segment_id = current_entry.get("segment_id") if current_entry else None
-
-    for entry in self.get_live_match_candidates(road_name, current_bearing):
-      if current_segment_id is not None and entry.get("segment_id") == current_segment_id:
-        continue
-
-      start_latitude = entry["start_coordinates"]["latitude"]
-      start_longitude = entry["start_coordinates"]["longitude"]
-
-      distance_to_start = calculate_distance_to_point(latitude, longitude, start_latitude, start_longitude)
-      if distance_to_start < 1:
-        continue
-
-      delta_x, delta_y = self.latlon_to_local_meters(latitude, longitude, start_latitude, start_longitude)
-      forward_distance = delta_x * heading_x + delta_y * heading_y
-      if forward_distance <= 0:
-        continue
-
-      score = (distance_to_start, self.bearing_delta(entry["bearing"], current_bearing))
-      if best_score is None or score < best_score:
-        best_match = entry
-        best_score = score
-
-    return best_match, best_score[0] if best_score is not None else 0
-
   def wait_for_api(self):
     while not is_url_pingable(OVERPASS_STATUS_URL):
       print("Waiting for Overpass API to be available...")
@@ -458,41 +305,6 @@ class MapSpeedLogger:
 
     self.previous_coordinates = {"latitude": current_latitude, "longitude": current_longitude}
 
-  def update_live_speed_limits(self):
-    gps_location = self.sm[self.gps_location_service]
-    current_latitude = gps_location.latitude
-    current_longitude = gps_location.longitude
-    current_bearing = gps_location.bearingDeg
-    road_name = self.sm["mapdOut"].roadName
-
-    if (current_latitude == 0 and current_longitude == 0) or not road_name or not self.filtered_dataset:
-      self.clear_live_speed_limits()
-      return
-
-    current_entry = self.find_current_speed_limit_entry(current_latitude, current_longitude, road_name, current_bearing)
-    next_entry, next_distance = self.find_next_speed_limit_entry(
-      current_latitude,
-      current_longitude,
-      road_name,
-      current_bearing,
-      current_entry=current_entry,
-    )
-
-    if current_entry:
-      self.params_memory.put_float("MapSpeedLimit", current_entry["speed_limit"])
-    else:
-      self.params_memory.remove("MapSpeedLimit")
-
-    if next_entry:
-      self.params_memory.put("NextMapSpeedLimit", {
-        "distance": next_distance,
-        "latitude": next_entry["start_coordinates"]["latitude"],
-        "longitude": next_entry["start_coordinates"]["longitude"],
-        "speedlimit": next_entry["speed_limit"],
-      })
-    else:
-      self.params_memory.remove("NextMapSpeedLimit")
-
   def process_new_entries(self, dataset, filtered_dataset):
     existing_segment_ids = {entry["segment_id"] for entry in filtered_dataset if "segment_id" in entry}
     entries_to_process = list(dataset)
@@ -551,9 +363,8 @@ class MapSpeedLogger:
 
     self.cached_box, self.cached_segments = None, {}
 
-    dataset = self.cleanup_dataset(self.params.get("SpeedLimits"))
-    filtered_dataset = self.cleanup_dataset(self.params.get("SpeedLimitsFiltered"))
-    filtered_dataset = self.enrich_filtered_dataset(dataset, filtered_dataset)
+    dataset = self.cleanup_dataset(self.get_json_param("SpeedLimits", []))
+    filtered_dataset = self.cleanup_dataset(self.get_json_param("SpeedLimitsFiltered", []))
 
     filtered_dataset = self.vet_entries(filtered_dataset)
     self.update_params(dataset, filtered_dataset)
@@ -564,7 +375,6 @@ class MapSpeedLogger:
       self.process_new_entries(dataset, filtered_dataset)
 
     self.update_params(dataset, filtered_dataset)
-    self.refresh_filtered_dataset(filtered_dataset)
     self.params_memory.put("UpdateSpeedLimitsStatus", "Completed!")
 
   def update_cached_segments(self, latitude, longitude, vetting=False):
@@ -621,6 +431,13 @@ class MapSpeedLogger:
 def main():
   logger = MapSpeedLogger()
 
+  def handle_signal(signum, frame):
+    logger.flush_pending_dataset_additions(force=True)
+    raise SystemExit(0)
+
+  signal.signal(signal.SIGINT, handle_signal)
+  signal.signal(signal.SIGTERM, handle_signal)
+
   previously_started = False
 
   while True:
@@ -628,7 +445,6 @@ def main():
 
     if logger.sm["deviceState"].started:
       logger.log_speed_limit()
-      logger.update_live_speed_limits()
 
       previously_started = True
     elif previously_started:
@@ -637,15 +453,12 @@ def main():
       if logger.sm["deviceState"].networkType in (NetworkType.ethernet, NetworkType.wifi):
         logger.params_memory.put_bool("UpdateSpeedLimits", True)
 
-      logger.clear_live_speed_limits()
-
       previously_started = False
     elif logger.params_memory.get_bool("UpdateSpeedLimits"):
       logger.process_speed_limits()
 
       logger.params_memory.remove("UpdateSpeedLimits")
     else:
-      logger.clear_live_speed_limits()
       time.sleep(5)
 
 if __name__ == "__main__":

system/manager/process_config.py

@@ -151,7 +151,7 @@ procs += [
   PythonProcess("device_syncd", "starpilot.system.device_syncd", always_run),
   PythonProcess("starpilot_process", "starpilot.starpilot_process", always_run),
   PythonProcess("mapd", "starpilot.navigation.mapd_wrapper", always_run),
-  PythonProcess("speed_limit_filler", "starpilot.system.speed_limit_filler", run_speed_limit_filler),
+  PythonProcess("speed_limit_filler", "starpilot.system.speed_limit_filler", run_speed_limit_filler, nice=19),
   PythonProcess("speed_limit_vision", "starpilot.system.speed_limit_vision", run_speed_limit_vision, nice=19),
 ]