#!/usr/bin/env python3 import json import math import requests import time from collections import OrderedDict, deque from datetime import datetime, timedelta, timezone from cereal import log, messaging from openpilot.common.conversions import Conversions as CV from openpilot.frogpilot.common.frogpilot_utilities import calculate_distance_to_point, calculate_lane_width, is_url_pingable from openpilot.frogpilot.common.frogpilot_variables import params, params_memory NetworkType = log.DeviceState.NetworkType BOUNDING_BOX_RADIUS_DEGREE = 0.1 MAX_ENTRIES = 1_000_000 MAX_OVERPASS_DATA_BYTES = 1_073_741_824 MAX_OVERPASS_REQUESTS = 10_000 METERS_PER_DEG_LAT = 111_320 VETTING_INTERVAL_DAYS = 7 OVERPASS_API_URL = "https://overpass-api.de/api/interpreter" OVERPASS_STATUS_URL = "https://overpass-api.de/api/status" class MapSpeedLogger: def __init__(self): self.cached_box = None self.previous_coordinates = None self.cached_segments = {} self.dataset_additions = deque(maxlen=MAX_ENTRIES) self.overpass_requests = json.loads(params.get("OverpassRequests") or "{}") self.overpass_requests.setdefault("day", datetime.now(timezone.utc).day) self.overpass_requests.setdefault("total_bytes", 0) self.overpass_requests.setdefault("total_requests", 0) self.session = requests.Session() self.session.headers.update({"Accept-Language": "en"}) self.session.headers.update({"User-Agent": "frogpilot-map-speed-logger/1.0 (https://github.com/FrogAi/FrogPilot)"}) self.sm = messaging.SubMaster(["deviceState", "frogpilotCarState", "frogpilotNavigation", "frogpilotPlan", "liveLocationKalman", "modelV2"]) @property def can_make_overpass_request(self): return self.overpass_requests["total_bytes"] < MAX_OVERPASS_DATA_BYTES and self.overpass_requests["total_requests"] < MAX_OVERPASS_REQUESTS @property def should_stop_processing(self): return self.sm["deviceState"].started or not params_memory.get_bool("UpdateSpeedLimits") @staticmethod def cleanup_dataset(dataset): cleaned_data = OrderedDict() for item in dataset: if "last_vetted" in item: required = {"incorrect_limit", "last_vetted", "segment_id", "source", "speed_limit", "start_coordinates"} else: required = {"bearing", "end_coordinates", "incorrect_limit", "road_name", "road_width", "source", "speed_limit", "start_coordinates"} if not required.issubset(item.keys()): continue entry_copy = item.copy() entry_copy.pop("last_vetted", None) key = json.dumps(entry_copy, sort_keys=True) cleaned_data[key] = item return deque(cleaned_data.values(), maxlen=MAX_ENTRIES) @staticmethod def meters_to_deg_lat(meters): return meters / METERS_PER_DEG_LAT @staticmethod def meters_to_deg_lon(meters, latitude): return meters / (METERS_PER_DEG_LAT * math.cos(latitude * CV.DEG_TO_RAD)) def get_speed_limit_source(self): sources = [ (self.sm["frogpilotNavigation"].navigationSpeedLimit, "NOO"), (self.sm["frogpilotPlan"].slcMapboxSpeedLimit, "Mapbox"), (self.sm["frogpilotCarState"].dashboardSpeedLimit, "Dashboard"), ] for speed_limit, source in sources: if speed_limit > 0: return speed_limit, source return None def is_in_cached_box(self, latitude, longitude): if self.cached_box is None: return False return self.cached_box["min_latitude"] <= latitude <= self.cached_box["max_latitude"] and \ self.cached_box["min_longitude"] <= longitude <= self.cached_box["max_longitude"] def record_overpass_request(self, content_bytes): self.overpass_requests["total_bytes"] += content_bytes self.overpass_requests["total_requests"] += 1 def reset_daily_api_limits(self): current_day = datetime.now(timezone.utc).day if current_day != self.overpass_requests["day"]: self.overpass_requests.update({ "day": current_day, "total_requests": 0, "total_bytes": 0, }) def update_params(self, dataset, filtered_dataset): params.put("OverpassRequests", json.dumps(self.overpass_requests)) params.put("SpeedLimits", json.dumps(list(dataset))) params.put("SpeedLimitsFiltered", json.dumps(list(filtered_dataset))) def wait_for_api(self): while not is_url_pingable(OVERPASS_STATUS_URL): print("Waiting for Overpass API to be available...") self.sm.update() if self.should_stop_processing: return False time.sleep(5) return True def fetch_from_overpass(self, latitude, longitude): min_lat = latitude - BOUNDING_BOX_RADIUS_DEGREE max_lat = latitude + BOUNDING_BOX_RADIUS_DEGREE min_lon = longitude - BOUNDING_BOX_RADIUS_DEGREE max_lon = longitude + BOUNDING_BOX_RADIUS_DEGREE self.cached_box = {"min_latitude": min_lat, "max_latitude": max_lat, "min_longitude": min_lon, "max_longitude": max_lon} self.cached_segments.clear() query = ( f"[out:json][timeout:90][maxsize:{MAX_OVERPASS_DATA_BYTES // 10}];" f"way({min_lat:.5f},{min_lon:.5f},{max_lat:.5f},{max_lon:.5f})" "[highway~'^(motorway|motorway_link|primary|primary_link|residential|" "secondary|secondary_link|tertiary|tertiary_link|trunk|trunk_link)$'];" "out geom qt;" ) try: response = self.session.post(OVERPASS_API_URL, data=query, timeout=90) self.record_overpass_request(len(response.content)) if response.status_code == 429: retry_after = int(response.headers.get("Retry-After", 10)) print(f"Overpass API rate limit hit. Retrying in {retry_after} seconds.") time.sleep(retry_after) response = self.session.post(OVERPASS_API_URL, data=query, timeout=90) self.record_overpass_request(len(response.content)) response.raise_for_status() return response.json().get("elements", []) except requests.exceptions.RequestException as exception: print(f"Overpass API request failed: {exception}") self.cached_segments.clear() return [] def filter_segments_for_entry(self, entry): bearing_rad = entry["bearing"] * CV.DEG_TO_RAD start_lat, start_lon = entry["start_coordinates"]["latitude"], entry["start_coordinates"]["longitude"] end_lat, end_lon = entry["end_coordinates"]["latitude"], entry["end_coordinates"]["longitude"] mid_lat = (start_lat + end_lat) / 2 forward_buffer_lat = self.meters_to_deg_lat(entry["speed_limit"]) forward_buffer_lon = self.meters_to_deg_lon(entry["speed_limit"], mid_lat) side_buffer_lat = self.meters_to_deg_lat(entry["road_width"]) side_buffer_lon = self.meters_to_deg_lon(entry["road_width"], mid_lat) delta_lat_fwd = forward_buffer_lat * math.cos(bearing_rad) delta_lon_fwd = forward_buffer_lon * math.sin(bearing_rad) delta_lat_side = side_buffer_lat * math.cos(bearing_rad + math.pi / 2) delta_lon_side = side_buffer_lon * math.sin(bearing_rad + math.pi / 2) min_lat = min(start_lat, end_lat) - abs(delta_lat_fwd) - abs(delta_lat_side) max_lat = max(start_lat, end_lat) + abs(delta_lat_fwd) + abs(delta_lat_side) min_lon = min(start_lon, end_lon) - abs(delta_lon_fwd) - abs(delta_lon_side) max_lon = max(start_lon, end_lon) + abs(delta_lon_fwd) + abs(delta_lon_side) relevant_segments = [] for segment in self.cached_segments.values(): if not segment or "nodes" not in segment: continue latitudes = [node[0] for node in segment["nodes"]] longitudes = [node[1] for node in segment["nodes"]] if not (max(latitudes) < min_lat or min(latitudes) > max_lat or max(longitudes) < min_lon or min(longitudes) > max_lon): relevant_segments.append(segment) return relevant_segments def log_speed_limit(self): if not self.sm.updated["liveLocationKalman"]: return localizer_valid = self.sm["liveLocationKalman"].status == log.LiveLocationKalman.Status.valid and self.sm["liveLocationKalman"].positionGeodetic.valid if not (self.sm["liveLocationKalman"].gpsOK and localizer_valid): self.previous_coordinates = None return current_latitude = self.sm["liveLocationKalman"].positionGeodetic.value[0] current_longitude = self.sm["liveLocationKalman"].positionGeodetic.value[1] if self.previous_coordinates is None: self.previous_coordinates = {"latitude": current_latitude, "longitude": current_longitude} return current_speed_source = self.get_speed_limit_source() valid_sources = {source[0] for source in [current_speed_source] if source and source[0] > 0} map_speed = params_memory.get_float("MapSpeedLimit") is_incorrect_limit = bool(map_speed > 0 and valid_sources and all(abs(map_speed - source) > 1 for source in valid_sources)) if map_speed > 0 and not is_incorrect_limit: self.previous_coordinates = None return road_name = params_memory.get("RoadName", encoding="utf-8") if not road_name or not current_speed_source: return distance = calculate_distance_to_point( self.previous_coordinates["latitude"] * CV.DEG_TO_RAD, self.previous_coordinates["longitude"] * CV.DEG_TO_RAD, current_latitude * CV.DEG_TO_RAD, current_longitude * CV.DEG_TO_RAD ) if distance < 1: return speed_limit, source = current_speed_source self.dataset_additions.append({ "bearing": math.degrees(self.sm["liveLocationKalman"].calibratedOrientationNED.value[2]), "end_coordinates": {"latitude": current_latitude, "longitude": current_longitude}, "incorrect_limit": is_incorrect_limit, "road_name": road_name, "road_width": calculate_lane_width(self.sm["modelV2"].laneLines[1], self.sm["modelV2"].laneLines[2]), "source": source, "speed_limit": speed_limit, "start_coordinates": self.previous_coordinates, }) self.previous_coordinates = {"latitude": current_latitude, "longitude": current_longitude} 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) total_entries = len(entries_to_process) for i, entry in enumerate(entries_to_process): self.sm.update() if self.should_stop_processing: break if not self.can_make_overpass_request: params_memory.put("UpdateSpeedLimitsStatus", "Hit API limit...") time.sleep(5) break params_memory.put("UpdateSpeedLimitsStatus", f"Processing: {i + 1} / {total_entries}") start_coords = entry["start_coordinates"] self.update_cached_segments(start_coords["latitude"], start_coords["longitude"]) segments = self.filter_segments_for_entry(entry) dataset.remove(entry) for segment in segments: segment_id = segment["segment_id"] if segment_id in existing_segment_ids: continue if segment["maxspeed"] and not entry.get("incorrect_limit"): continue if segment["road_name"] != entry.get("road_name"): continue filtered_dataset.append({ "incorrect_limit": entry.get("incorrect_limit"), "last_vetted": datetime.now(timezone.utc).isoformat(), "segment_id": segment_id, "source": entry["source"], "speed_limit": entry["speed_limit"], "start_coordinates": entry["start_coordinates"], }) existing_segment_ids.add(segment_id) if i % 100 == 0: self.update_params(dataset, filtered_dataset) def process_speed_limits(self): self.reset_daily_api_limits() if not self.wait_for_api(): return self.cached_box, self.cached_segments = None, {} dataset = self.cleanup_dataset(json.loads(params.get("SpeedLimits") or "[]")) filtered_dataset = self.cleanup_dataset(json.loads(params.get("SpeedLimitsFiltered") or "[]")) filtered_dataset = self.vet_entries(filtered_dataset) self.update_params(dataset, filtered_dataset) if dataset and not self.should_stop_processing: self.cached_box, self.cached_segments = None, {} params_memory.put("UpdateSpeedLimitsStatus", "Calculating...") self.process_new_entries(dataset, filtered_dataset) self.update_params(dataset, filtered_dataset) params_memory.put("UpdateSpeedLimitsStatus", "Completed!") def update_cached_segments(self, latitude, longitude, vetting=False): if not self.is_in_cached_box(latitude, longitude): elements = self.fetch_from_overpass(latitude, longitude) for way in elements: if way.get("type") == "way" and (segment_id := way.get("id")): tags = way.get("tags", {}) if vetting: self.cached_segments[segment_id] = tags.get("maxspeed") elif "geometry" in way and (nodes := way["geometry"]): self.cached_segments[segment_id] = { "maxspeed": tags.get("maxspeed"), "nodes": [(node["lat"], node["lon"]) for node in nodes], "road_name": tags.get("name"), "segment_id": segment_id, } def vet_entries(self, filtered_dataset): dataset_list = list(filtered_dataset) total_to_vet = len(filtered_dataset) vetted_entries = deque(maxlen=MAX_ENTRIES) for i, entry in enumerate(dataset_list): self.sm.update() if self.should_stop_processing: vetted_entries.extend(dataset_list[i:]) break if not self.can_make_overpass_request: params_memory.put("UpdateSpeedLimitsStatus", "Hit API limit...") time.sleep(5) vetted_entries.extend(dataset_list[i:]) break params_memory.put("UpdateSpeedLimitsStatus", f"Vetting: {i + 1} / {total_to_vet}") last_vetted_time = datetime.fromisoformat(entry["last_vetted"]) if datetime.now(timezone.utc) - last_vetted_time < timedelta(days=VETTING_INTERVAL_DAYS): vetted_entries.append(entry) continue start_coords = entry["start_coordinates"] self.update_cached_segments(start_coords["latitude"], start_coords["longitude"], vetting=True) current_maxspeed = self.cached_segments.get(entry["segment_id"]) if current_maxspeed is None or (entry.get("incorrect_limit") and current_maxspeed != entry.get("speed_limit")): entry["last_vetted"] = datetime.now(timezone.utc).isoformat() vetted_entries.append(entry) return self.cleanup_dataset(list(vetted_entries)) def main(): logger = MapSpeedLogger() previously_started = False while True: logger.sm.update() if logger.sm["deviceState"].started: logger.log_speed_limit() previously_started = True elif previously_started: existing_dataset = json.loads(params.get("SpeedLimits") or "[]") existing_dataset.extend(logger.dataset_additions) new_dataset = logger.cleanup_dataset(existing_dataset) params.put("SpeedLimits", json.dumps(list(new_dataset))) if logger.sm["deviceState"].networkType in (NetworkType.ethernet, NetworkType.wifi): params_memory.put_bool("UpdateSpeedLimits", True) logger.dataset_additions.clear() previously_started = False elif params_memory.get_bool("UpdateSpeedLimits"): logger.process_speed_limits() params_memory.remove("UpdateSpeedLimits") else: time.sleep(5) if __name__ == "__main__": main()