IQ.Pilot Release Commit @ df6f990

iqpilot/mapd/live_map_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 get_debug(msg, log_to_cloud=True):
+  if _CLOUDLOG_DEBUG and log_to_cloud:
+    cloudlog.debug(msg)
+  if _DEBUG:
+    print(msg)

iqpilot/mapd/live_map_data/base_map_data.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
+
+MAX_SPEED_LIMIT = V_CRUISE_UNSET * CV.KPH_TO_MS
+
+
+class BaseMapData(ABC):
+  def __init__(self):
+    self.params = Params()
+
+    self.sm = messaging.SubMaster(['liveLocationKalman'])
+    self.pm = messaging.PubMaster(['iqLiveData'])
+
+    self.localizer_valid = False
+    self.last_bearing = None
+    self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params)
+
+  @abstractmethod
+  def update_location(self) -> None:
+    pass
+
+  @abstractmethod
+  def get_current_speed_limit(self) -> float:
+    pass
+
+  @abstractmethod
+  def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
+    pass
+
+  @abstractmethod
+  def get_current_road_name(self) -> str:
+    pass
+
+  def publish(self) -> None:
+    speed_limit = self.get_current_speed_limit()
+    next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
+
+    mapd_send = messaging.new_message('iqLiveData')
+    mapd_send.valid = self.sm['liveLocationKalman'].gpsOK
+    live_map_data = mapd_send.iqLiveData
+
+    live_map_data.speedLimitValid = bool(MAX_SPEED_LIMIT > speed_limit > 0)
+    live_map_data.speedLimit = speed_limit
+    live_map_data.speedLimitAheadValid = bool(MAX_SPEED_LIMIT > next_speed_limit > 0)
+    live_map_data.speedLimitAhead = next_speed_limit
+    live_map_data.speedLimitAheadDistance = next_speed_limit_distance
+    live_map_data.roadName = self.get_current_road_name()
+
+    self.pm.send('iqLiveData', mapd_send)
+
+  def tick(self) -> None:
+    self.sm.update(0)
+    self.update_location()
+    self.publish()

iqpilot/mapd/live_map_data/debug.py

@@ -0,0 +1,53 @@
+"""
+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 cereal import messaging
+from openpilot.common.gps import get_gps_location_service
+from openpilot.common.params import Params
+from openpilot.common.realtime import config_realtime_process
+from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller.common import Policy
+from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver
+from openpilot.iqpilot.mapd.live_map_data import get_debug
+
+
+def excepthook(args):
+  get_debug(f'MapD: 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)
+
+  params = Params()
+  gps_location_service = get_gps_location_service(params)
+
+  while True:
+    live_map_data_iq_thread_debug(gps_location_service)
+
+
+def live_map_data_iq_thread_debug(gps_location_service):
+  _sub_master = messaging.SubMaster(['carState', 'livePose', 'iqLiveData', 'iqPlan', 'iqCarState', gps_location_service])
+  _sub_master.update()
+
+  v_ego = _sub_master['carState'].vEgo
+  _resolver = SpeedLimitResolver()
+  _resolver.policy = Policy.car_state_priority
+  _resolver.update(v_ego, _sub_master)
+  print(_resolver.speed_limit, _resolver.distance, _resolver.source)
+
+
+def main():
+  threading.excepthook = excepthook
+  live_map_data_iq_thread()
+
+
+if __name__ == "__main__":
+  main()

iqpilot/mapd/live_map_data/osm_map_data.py

@@ -0,0 +1,58 @@
+"""
+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 log
+from openpilot.common.params import Params
+from openpilot.iqpilot.mapd.live_map_data.base_map_data import BaseMapData
+from openpilot.iqpilot.navd.helpers import Coordinate
+
+
+class OsmMapData(BaseMapData):
+  def __init__(self):
+    super().__init__()
+    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
+
+  def update_location(self) -> None:
+    location = self.sm['liveLocationKalman']
+    self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
+
+    if self.localizer_valid:
+      self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
+      self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
+
+    if self.last_position is None:
+      return
+
+    params = {
+      "latitude": self.last_position.latitude,
+      "longitude": self.last_position.longitude,
+    }
+
+    if self.last_bearing is not None:
+      params['bearing'] = self.last_bearing
+
+    self.mem_params.put("LastGPSPosition", json.dumps(params))
+
+  def get_current_speed_limit(self) -> float:
+    return float(self.mem_params.get("MapSpeedLimit") or 0.0)
+
+  def get_current_road_name(self) -> str:
+    return str(self.mem_params.get("RoadName") or "")
+
+  def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
+    next_speed_limit_section_str = self.mem_params.get("NextMapSpeedLimit")
+    next_speed_limit_section = next_speed_limit_section_str if next_speed_limit_section_str else {}
+    next_speed_limit = next_speed_limit_section.get('speedlimit', 0.0)
+    next_speed_limit_latitude = next_speed_limit_section.get('latitude')
+    next_speed_limit_longitude = next_speed_limit_section.get('longitude')
+    next_speed_limit_distance = 0.0
+
+    if next_speed_limit_latitude and next_speed_limit_longitude:
+      next_speed_limit_coordinates = Coordinate(next_speed_limit_latitude, next_speed_limit_longitude)
+      next_speed_limit_distance = (self.last_position or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates)
+
+    return next_speed_limit, next_speed_limit_distance

iqpilot/mapd/live_map_data/standalone.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.mapd.live_map_data import get_debug
+from openpilot.iqpilot.mapd.live_map_data.osm_map_data import OsmMapData
+
+
+def excepthook(args):
+  get_debug(f'MapD: 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 = OsmMapData()
+  rk = Ratekeeper(1, print_delay_threshold=None)
+
+  while True:
+    live_map_iq.tick()
+    rk.keep_time()
+
+
+def main():
+  threading.excepthook = excepthook
+  live_map_data_iq_thread()
+
+
+if __name__ == "__main__":
+  main()