Skip to content
This repository has been archived by the owner on May 1, 2024. It is now read-only.

Commit

Permalink
Adopted latest torqued
Browse files Browse the repository at this point in the history
  • Loading branch information
eFiniLan committed Dec 20, 2023
1 parent 6c7b7f6 commit 931cc12
Show file tree
Hide file tree
Showing 5 changed files with 92 additions and 72 deletions.
1 change: 1 addition & 0 deletions common/params.cc
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"CarParamsCache", CLEAR_ON_MANAGER_START},
{"CarParamsPersistent", PERSISTENT},
{"CarParamsPrevRoute", PERSISTENT},
{"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"CompletedTrainingVersion", PERSISTENT},
{"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
Expand Down
5 changes: 5 additions & 0 deletions selfdrive/controls/controlsd.py
Original file line number Diff line number Diff line change
Expand Up @@ -127,6 +127,11 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None):
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
self.CP.safetyConfigs = [safety_config]

# Write previous route's CarParams
prev_cp = self.params.get("CarParamsPersistent")
if prev_cp is not None:
self.params.put("CarParamsPrevRoute", prev_cp)

# Write CarParams for radard
cp_bytes = self.CP.to_bytes()
self.params.put("CarParams", cp_bytes)
Expand Down
64 changes: 64 additions & 0 deletions selfdrive/locationd/helpers.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
import numpy as np
from typing import List, Optional, Tuple, Any

from cereal import log


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)


class ParameterEstimator:
""" Base class for parameter estimators """
def reset(self) -> None:
raise NotImplementedError

def handle_log(self, t: int, which: str, msg: log.Event) -> None:
raise NotImplementedError

def get_msg(self, valid: bool, with_points: bool) -> log.Event:
raise NotImplementedError
87 changes: 17 additions & 70 deletions selfdrive/locationd/torqued.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,4 @@
#!/usr/bin/env python3
import os
import sys
import signal
import numpy as np
from collections import deque, defaultdict

Expand All @@ -12,6 +9,7 @@
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, ParameterEstimator

HISTORY = 5 # secs
POINTS_PER_BUCKET = 1500
Expand Down Expand Up @@ -43,56 +41,15 @@ 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))
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())) 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:
class TorqueEstimator(ParameterEstimator):
def __init__(self, CP, decimated=False):
self.hist_len = int(HISTORY / DT_MDL)
self.lag = CP.steerActuatorDelay + .2 # from controlsd
Expand All @@ -113,7 +70,7 @@ def __init__(self, CP, decimated=False):
self.offline_friction = 0.0
self.offline_latAccelFactor = 0.0
self.resets = 0.0
self.use_params = CP.carName in ALLOWED_CARS
self.use_params = CP.carName in ALLOWED_CARS and CP.lateralTuning.which() == 'torque'

if CP.lateralTuning.which() == 'torque':
self.offline_friction = CP.lateralTuning.torque.friction
Expand All @@ -135,7 +92,7 @@ def __init__(self, CP, decimated=False):

# try to restore cached params
params = Params()
params_cache = params.get("LiveTorqueCarParams")
params_cache = params.get("CarParamsPrevRoute")
torque_cache = params.get("LiveTorqueParameters")
if params_cache is not None and torque_cache is not None:
try:
Expand All @@ -158,7 +115,6 @@ def __init__(self, CP, decimated=False):
cloudlog.info("restored torque params from cache")
except Exception:
cloudlog.exception("failed to restore cached torque params")
params.remove("LiveTorqueCarParams")
params.remove("LiveTorqueParameters")

self.filtered_params = {}
Expand All @@ -176,7 +132,11 @@ def reset(self):
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)
Expand Down Expand Up @@ -256,34 +216,17 @@ def get_msg(self, valid=True, with_points=False):
return msg


def main(sm=None, pm=None):
def main():
config_realtime_process([0, 1, 2, 3], 5)

if sm is None:
sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll=['liveLocationKalman'])

if pm is None:
pm = messaging.PubMaster(['liveTorqueParameters'])
pm = messaging.PubMaster(['liveTorqueParameters'])
sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll=['liveLocationKalman'])

params = Params()
CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
# with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP:
estimator = TorqueEstimator(CP)

def cache_params(sig, frame):
signal.signal(sig, signal.SIG_DFL)
cloudlog.warning("caching torque params")

params = Params()
params.put("LiveTorqueCarParams", CP.as_builder().to_bytes())

msg = estimator.get_msg(with_points=True)
params.put("LiveTorqueParameters", msg.to_bytes())

sys.exit(0)
if "REPLAY" not in os.environ:
signal.signal(signal.SIGINT, cache_params)

while True:
sm.update()
if sm.all_checks():
Expand All @@ -296,6 +239,10 @@ def cache_params(sig, frame):
if sm.frame % 5 == 0:
pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))

# Cache points every 60 seconds while onroad
if sm.frame % 240 == 0:
msg = estimator.get_msg(valid=sm.all_checks(), with_points=True)
params.put_nonblocking("LiveTorqueParameters", msg.to_bytes())

if __name__ == "__main__":
main()
7 changes: 5 additions & 2 deletions selfdrive/test/process_replay/process_replay.py
Original file line number Diff line number Diff line change
Expand Up @@ -589,10 +589,13 @@ def get_custom_params_from_lr(lr: Union[LogReader, List[capnp._DynamicStructRead
assert initial_state in ["first", "last"]
msg_index = 0 if initial_state == "first" else -1

assert len(car_params) > 0, "carParams required for initial state of liveParameters and liveTorqueCarParams"
assert len(car_params) > 0, "carParams required for initial state of liveParameters and CarParamsPrevRoute"
CP = car_params[msg_index].carParams

custom_params = {}
custom_params = {
"CarParamsPrevRoute": CP.as_builder().to_bytes()
}

if len(live_calibration) > 0:
custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes()
if len(live_parameters) > 0:
Expand Down

0 comments on commit 931cc12

Please sign in to comment.