IQ.Pilot Prebuilt Release @ ab07000

iqpilot/iq_maps/road_data/__init__.py

@@ -0,0 +1,25 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Shared tunables and a small debug logger for the offline road-name / turn-speed path.
+"""
+from openpilot.common.swaglog import cloudlog
+
+# seconds of road ahead we scan for upcoming turn-speed zones published on iqLiveData
+LOOK_AHEAD_HORIZON_TIME = 15.0
+# clear the on-screen road name once it has gone this long without a refresh (s)
+ROAD_NAME_TIMEOUT = 30
+
+R = 6373000.0            # mean Earth radius in metres (great-circle distance math)
+QUERY_RADIUS = 3000      # online OSM query reach, metres
+QUERY_RADIUS_OFFLINE = 2250  # offline-tile OSM query reach, metres
+
+_DEBUG = False
+_CLOUDLOG_DEBUG = False
+
+
+def debug_road_data(msg, log_to_cloud=True):
+  if _CLOUDLOG_DEBUG and log_to_cloud:
+    cloudlog.debug(msg)
+  if _DEBUG:
+    print(msg)

iqpilot/iq_maps/road_data/iq_road_layer.py

@@ -0,0 +1,67 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import json
+import math
+import platform
+
+from cereal import custom
+from openpilot.common.params import Params
+from openpilot.iqpilot.iq_maps.road_data.signal_bridge import RoadSignalBridge
+from openpilot.iqpilot.navd.helpers import Coordinate
+
+
+class IQRoadLayer(RoadSignalBridge):
+  def __init__(self):
+    super().__init__()
+    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
+
+  def refresh_position(self) -> None:
+    location = self.location_sub['iqLiveLocation']
+    self.fix_ready = (
+      location.solutionState == custom.IQLiveLocation.SolutionState.ready
+      and location.geodeticPosition.isValid
+    )
+
+    if self.fix_ready:
+      self.heading_deg = math.degrees(location.alignedOrientationNed.values[2])
+      self.last_coordinate = Coordinate(location.geodeticPosition.values[0], location.geodeticPosition.values[1])
+
+    if self.last_coordinate is None:
+      return
+
+    payload = {
+      "latitude": self.last_coordinate.latitude,
+      "longitude": self.last_coordinate.longitude,
+    }
+
+    if self.heading_deg is not None:
+      payload["bearing"] = self.heading_deg
+
+    self.mem_params.put("LastGPSPosition", json.dumps(payload))
+
+  def read_current_limit(self) -> float:
+    return float(self.mem_params.get("MapSpeedLimit") or 0.0)
+
+  def read_current_road(self) -> str:
+    return str(self.mem_params.get("RoadName") or "")
+
+  def read_upcoming_limit(self) -> tuple[float, float]:
+    raw_segment = self.mem_params.get("NextMapSpeedLimit")
+    if isinstance(raw_segment, bytes):
+      raw_segment = raw_segment.decode("utf-8")
+    try:
+      upcoming_segment = json.loads(raw_segment) if isinstance(raw_segment, str) and raw_segment else (raw_segment or {})
+    except json.JSONDecodeError:
+      upcoming_segment = {}
+
+    next_limit = float(upcoming_segment.get("speedlimit", 0.0) or 0.0)
+    target_lat = upcoming_segment.get("latitude")
+    target_lon = upcoming_segment.get("longitude")
+    distance_to_limit = 0.0
+
+    if target_lat is not None and target_lon is not None:
+      limit_coordinate = Coordinate(float(target_lat), float(target_lon))
+      distance_to_limit = (self.last_coordinate or Coordinate(0, 0)).distance_to(limit_coordinate)
+
+    return next_limit, distance_to_limit

iqpilot/iq_maps/road_data/road_daemon.py

@@ -0,0 +1,35 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import threading
+import traceback
+
+from openpilot.common.realtime import Ratekeeper, config_realtime_process
+from openpilot.iqpilot.iq_maps.road_data import debug_road_data
+from openpilot.iqpilot.iq_maps.road_data.iq_road_layer import IQRoadLayer
+
+ROAD_LAYER_HZ = 1
+ROAD_LAYER_CORES = [0, 1, 2, 3]
+
+
+def _log_thread_exception(args) -> None:
+  debug_road_data(f"IQ maps threading exception:\n{args}")
+  traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback)
+
+
+def run() -> None:
+  config_realtime_process(ROAD_LAYER_CORES, 5)
+  layer = IQRoadLayer()
+  rk = Ratekeeper(ROAD_LAYER_HZ, print_delay_threshold=None)
+  while True:
+    layer.step()
+    rk.keep_time()
+
+
+def main() -> None:
+  threading.excepthook = _log_thread_exception
+  run()
+
+
+if __name__ == "__main__":
+  main()

iqpilot/iq_maps/road_data/signal_bridge.py

@@ -0,0 +1,62 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from abc import abstractmethod, ABC
+
+import cereal.messaging as messaging
+from openpilot.common.params import Params
+from openpilot.common.constants import CV
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.iqpilot.navd.helpers import coordinate_from_param
+
+ROAD_SPEED_CEILING = V_CRUISE_UNSET * CV.KPH_TO_MS
+
+
+class RoadSignalBridge(ABC):
+  def __init__(self):
+    self.params = Params()
+
+    self.location_sub = messaging.SubMaster(['iqLiveLocation'])
+    self.output_pub = messaging.PubMaster(['iqLiveData'])
+
+    self.fix_ready = False
+    self.heading_deg = None
+    self.last_coordinate = coordinate_from_param("LastGPSPositionIQLoc", self.params)
+
+  @abstractmethod
+  def refresh_position(self) -> None:
+    pass
+
+  @abstractmethod
+  def read_current_limit(self) -> float:
+    pass
+
+  @abstractmethod
+  def read_upcoming_limit(self) -> tuple[float, float]:
+    pass
+
+  @abstractmethod
+  def read_current_road(self) -> str:
+    pass
+
+  def publish_snapshot(self) -> None:
+    active_limit = self.read_current_limit()
+    next_limit, next_limit_distance = self.read_upcoming_limit()
+
+    outbound = messaging.new_message('iqLiveData')
+    outbound.valid = self.location_sub['iqLiveLocation'].gpsHealthy
+    live_data = outbound.iqLiveData
+
+    live_data.speedLimitValid = bool(ROAD_SPEED_CEILING > active_limit > 0)
+    live_data.speedLimit = active_limit
+    live_data.speedLimitAheadValid = bool(ROAD_SPEED_CEILING > next_limit > 0)
+    live_data.speedLimitAhead = next_limit
+    live_data.speedLimitAheadDistance = next_limit_distance
+    live_data.roadName = self.read_current_road()
+
+    self.output_pub.send('iqLiveData', outbound)
+
+  def step(self) -> None:
+    self.location_sub.update(0)
+    self.refresh_position()
+    self.publish_snapshot()