IQ.Lvbs CI [bot]
486 lines
17 KiB
Python
486 lines
17 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
"""
|
|
import calendar
|
|
import json
|
|
import time
|
|
import numpy as np
|
|
|
|
from concurrent.futures import ThreadPoolExecutor
|
|
|
|
from openpilot.common.constants import CV
|
|
from openpilot.common.realtime import DT_MDL
|
|
from openpilot.common.swaglog import cloudlog
|
|
|
|
from openpilot.iqpilot.common.k3_slc_log import k3_slc_log
|
|
from openpilot.iqpilot.common.slc_utilities import calculate_bearing_offset, is_url_pingable
|
|
from openpilot.iqpilot.common.slc_variables import (
|
|
FREE_MAPBOX_REQUESTS,
|
|
OFFSET_MAP_IMPERIAL,
|
|
OFFSET_MAP_METRIC
|
|
)
|
|
|
|
try:
|
|
import requests
|
|
except ImportError:
|
|
requests = None
|
|
|
|
|
|
class SpeedLimitController:
|
|
def __init__(self, params):
|
|
self.params = params
|
|
|
|
self.calling_mapbox = False
|
|
self.override_slc = False
|
|
|
|
self.denied_target = 0
|
|
self.map_speed_limit = 0
|
|
self.mapbox_limit = 0
|
|
self.next_speed_limit = 0
|
|
self.overridden_speed = 0
|
|
self.segment_distance = 0
|
|
self.speed_limit_changed_timer = 0
|
|
self.target = 0
|
|
self.unconfirmed_speed_limit = 0
|
|
|
|
self.previous_source = "None"
|
|
self.source = "None"
|
|
self.active_source = "None"
|
|
self.active_target = 0.0
|
|
|
|
self.speed_limit_accepted = False
|
|
|
|
self.gps_valid = False
|
|
self.gps_position = {
|
|
"bearing": 0,
|
|
"latitude": 0,
|
|
"longitude": 0
|
|
}
|
|
|
|
mapbox_requests_raw = self.params.get("MapBoxRequests")
|
|
if isinstance(mapbox_requests_raw, dict):
|
|
self.mapbox_requests = mapbox_requests_raw
|
|
elif mapbox_requests_raw is not None:
|
|
try:
|
|
if isinstance(mapbox_requests_raw, bytes):
|
|
self.mapbox_requests = json.loads(mapbox_requests_raw.decode('utf-8'))
|
|
elif isinstance(mapbox_requests_raw, str):
|
|
self.mapbox_requests = json.loads(mapbox_requests_raw)
|
|
else:
|
|
self.mapbox_requests = {}
|
|
except (json.JSONDecodeError, AttributeError, TypeError):
|
|
self.mapbox_requests = {}
|
|
else:
|
|
self.mapbox_requests = {}
|
|
self.mapbox_requests.setdefault("total_requests", 0)
|
|
self.mapbox_requests.setdefault("max_requests", FREE_MAPBOX_REQUESTS - (28 * 100))
|
|
|
|
self.mapbox_host = "https://api.mapbox.com"
|
|
self.mapbox_token = self.params.get("MapboxToken")
|
|
if self.mapbox_token is not None and isinstance(self.mapbox_token, bytes):
|
|
self.mapbox_token = self.mapbox_token.decode('utf-8')
|
|
|
|
previous_limit = self.params.get("PreviousSpeedLimit")
|
|
if previous_limit is not None:
|
|
try:
|
|
if isinstance(previous_limit, bytes):
|
|
self.previous_target = float(previous_limit.decode('utf-8'))
|
|
else:
|
|
self.previous_target = float(previous_limit)
|
|
except (ValueError, AttributeError):
|
|
self.previous_target = 0.0
|
|
else:
|
|
self.previous_target = 0.0
|
|
|
|
self.executor = ThreadPoolExecutor(max_workers=1)
|
|
self._last_mapbox_log_t = 0.0
|
|
self._last_mapbox_diag_t = 0.0
|
|
self._last_mapbox_diag_message = None
|
|
|
|
self.session = requests.Session() if requests is not None else None
|
|
if self.session is not None:
|
|
self.session.headers.update({"Accept-Language": "en"})
|
|
self.session.headers.update({"User-Agent": "iqpilot-mapbox-speed-limit-retriever/1.0"})
|
|
|
|
@staticmethod
|
|
def _is_alive(sm, key: str) -> bool:
|
|
if hasattr(sm, "alive"):
|
|
return bool(sm.alive.get(key, False))
|
|
return isinstance(sm, dict) and key in sm
|
|
|
|
def get_offset(self, is_metric):
|
|
offset_map = OFFSET_MAP_METRIC if is_metric else OFFSET_MAP_IMPERIAL
|
|
for low, high, offset_param in offset_map:
|
|
if low < self.target < high:
|
|
offset_value = self.params.get(offset_param)
|
|
if offset_value is not None:
|
|
if isinstance(offset_value, bytes):
|
|
return float(offset_value.decode('utf-8'))
|
|
return float(offset_value)
|
|
return 0
|
|
return 0
|
|
|
|
def _log_mapbox_diag(self, message: str, force: bool = False) -> None:
|
|
now_mono = time.monotonic()
|
|
if not force and message == self._last_mapbox_diag_message and now_mono - self._last_mapbox_diag_t < 5.0:
|
|
return
|
|
if not force and now_mono - self._last_mapbox_diag_t < 2.0:
|
|
return
|
|
self._last_mapbox_diag_t = now_mono
|
|
self._last_mapbox_diag_message = message
|
|
cloudlog.info(message)
|
|
k3_slc_log(message)
|
|
|
|
def update_gps(self, sm):
|
|
llk_valid = False
|
|
if self._is_alive(sm, "liveLocationKalman"):
|
|
llk = sm["liveLocationKalman"]
|
|
llk_valid = bool(getattr(llk, "gpsOK", False))
|
|
|
|
if self._is_alive(sm, "gpsLocationExternal"):
|
|
gps_location = sm["gpsLocationExternal"]
|
|
elif self._is_alive(sm, "gpsLocation"):
|
|
gps_location = sm["gpsLocation"]
|
|
else:
|
|
gps_location = None
|
|
|
|
gps_has_fix = False
|
|
if gps_location is not None:
|
|
gps_has_fix = bool(getattr(gps_location, "hasFix", False))
|
|
gps_has_fix |= bool(getattr(gps_location, "flags", 0) > 0)
|
|
|
|
if gps_location and (gps_has_fix or llk_valid):
|
|
self.gps_valid = True
|
|
self.gps_position = {
|
|
"bearing": getattr(gps_location, 'bearingDeg', 0),
|
|
"latitude": getattr(gps_location, 'latitude', 0),
|
|
"longitude": getattr(gps_location, 'longitude', 0)
|
|
}
|
|
else:
|
|
self.gps_valid = False
|
|
|
|
def get_mapbox_speed_limit(self, now, time_validated, v_ego, sm):
|
|
if requests is None or self.session is None:
|
|
self._log_mapbox_diag("SLC Mapbox skipped: requests session unavailable")
|
|
self.mapbox_limit = 0
|
|
self.segment_distance = 0
|
|
return
|
|
|
|
steer_angle = sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg
|
|
if not self.gps_valid or not self.mapbox_token or steer_angle >= 45:
|
|
self._log_mapbox_diag(
|
|
"SLC Mapbox skipped: "
|
|
f"gps_valid={self.gps_valid} token={bool(self.mapbox_token)} steer_angle={round(float(steer_angle), 2)}"
|
|
)
|
|
self.mapbox_limit = 0
|
|
self.segment_distance = 0
|
|
return
|
|
|
|
if v_ego < 1:
|
|
self._log_mapbox_diag(f"SLC Mapbox skipped: low_speed v_ego={round(float(v_ego), 2)}")
|
|
return
|
|
|
|
if self.segment_distance > 0:
|
|
self._log_mapbox_diag(
|
|
"SLC Mapbox deferred: "
|
|
f"segment_distance={round(float(self.segment_distance), 2)} v_ego={round(float(v_ego), 2)}"
|
|
)
|
|
self.segment_distance -= v_ego * DT_MDL
|
|
return
|
|
|
|
if self.calling_mapbox:
|
|
self._log_mapbox_diag("SLC Mapbox deferred: request already in flight")
|
|
self.segment_distance = v_ego
|
|
return
|
|
|
|
def make_request():
|
|
try:
|
|
self.calling_mapbox = True
|
|
|
|
successful = False
|
|
|
|
if not is_url_pingable(self.mapbox_host):
|
|
self._log_mapbox_diag("SLC Mapbox skipped: host not pingable", force=True)
|
|
self.segment_distance = 1000
|
|
return None
|
|
|
|
if time_validated:
|
|
current_month = now.month
|
|
if current_month != self.mapbox_requests.get("month"):
|
|
self.mapbox_requests.update({
|
|
"month": current_month,
|
|
"total_requests": 0,
|
|
"max_requests": FREE_MAPBOX_REQUESTS - calendar.monthrange(now.year, current_month)[1] * 100,
|
|
})
|
|
|
|
self.mapbox_requests["total_requests"] += 1
|
|
self.params.put_nonblocking("MapBoxRequests", self.mapbox_requests)
|
|
|
|
current_bearing = self.gps_position.get("bearing")
|
|
current_latitude = self.gps_position.get("latitude")
|
|
current_longitude = self.gps_position.get("longitude")
|
|
|
|
future_latitude, future_longitude = calculate_bearing_offset(current_latitude, current_longitude, current_bearing, v_ego)
|
|
|
|
self._log_mapbox_diag(
|
|
"SLC Mapbox request: "
|
|
f"lat={round(float(current_latitude), 6)} lon={round(float(current_longitude), 6)} "
|
|
f"bearing={round(float(current_bearing), 2)} v_ego={round(float(v_ego), 2)} "
|
|
f"future_lat={round(float(future_latitude), 6)} future_lon={round(float(future_longitude), 6)}",
|
|
force=True,
|
|
)
|
|
|
|
url = (
|
|
f"{self.mapbox_host}/matching/v5/mapbox/driving/"
|
|
f"{current_longitude},{current_latitude};"
|
|
f"{future_longitude},{future_latitude}.json"
|
|
)
|
|
|
|
mapbox_params = {
|
|
"access_token": self.mapbox_token,
|
|
"annotations": "maxspeed,distance",
|
|
"geometries": "polyline6",
|
|
"overview": "full",
|
|
"steps": "false",
|
|
"radiuses": "10;10",
|
|
"tidy": "true",
|
|
}
|
|
|
|
response = self.session.get(url, params=mapbox_params, timeout=10)
|
|
response.raise_for_status()
|
|
|
|
successful = True
|
|
|
|
return response.json()
|
|
except Exception as exception:
|
|
now_mono = time.monotonic()
|
|
if now_mono - self._last_mapbox_log_t >= 5.0:
|
|
self._last_mapbox_log_t = now_mono
|
|
message = f"SLC Mapbox request failed: {exception}"
|
|
cloudlog.warning(message)
|
|
k3_slc_log(message)
|
|
finally:
|
|
self.calling_mapbox = False
|
|
|
|
if not successful:
|
|
self.mapbox_limit = 0
|
|
self.segment_distance = v_ego
|
|
|
|
return None
|
|
|
|
def complete_request(future):
|
|
try:
|
|
data = future.result()
|
|
if data:
|
|
matchings = data.get("matchings") or []
|
|
if not matchings:
|
|
self._log_mapbox_diag("SLC Mapbox callback: no matchings", force=True)
|
|
self.mapbox_limit = 0
|
|
self.segment_distance = v_ego
|
|
return
|
|
|
|
legs = (matchings[0] or {}).get("legs") or []
|
|
if not legs:
|
|
self._log_mapbox_diag("SLC Mapbox callback: no legs", force=True)
|
|
self.mapbox_limit = 0
|
|
self.segment_distance = v_ego
|
|
return
|
|
|
|
annotation = legs[0].get("annotation") or {}
|
|
|
|
distances = annotation.get("distance") or [v_ego]
|
|
segment_distance = distances[0]
|
|
|
|
speed_data = annotation.get("maxspeed", [])
|
|
speed_limit_kph = 0
|
|
if speed_data:
|
|
first_segment_speed = speed_data[0]
|
|
speed_limit_kph = (first_segment_speed.get("speed") if first_segment_speed.get("speed") != "none" else 0) or 0
|
|
|
|
if speed_limit_kph > 0:
|
|
self.mapbox_limit = speed_limit_kph * CV.KPH_TO_MS
|
|
self.segment_distance = segment_distance
|
|
self._log_mapbox_diag(
|
|
"SLC Mapbox callback: "
|
|
f"speed_limit_kph={round(float(speed_limit_kph), 2)} segment_distance={round(float(segment_distance), 2)}",
|
|
force=True,
|
|
)
|
|
return
|
|
|
|
self._log_mapbox_diag(
|
|
"SLC Mapbox callback: "
|
|
f"no usable maxspeed speed_data_len={len(speed_data)} segment_distance={round(float(segment_distance), 2)}",
|
|
force=True,
|
|
)
|
|
|
|
self.mapbox_limit = 0
|
|
self.segment_distance = v_ego
|
|
|
|
except Exception as exception:
|
|
now_mono = time.monotonic()
|
|
if now_mono - self._last_mapbox_log_t >= 5.0:
|
|
self._last_mapbox_log_t = now_mono
|
|
message = f"SLC Mapbox callback failed: {exception}"
|
|
cloudlog.warning(message)
|
|
k3_slc_log(message)
|
|
self.mapbox_limit = 0
|
|
self.segment_distance = v_ego
|
|
|
|
future = self.executor.submit(make_request)
|
|
future.add_done_callback(complete_request)
|
|
|
|
def handle_limit_change(self, desired_source, desired_target, sm, slc_params):
|
|
self.speed_limit_changed_timer += DT_MDL
|
|
|
|
car_state_iq = sm["iqCarState"]
|
|
accel_pressed = getattr(car_state_iq, "accelPressed", False)
|
|
speed_limit_accepted = (accel_pressed and sm["carControl"].longActive) or self.speed_limit_accepted
|
|
|
|
decel_pressed = getattr(car_state_iq, "decelPressed", False)
|
|
speed_limit_denied = decel_pressed or (self.speed_limit_changed_timer >= 30)
|
|
|
|
if speed_limit_accepted:
|
|
self.overridden_speed = 0
|
|
|
|
self.source = desired_source
|
|
self.target = desired_target
|
|
|
|
self.speed_limit_accepted = False
|
|
|
|
elif speed_limit_denied:
|
|
self.denied_target = desired_target
|
|
|
|
self.previous_source = desired_source
|
|
self.previous_target = desired_target
|
|
|
|
elif desired_target < self.target and not slc_params.get("speed_limit_confirmation_lower", False):
|
|
self.source = desired_source
|
|
self.target = desired_target
|
|
|
|
elif desired_target > self.target and not slc_params.get("speed_limit_confirmation_higher", False):
|
|
self.source = desired_source
|
|
self.target = desired_target
|
|
|
|
else:
|
|
self.source = "None"
|
|
self.unconfirmed_speed_limit = desired_target
|
|
|
|
if self.target != self.previous_target and self.target > 0 and not speed_limit_denied:
|
|
self.denied_target = 0
|
|
|
|
self.previous_source = self.source
|
|
self.previous_target = self.target
|
|
|
|
self.params.put_nonblocking("PreviousSpeedLimit", float(self.target))
|
|
|
|
def update_limits(self, dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm, slc_params):
|
|
self.update_gps(sm)
|
|
self.update_map_speed_limit(v_ego, sm, slc_params)
|
|
|
|
if self.mapbox_requests["total_requests"] < self.mapbox_requests["max_requests"]:
|
|
self.get_mapbox_speed_limit(now, time_validated, v_ego, sm)
|
|
else:
|
|
self.mapbox_limit = 0
|
|
self.segment_distance = 0
|
|
|
|
limits = {
|
|
"Dashboard": dashboard_speed_limit,
|
|
"Mapbox": self.mapbox_limit,
|
|
"Map Data": self.map_speed_limit
|
|
}
|
|
filtered_limits = {source: limit for source, limit in limits.items() if limit >= 1}
|
|
|
|
priority_highest = slc_params.get("speed_limit_priority_highest", False)
|
|
priority_lowest = slc_params.get("speed_limit_priority_lowest", False)
|
|
priority1 = slc_params.get("speed_limit_priority1", "Dashboard")
|
|
priority2 = slc_params.get("speed_limit_priority2", "Mapbox")
|
|
priority3 = slc_params.get("speed_limit_priority3", "Map Data")
|
|
|
|
if priority_highest:
|
|
desired_source = max(filtered_limits, key=filtered_limits.get, default="None")
|
|
desired_target = filtered_limits.get(desired_source, 0)
|
|
|
|
elif priority_lowest:
|
|
desired_source = min(filtered_limits, key=filtered_limits.get, default="None")
|
|
desired_target = filtered_limits.get(desired_source, 0)
|
|
|
|
elif filtered_limits:
|
|
for priority in [priority1, priority2, priority3]:
|
|
if priority in filtered_limits:
|
|
desired_source = priority
|
|
desired_target = filtered_limits[desired_source]
|
|
break
|
|
else:
|
|
desired_source = "None"
|
|
desired_target = 0
|
|
|
|
else:
|
|
desired_source = "None"
|
|
desired_target = 0
|
|
|
|
if desired_target == 0 or self.target == 0:
|
|
if self.denied_target != self.previous_target > 0 and slc_params.get("slc_fallback_previous_speed_limit", False):
|
|
desired_source = self.previous_source
|
|
desired_target = self.previous_target
|
|
|
|
self.target = desired_target
|
|
|
|
elif sm["selfdriveState"].enabled and slc_params.get("slc_fallback_set_speed", False):
|
|
desired_source = "None"
|
|
desired_target = v_cruise
|
|
|
|
self.active_source = desired_source
|
|
self.active_target = float(desired_target)
|
|
|
|
if abs(desired_target - self.previous_target) >= 1:
|
|
self.handle_limit_change(desired_source, desired_target, sm, slc_params)
|
|
elif desired_source != self.source and abs(desired_target - self.target) < 1:
|
|
self.source = desired_source
|
|
else:
|
|
self.speed_limit_changed_timer = 0
|
|
self.unconfirmed_speed_limit = 0
|
|
|
|
def update_map_speed_limit(self, v_ego, sm, slc_params):
|
|
|
|
if self._is_alive(sm, "iqLiveData"):
|
|
map_data = sm["iqLiveData"]
|
|
self.map_speed_limit = getattr(map_data, "speedLimit", 0) if getattr(map_data, "speedLimitValid", False) else 0
|
|
self.next_speed_limit = getattr(map_data, "speedLimitAhead", 0) if getattr(map_data, "speedLimitAheadValid", False) else 0
|
|
|
|
if self.next_speed_limit > 0:
|
|
if self.map_speed_limit < self.next_speed_limit:
|
|
lookahead_higher = slc_params.get("map_speed_lookahead_higher", 5)
|
|
max_lookahead = lookahead_higher * v_ego
|
|
elif self.map_speed_limit > self.next_speed_limit:
|
|
lookahead_lower = slc_params.get("map_speed_lookahead_lower", 5)
|
|
max_lookahead = lookahead_lower * v_ego
|
|
else:
|
|
max_lookahead = 0
|
|
|
|
next_distance = getattr(map_data, "speedLimitAheadDistance", 0)
|
|
if next_distance < max_lookahead:
|
|
self.map_speed_limit = self.next_speed_limit
|
|
else:
|
|
self.map_speed_limit = 0
|
|
self.next_speed_limit = 0
|
|
|
|
def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm, slc_params, is_metric):
|
|
offset = self.get_offset(is_metric)
|
|
|
|
self.override_slc = self.overridden_speed > self.target + offset > 0
|
|
self.override_slc |= sm["carState"].gasPressed and v_ego > self.target + offset > 0
|
|
self.override_slc &= sm["selfdriveState"].enabled
|
|
|
|
if self.override_slc:
|
|
if slc_params.get("speed_limit_controller_override_manual", False):
|
|
if sm["carState"].gasPressed:
|
|
self.overridden_speed = max(v_ego + v_ego_diff, self.overridden_speed)
|
|
self.overridden_speed = float(np.clip(self.overridden_speed, self.target + offset, v_cruise + v_cruise_diff))
|
|
elif slc_params.get("speed_limit_controller_override_set_speed", False):
|
|
self.overridden_speed = v_cruise + v_cruise_diff
|
|
|
|
self.source = "None"
|
|
else:
|
|
self.overridden_speed = 0
|