diff --git a/README.md b/README.md index 82d9adf..f23614f 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/__init__.py b/leads/__init__.py index 8bc4e2f..fe8f64a 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 0000000..9e26dfe --- /dev/null +++ b/leads/_ltm/core @@ -0,0 +1 @@ +{} \ No newline at end of file diff --git a/leads/data.py b/leads/data.py index 8f3a274..40dc1db 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 e0dbb54..72dfeb6 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 a8e6fee..46f9adb 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) diff --git a/leads/ltm.py b/leads/ltm.py new file mode 100644 index 0000000..91f1774 --- /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() diff --git a/leads_vec/config.py b/leads_vec/config.py index 7974e67..9c5f817 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) diff --git a/leads_vec/devices.py b/leads_vec/devices.py index 1d16f4e..40e1a1b 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),