From 090551f2f9e195c80c72fe2b602d62c3884068d5 Mon Sep 17 00:00:00 2001 From: ATATC Date: Sun, 5 Jan 2025 13:05:57 +0800 Subject: [PATCH 1/5] Added LTM utils. (#462) --- leads/__init__.py | 1 + leads/_ltm/core | 1 + leads/ltm.py | 32 ++++++++++++++++++++++++++++++++ 3 files changed, 34 insertions(+) create mode 100644 leads/_ltm/core create mode 100644 leads/ltm.py diff --git a/leads/__init__.py b/leads/__init__.py index 8bc4e2f6..fe8f64a7 100644 --- a/leads/__init__.py +++ b/leads/__init__.py @@ -5,6 +5,7 @@ from leads.event import * from leads.leads import * from leads.logger import Level, L +from leads.ltm import * from leads.plugin import * from leads.registry import * from leads.sft import SFT, mark_device, read_device_marker diff --git a/leads/_ltm/core b/leads/_ltm/core new file mode 100644 index 00000000..9e26dfee --- /dev/null +++ b/leads/_ltm/core @@ -0,0 +1 @@ +{} \ No newline at end of file diff --git a/leads/ltm.py b/leads/ltm.py new file mode 100644 index 00000000..91f17748 --- /dev/null +++ b/leads/ltm.py @@ -0,0 +1,32 @@ +from json import loads as _loads, dumps as _dumps +from os.path import abspath as _abspath + +from leads.types import SupportedConfigValue as _SupportedConfigValue + +_ltm: dict[str, _SupportedConfigValue] = {} + + +def _load_ltm() -> None: + global _ltm + with open(f"{_abspath(__file__)[:-6]}_ltm/core", "r") as f: + ltm_content = f.read() + if not (ltm_content.startswith("{") and ltm_content.endswith("}")): + ltm_content = "{}" + _ltm = _loads(ltm_content) + + +def _sync_ltm() -> None: + with open(f"{_abspath(__file__)[:-6]}_ltm/core", "w") as f: + f.write(_dumps(_ltm)) + + +def ltm_get(key: str) -> _SupportedConfigValue: + return _ltm[key] + + +def ltm_set(key: str, value: _SupportedConfigValue) -> None: + _ltm[key] = value + _sync_ltm() + + +_load_ltm() From d58c494d0ae9796491a1e935c61aabb4960a95ab Mon Sep 17 00:00:00 2001 From: ATATC Date: Sun, 5 Jan 2025 13:07:55 +0800 Subject: [PATCH 2/5] Added config `use_ltm`. (#462) --- README.md | 1 + leads_vec/config.py | 1 + 2 files changed, 2 insertions(+) diff --git a/README.md b/README.md index 82d9adf2..f23614fc 100644 --- a/README.md +++ b/README.md @@ -607,6 +607,7 @@ Note that a purely empty file could cause an error. | `comm_stream_port` | `bool` | Port on which the streaming system runs on | Main, Remote | `16901` | | `data_dir` | `str` | Directory for the data recording system | Main, Remote | `"data"` | | `save_data` | `bool` | `True`: save data; `False`: discard data | Remote | `False` | +| `use_ltm` | `bool` | `True`: use long-term memory; `False`: short-term memory only | Main | `False` | For device-related implicit configurations, please see the [devices module](leads_vec/devices.py). diff --git a/leads_vec/config.py b/leads_vec/config.py index 7974e675..9c5f817c 100644 --- a/leads_vec/config.py +++ b/leads_vec/config.py @@ -9,4 +9,5 @@ def __init__(self, base: dict[str, _Any]) -> None: self.comm_stream: bool = False self.comm_stream_port: int = 16901 self.data_dir: str = "data" + self.use_ltm: bool = False super().__init__(base) From 702ee80942d747fbabee4b01a759ab763722a4df Mon Sep 17 00:00:00 2001 From: ATATC Date: Sun, 5 Jan 2025 13:25:10 +0800 Subject: [PATCH 3/5] Migrated `distance_between()` to `leads.data`. (#462) --- leads/data.py | 14 +++++++++++++- leads/data_persistence/analyzer/inference.py | 4 +++- leads/data_persistence/analyzer/utils.py | 15 --------------- 3 files changed, 16 insertions(+), 17 deletions(-) diff --git a/leads/data.py b/leads/data.py index 8f3a2749..40dc1db8 100644 --- a/leads/data.py +++ b/leads/data.py @@ -2,7 +2,7 @@ from time import time as _time from typing import override as _override, Any as _Any -from numpy import radians as _radians, degrees as _degrees, cos as _cos +from numpy import radians as _radians, degrees as _degrees, cos as _cos, sqrt as _sqrt class Serializable(object): @@ -161,6 +161,18 @@ def meters2dlon(meters: float, lat: float) -> float: return _degrees(meters / 6378137 / _cos(_radians(lat))) +def distance_between(lat_0: float, lon_0: float, lat: float, lon: float) -> float: + """ + Calculate the distance between two locations on the Earth. + :param lat_0: the latitude of the first location + :param lon_0: the longitude of the first location + :param lat: the latitude of the second location + :param lon: the longitude of the second location + :return: + """ + return _sqrt(dlon2meters(lon - lon_0, .5 * (lat_0 + lat)) ** 2 + dlat2meters(lat - lat_0) ** 2) + + def format_duration(duration: float) -> str: """ Wrap the duration into a formatted string. diff --git a/leads/data_persistence/analyzer/inference.py b/leads/data_persistence/analyzer/inference.py index e0dbb542..72dfeb6c 100644 --- a/leads/data_persistence/analyzer/inference.py +++ b/leads/data_persistence/analyzer/inference.py @@ -1,8 +1,9 @@ from abc import ABCMeta as _ABCMeta, abstractmethod as _abstractmethod from typing import Any as _Any, override as _override, Generator as _Generator, Literal as _Literal +from leads.data import distance_between from leads.data_persistence.analyzer.utils import time_invalid, speed_invalid, acceleration_invalid, \ - mileage_invalid, latitude_invalid, longitude_invalid, distance_between + mileage_invalid, latitude_invalid, longitude_invalid from leads.data_persistence.core import CSVDataset, DEFAULT_HEADER, VISUAL_HEADER_ONLY @@ -239,6 +240,7 @@ class VisualDataRealignmentByLatency(Inference): Offset the delay introduced by camera recording and video encoding so that the sensor data and the picture of the same frame match. """ + def __init__(self, *channels: _Literal["front", "left", "right", "rear"]) -> None: super().__init__((0, 1), VISUAL_HEADER_ONLY) self._channels: tuple[_Literal["front", "left", "right", "rear"], ...] = channels if channels else ( diff --git a/leads/data_persistence/analyzer/utils.py b/leads/data_persistence/analyzer/utils.py index a8e6fee9..46f9adbe 100644 --- a/leads/data_persistence/analyzer/utils.py +++ b/leads/data_persistence/analyzer/utils.py @@ -1,8 +1,5 @@ from typing import Any as _Any -from leads.data import dlat2meters, dlon2meters -from .._computational import sqrt as _sqrt - def time_invalid(o: _Any) -> bool: return not isinstance(o, int) @@ -30,15 +27,3 @@ def longitude_invalid(o: _Any) -> bool: def latency_invalid(o: _Any) -> bool: return not isinstance(o, int | float) - - -def distance_between(lat_0: float, lon_0: float, lat: float, lon: float) -> float: - """ - Calculate the distance between two locations on the Earth. - :param lat_0: the latitude of the first location - :param lon_0: the longitude of the first location - :param lat: the latitude of the second location - :param lon: the longitude of the second location - :return: - """ - return _sqrt(dlon2meters(lon - lon_0, .5 * (lat_0 + lat)) ** 2 + dlat2meters(lat - lat_0) ** 2) From 60f216730087bfc8a90f18daa649259fb76fcfde Mon Sep 17 00:00:00 2001 From: ATATC Date: Sun, 5 Jan 2025 13:28:23 +0800 Subject: [PATCH 4/5] Supported odometer under GPS-only mode. (#462) --- leads_vec/devices.py | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/leads_vec/devices.py b/leads_vec/devices.py index 1d16f4e5..40e1a1b0 100644 --- a/leads_vec/devices.py +++ b/leads_vec/devices.py @@ -4,7 +4,8 @@ Controller, CENTER_REAR_WHEEL_SPEED_SENSOR, require_config, mark_device, ODOMETER, GPS_RECEIVER, \ ConcurrentOdometer, LEFT_INDICATOR, RIGHT_INDICATOR, VOLTAGE_SENSOR, DataContainer, has_device, \ FRONT_VIEW_CAMERA, LEFT_VIEW_CAMERA, RIGHT_VIEW_CAMERA, REAR_VIEW_CAMERA, VisualDataContainer, BRAKE_INDICATOR, \ - SFT, read_device_marker, has_controller, POWER_CONTROLLER, WHEEL_SPEED_CONTROLLER, ACCELEROMETER, require_context + SFT, read_device_marker, has_controller, POWER_CONTROLLER, WHEEL_SPEED_CONTROLLER, ACCELEROMETER, require_context, \ + ltm_get, ltm_set, distance_between from leads_arduino import ArduinoMicro, WheelSpeedSensor, VoltageSensor, Accelerometer, Acceleration from leads_comm_serial import SOBD from leads_gpio import NMEAGPSReceiver, LEDGroup, LED, LEDGroupCommand, LEDCommand, Entire, Transition, Button, \ @@ -54,7 +55,6 @@ def initialize(self, *parent_tags: str) -> None: @override def read(self) -> DataContainer: general = { - "mileage": self.device(ODOMETER).read(), "gps_valid": (gps := self.device(GPS_RECEIVER).read())[0], "gps_ground_speed": gps[1], "latitude": gps[2], @@ -62,8 +62,12 @@ def read(self) -> DataContainer: **self.device(POWER_CONTROLLER).read() } wsc = self.device(WHEEL_SPEED_CONTROLLER).read() + odometer = self.device(ODOMETER) if GPS_ONLY: wsc["speed"] = gps[1] + prev = require_context().data() + odometer.write(odometer.read() + distance_between(prev.latitude, prev.longitude, gps[2], gps[3])) + general["mileage"] = odometer.read() visual = {} if has_device(FRONT_VIEW_CAMERA): cam = get_camera(FRONT_VIEW_CAMERA, Base64Camera) @@ -138,10 +142,17 @@ class AverageOdometer(ConcurrentOdometer): def initialize(self, *parent_tags: str) -> None: mark_device(self, "WSC") super().initialize(*parent_tags) + if config.use_ltm: + self.write(ltm_get("mileage")) + + @override + def write(self, payload: float) -> None: + super().write(payload) + ltm_set("mileage", payload) @override def read(self) -> float: - return super().read() / 3 + return super().read() if GPS_ONLY else super().read() / 3 @device(*(((LEFT_FRONT_WHEEL_SPEED_SENSOR, RIGHT_FRONT_WHEEL_SPEED_SENSOR, CENTER_REAR_WHEEL_SPEED_SENSOR), From a326da9393fb903eb4f8b7effe9746d6dc84ddce Mon Sep 17 00:00:00 2001 From: ATATC Date: Thu, 16 Jan 2025 10:33:14 -0500 Subject: [PATCH 5/5] Merged submodules. --- readthedocs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/readthedocs b/readthedocs index 38ec8bde..5c28291d 160000 --- a/readthedocs +++ b/readthedocs @@ -1 +1 @@ -Subproject commit 38ec8bde008515782711d940a403c7734d2e0d1c +Subproject commit 5c28291d299953eb9bda0232af34b82f24df6833