diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index 703c2c167..3c21ae823 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/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: diff --git a/starpilot/system/speed_limit_filler.py b/starpilot/system/speed_limit_filler.py index 52c1e2d58..f172c697a 100644 --- a/starpilot/system/speed_limit_filler.py +++ b/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__": diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 30496c419..abfd71546 100644 --- a/system/manager/process_config.py +++ b/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), ]