IQ.Pilot Release Commit @ d58810f

iqpilot/iq_maps/road_data/__init__.py

@@ -0,0 +1,19 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from openpilot.common.swaglog import cloudlog
+
+LOOK_AHEAD_HORIZON_TIME = 15.  # s. Time horizon for look ahead of turn speed sections to provide on iqLiveData msg.
+_DEBUG = False
+_CLOUDLOG_DEBUG = False
+ROAD_NAME_TIMEOUT = 30  # secs
+R = 6373000.0  # approximate radius of earth in mts
+QUERY_RADIUS = 3000  # mts. Radius to use on OSM data queries.
+QUERY_RADIUS_OFFLINE = 2250  # mts. Radius to use on offline OSM data queries.
+
+
+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,39 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+# DISCLAIMER: This code is intended principally for development and debugging purposes.
+# Although it provides a standalone entry point to the program, users should refer
+# to the actual implementations for consumption. Usage outside of development scenarios
+# is not advised and could lead to unpredictable results.
+
+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
+
+
+def excepthook(args):
+  debug_road_data(f'IQ maps threading exception:\n{args}')
+  traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback)
+
+
+def live_map_data_iq_thread():
+  config_realtime_process([0, 1, 2, 3], 5)
+
+  live_map_iq = IQRoadLayer()
+  rk = Ratekeeper(1, print_delay_threshold=None)
+
+  while True:
+    live_map_iq.step()
+    rk.keep_time()
+
+
+def main():
+  threading.excepthook = excepthook
+  live_map_data_iq_thread()
+
+
+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()