From db2344cdd28c1f4fe2ab27b102df7c81d10a8bef Mon Sep 17 00:00:00 2001 From: Vivek Aithal Date: Fri, 13 Oct 2023 17:24:04 -0700 Subject: [PATCH] torqued: Refactor to share code with magd (#30238) * refactor to extract common functions and classes out * Add helpers.py to release files * refactor old-commit-hash: 9e24a11f17117caebc595165f59bfc0109af0287 --- release/files_common | 1 + selfdrive/locationd/helpers.py | 50 +++++++++++++++++++++++++++++++++ selfdrive/locationd/torqued.py | 51 +++++----------------------------- 3 files changed, 58 insertions(+), 44 deletions(-) create mode 100644 selfdrive/locationd/helpers.py diff --git a/release/files_common b/release/files_common index ef9f5266e1..7fa7ad50d2 100644 --- a/release/files_common +++ b/release/files_common @@ -246,6 +246,7 @@ selfdrive/locationd/models/gnss_helpers.py selfdrive/locationd/torqued.py selfdrive/locationd/calibrationd.py +selfdrive/locationd/helpers.py system/logcatd/.gitignore system/logcatd/SConscript diff --git a/selfdrive/locationd/helpers.py b/selfdrive/locationd/helpers.py new file mode 100644 index 0000000000..f41b72c703 --- /dev/null +++ b/selfdrive/locationd/helpers.py @@ -0,0 +1,50 @@ +import numpy as np +from typing import List, Optional, Tuple, Any + + +class NPQueue: + def __init__(self, maxlen: int, rowsize: int) -> None: + self.maxlen = maxlen + self.arr = np.empty((0, rowsize)) + + def __len__(self) -> int: + return len(self.arr) + + def append(self, pt: List[float]) -> None: + if len(self.arr) < self.maxlen: + self.arr = np.append(self.arr, [pt], axis=0) + else: + self.arr[:-1] = self.arr[1:] + self.arr[-1] = pt + + +class PointBuckets: + def __init__(self, x_bounds: List[Tuple[float, float]], min_points: List[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None: + self.x_bounds = x_bounds + self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds} + self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True)) + self.min_points_total = min_points_total + + def bucket_lengths(self) -> List[int]: + return [len(v) for v in self.buckets.values()] + + def __len__(self) -> int: + return sum(self.bucket_lengths()) + + def is_valid(self) -> bool: + individual_buckets_valid = all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)) + total_points_valid = self.__len__() >= self.min_points_total + return individual_buckets_valid and total_points_valid + + def add_point(self, x: float, y: float, bucket_val: float) -> None: + raise NotImplementedError + + def get_points(self, num_points: Optional[int] = None) -> Any: + points = np.vstack([x.arr for x in self.buckets.values()]) + if num_points is None: + return points + return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)] + + def load_points(self, points: List[List[float]]) -> None: + for point in points: + self.add_point(*point) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 0b624d5790..719651bcb1 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -12,6 +12,7 @@ from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.filter_simple import FirstOrderFilter from openpilot.system.swaglog import cloudlog from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY +from openpilot.selfdrive.locationd.helpers import PointBuckets HISTORY = 5 # secs POINTS_PER_BUCKET = 1500 @@ -43,55 +44,13 @@ def slope2rot(slope): return np.array([[cos, -sin], [sin, cos]]) -class NPQueue: - def __init__(self, maxlen, rowsize): - self.maxlen = maxlen - self.arr = np.empty((0, rowsize)) - - def __len__(self): - return len(self.arr) - - def append(self, pt): - if len(self.arr) < self.maxlen: - self.arr = np.append(self.arr, [pt], axis=0) - else: - self.arr[:-1] = self.arr[1:] - self.arr[-1] = pt - - -class PointBuckets: - def __init__(self, x_bounds, min_points, min_points_total): - self.x_bounds = x_bounds - self.buckets = {bounds: NPQueue(maxlen=POINTS_PER_BUCKET, rowsize=3) for bounds in x_bounds} - self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True)) - self.min_points_total = min_points_total - - def bucket_lengths(self): - return [len(v) for v in self.buckets.values()] - - def __len__(self): - return sum(self.bucket_lengths()) - - def is_valid(self): - return all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)) \ - and (self.__len__() >= self.min_points_total) - +class TorqueBuckets(PointBuckets): def add_point(self, x, y): for bound_min, bound_max in self.x_bounds: if (x >= bound_min) and (x < bound_max): self.buckets[(bound_min, bound_max)].append([x, 1.0, y]) break - def get_points(self, num_points=None): - points = np.vstack([x.arr for x in self.buckets.values()]) - if num_points is None: - return points - return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)] - - def load_points(self, points): - for x, y in points: - self.add_point(x, y) - class TorqueEstimator: def __init__(self, CP, decimated=False): @@ -175,7 +134,11 @@ class TorqueEstimator: self.resets += 1.0 self.decay = MIN_FILTER_DECAY self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len)) - self.filtered_points = PointBuckets(x_bounds=STEER_BUCKET_BOUNDS, min_points=self.min_bucket_points, min_points_total=self.min_points_total) + self.filtered_points = TorqueBuckets(x_bounds=STEER_BUCKET_BOUNDS, + min_points=self.min_bucket_points, + min_points_total=self.min_points_total, + points_per_bucket=POINTS_PER_BUCKET, + rowsize=3) def estimate_params(self): points = self.filtered_points.get_points(self.fit_points)