diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5a6be5ea28..f47bb9eb81 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -257,21 +257,7 @@ struct LiveMapDataSP @0xf416ec09499d9d19 { speedLimitAheadValid @2 :Bool; speedLimitAhead @3 :Float32; speedLimitAheadDistance @4 :Float32; - turnSpeedLimitValid @5 :Bool; - turnSpeedLimit @6 :Float32; - turnSpeedLimitEndDistance @7 :Float32; - turnSpeedLimitSign @8 :Int16; - turnSpeedLimitsAhead @9 :List(Float32); - turnSpeedLimitsAheadDistances @10 :List(Float32); - turnSpeedLimitsAheadSigns @11 :List(Int16); - lastGpsTimestamp @12 :Int64; # Milliseconds since January 1, 1970. - currentRoadName @13 :Text; - lastGpsLatitude @14 :Float64; - lastGpsLongitude @15 :Float64; - lastGpsSpeed @16 :Float32; - lastGpsBearingDeg @17 :Float32; - lastGpsAccuracy @18 :Float32; - lastGpsBearingAccuracyDeg @19 :Float32; + roadName @5 :Text; } struct CustomReserved9 @0xa1680744031fdb2d { diff --git a/sunnypilot/mapd/live_map_data/__init__.py b/sunnypilot/mapd/live_map_data/__init__.py index 77f21e346f..557ad06515 100644 --- a/sunnypilot/mapd/live_map_data/__init__.py +++ b/sunnypilot/mapd/live_map_data/__init__.py @@ -1,4 +1,9 @@ -from cereal import custom +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" from openpilot.common.swaglog import cloudlog LOOK_AHEAD_HORIZON_TIME = 15. # s. Time horizon for look ahead of turn speed sections to provide on liveMapDataSP msg. diff --git a/sunnypilot/mapd/live_map_data/base_map_data.py b/sunnypilot/mapd/live_map_data/base_map_data.py index c95b51c05f..60071238ba 100644 --- a/sunnypilot/mapd/live_map_data/base_map_data.py +++ b/sunnypilot/mapd/live_map_data/base_map_data.py @@ -1,10 +1,16 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" import time from abc import abstractmethod, ABC from cereal import messaging from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params -from openpilot.sunnypilot.navd.helpers import Coordinate +from openpilot.sunnypilot.navd.helpers import Coordinate, coordinate_from_param class BaseMapData(ABC): @@ -16,8 +22,15 @@ class BaseMapData(ABC): self._sm = messaging.SubMaster(['livePose', 'carControl'] + self._gps_packets) self._pm = messaging.PubMaster(['liveMapDataSP']) + self.gps_location_service = get_gps_location_service(self.params) + self.sm = messaging.SubMaster(['livePose', 'carControl'] + [self.gps_location_service]) + self.pm = messaging.PubMaster(['liveMapDataSP']) + + self.last_position = coordinate_from_param("LastGPSPosition", self.params) + self.last_altitude = None + @abstractmethod - def update_location(self, current_location: Coordinate | None) -> None: + def update_location(self) -> None: pass @abstractmethod @@ -25,45 +38,31 @@ class BaseMapData(ABC): pass @abstractmethod - def get_next_speed_limit_and_distance(self, current_location: Coordinate | None) -> tuple[float, float]: + def get_next_speed_limit_and_distance(self) -> tuple[float, float]: pass @abstractmethod def get_current_road_name(self) -> str: pass - def _is_gps_data_valid(self) -> bool: - all_sock_alive = self._sm.all_alive(service_list=[self._gps_location_service]) - all_sock_valid = self._sm.all_valid(service_list=[self._gps_location_service]) - return bool(all_sock_alive and all_sock_valid) - - def get_current_location(self) -> Coordinate | None: - if not self._sm.updated[self._gps_location_service] or not self._sm.valid[self._gps_location_service]: - return None - - _last_gps = self._sm[self._gps_location_service] + def get_current_location(self) -> None: + gps = self.sm[self.gps_location_service] # ignore the message if the fix is invalid - gps_ok = self._sm.updated[self._gps_location_service] or (time.monotonic() - self._sm.logMonoTime[self._gps_location_service] / 1e9) > 2.0 - if not gps_ok and self._sm['livePose'].inputsOK: + gps_ok = self.sm.updated[self.gps_location_service] or (time.monotonic() - self.sm.logMonoTime[self.gps_location_service] / 1e9) > 2.0 + if not gps_ok and self.sm['livePose'].inputsOK: return None - result = Coordinate(_last_gps.latitude, _last_gps.longitude) - result.annotations['unixTimestampMillis'] = _last_gps.unixTimestampMillis - result.annotations['speed'] = _last_gps.speed - result.annotations['bearingDeg'] = _last_gps.bearingDeg - result.annotations['accuracy'] = _last_gps.horizontalAccuracy - result.annotations['bearingAccuracyDeg'] = _last_gps.bearingAccuracyDeg - - return result + # livePose has these data, but aren't on cereal + self.last_position = Coordinate(gps.latitude, gps.longitude) + self.last_altitude = gps.altitude def publish(self) -> None: speed_limit = self.get_current_speed_limit() - current_road_name = self.get_current_road_name() - next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance(self._last_gps) + next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance() mapd_sp_send = messaging.new_message('liveMapDataSP') - mapd_sp_send.valid = self._is_gps_data_valid() + mapd_sp_send.valid = self.sm.all_checks(service_list=[self.gps_location_service, 'livePose']) live_map_data = mapd_sp_send.liveMapDataSP if self._last_gps: @@ -80,12 +79,12 @@ class BaseMapData(ABC): live_map_data.speedLimitAheadValid = bool(next_speed_limit > 0) live_map_data.speedLimitAhead = next_speed_limit live_map_data.speedLimitAheadDistance = next_speed_limit_distance - live_map_data.currentRoadName = current_road_name + live_map_data.roadName = self.get_current_road_name() - self._pm.send('liveMapDataSP', mapd_sp_send) + self.pm.send('liveMapDataSP', mapd_sp_send) def tick(self) -> None: - self._sm.update() - self._last_gps = self.get_current_location() - self.update_location(self._last_gps) + self.sm.update() + self.get_current_location() + self.update_location() self.publish() diff --git a/sunnypilot/mapd/live_map_data/debug.py b/sunnypilot/mapd/live_map_data/debug.py index 8209e7b9c0..da9f4d7771 100644 --- a/sunnypilot/mapd/live_map_data/debug.py +++ b/sunnypilot/mapd/live_map_data/debug.py @@ -1,3 +1,9 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" # 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 diff --git a/sunnypilot/mapd/live_map_data/osm_map_data.py b/sunnypilot/mapd/live_map_data/osm_map_data.py index 8ea85b792b..b9449669d8 100644 --- a/sunnypilot/mapd/live_map_data/osm_map_data.py +++ b/sunnypilot/mapd/live_map_data/osm_map_data.py @@ -1,9 +1,16 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" import json import platform from openpilot.common.params import Params from openpilot.sunnypilot.navd.helpers import Coordinate from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData +from openpilot.sunnypilot.navd.helpers import Coordinate class OsmMapData(BaseMapData): @@ -12,16 +19,17 @@ class OsmMapData(BaseMapData): self.params = Params() self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params - def update_location(self, current_location: Coordinate | None) -> None: - if not current_location: + def update_location(self) -> None: + if self.last_position is None or self.last_altitude is None: return - last_gps_position_for_osm = { - "latitude": current_location.latitude, - "longitude": current_location.longitude, - "bearing": current_location.annotations.get("bearingDeg", 0) + params = { + "latitude": self.last_position.latitude, + "longitude": self.last_position.longitude, + "altitude": self.last_altitude, } - self.mem_params.put("LastGPSPosition", json.dumps(last_gps_position_for_osm)) + + self.mem_params.put("LastGPSPosition", json.dumps(params)) def get_current_speed_limit(self) -> float: return float(self.mem_params.get("MapSpeedLimit", encoding='utf8') or 0.0) @@ -29,7 +37,7 @@ class OsmMapData(BaseMapData): def get_current_road_name(self) -> str: return self.mem_params.get("RoadName", encoding='utf8') or "" - def get_next_speed_limit_and_distance(self, current_location: Coordinate | None) -> tuple[float, float]: + def get_next_speed_limit_and_distance(self) -> tuple[float, float]: next_speed_limit_section_str = self.mem_params.get("NextMapSpeedLimit", encoding='utf8') next_speed_limit_section = json.loads(next_speed_limit_section_str) if next_speed_limit_section_str else {} next_speed_limit = next_speed_limit_section.get('speedlimit', 0.0) @@ -39,6 +47,6 @@ class OsmMapData(BaseMapData): 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 = (current_location or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates) + 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 diff --git a/sunnypilot/mapd/live_map_data/standalone.py b/sunnypilot/mapd/live_map_data/standalone.py index a9347a2b29..f73ecc7724 100644 --- a/sunnypilot/mapd/live_map_data/standalone.py +++ b/sunnypilot/mapd/live_map_data/standalone.py @@ -1,3 +1,9 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" # 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 diff --git a/sunnypilot/mapd/mapd_installer.py b/sunnypilot/mapd/mapd_installer.py index c2d00d7ecf..ef7a3389e3 100755 --- a/sunnypilot/mapd/mapd_installer.py +++ b/sunnypilot/mapd/mapd_installer.py @@ -1,3 +1,9 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" #!/usr/bin/env python3 import logging import os @@ -81,10 +87,10 @@ class MapdInstallManager: Params().put("MapdVersion", version) @staticmethod - def get_installed_version() -> str | None: - return Params().get("MapdVersion", encoding="utf-8") + def get_installed_version() -> str: + return Params().get("MapdVersion", encoding="utf-8") or "" - def wait_for_internet_connection(self, return_on_failure: bool = False) -> bool | None: + def wait_for_internet_connection(self, return_on_failure: bool = False) -> bool: max_retries = 10 for retries in range(max_retries + 1): self._spinner.update(f"Waiting for internet connection... [{retries}/{max_retries}]") @@ -97,21 +103,23 @@ class MapdInstallManager: if return_on_failure and retries == max_retries: return False - def non_prebuilt_install(self) -> bool | None: + return False + + def non_prebuilt_install(self) -> None: sm = messaging.SubMaster(['deviceState']) metered = sm['deviceState'].networkMetered if metered: self._spinner.update("Can't proceed with mapd install since network is metered!") time.sleep(5) - return False + return try: self.ensure_directories_exist() if not self.download_needed(): self._spinner.update("Mapd is good!") time.sleep(0.1) - return True + return if self.wait_for_internet_connection(return_on_failure=True): self._spinner.update(f"Downloading pfeiferj's mapd [{install_manager.get_installed_version()}] => [{VERSION}].") diff --git a/sunnypilot/mapd/mapd_manager.py b/sunnypilot/mapd/mapd_manager.py index 7fa3509600..e5f43f5d4b 100644 --- a/sunnypilot/mapd/mapd_manager.py +++ b/sunnypilot/mapd/mapd_manager.py @@ -1,3 +1,9 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" import json import time import platform