From 0d7458bdb393e9b614c63a900d0625853d542c40 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 13 Oct 2021 01:46:28 -0500 Subject: [PATCH 01/30] adjustable following distance? --- .../lib/longitudinal_mpc_lib/long_mpc.py | 44 ++++++++++++++----- 1 file changed, 32 insertions(+), 12 deletions(-) mode change 100644 => 100755 selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py old mode 100644 new mode 100755 index df6d8e0fb64316..48f994054d5d7f --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -51,11 +51,11 @@ MAX_BRAKE = 9.81 -def get_stopped_equivalence_factor(v_lead): - return T_REACT * v_lead + (v_lead*v_lead) / (2 * MAX_BRAKE) +def get_stopped_equivalence_factor(v_lead, t_react=T_REACT): + return t_react * v_lead + (v_lead*v_lead) / (2 * MAX_BRAKE) -def get_safe_obstacle_distance(v_ego): - return 2 * T_REACT * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0 +def get_safe_obstacle_distance(v_ego, t_react=T_REACT): + return 2 * t_react * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0 def desired_follow_distance(v_ego, v_lead): return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead) @@ -193,6 +193,7 @@ def __init__(self, e2e=False): self.accel_limit_arr[:,0] = -1.2 self.accel_limit_arr[:,1] = 1.2 self.source = SOURCES[2] + self.desired_TR = 1.8 def reset(self): self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR) @@ -285,7 +286,9 @@ def process_lead(self, lead): v_lead = clip(v_lead, 0.0, 1e8) a_lead = clip(a_lead, -10., 5.) lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) - return lead_xv + # TODO: experiment with setting accel to 0 so lead's accel is factored in (only distance is considered now) + lead_xv_ideal = self.extrapolate_lead(get_stopped_equivalence_factor(v_lead, self.desired_TR), v_lead, a_lead, a_lead_tau) + return lead_xv, lead_xv_ideal def set_accel_limits(self, min_a, max_a): self.cruise_min_a = min_a @@ -295,8 +298,8 @@ def update(self, carstate, radarstate, v_cruise): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status - lead_xv_0 = self.process_lead(radarstate.leadOne) - lead_xv_1 = self.process_lead(radarstate.leadTwo) + lead_xv_0, lead_xv_ideal_0 = self.process_lead(radarstate.leadOne) + lead_xv_1, lead_xv_ideal_1 = self.process_lead(radarstate.leadTwo) # set accel limits in params self.params[:,0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL]) @@ -305,8 +308,25 @@ def update(self, carstate, radarstate, v_cruise): # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. - lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) - lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + + # This may all be wrong, just some tests to enable a hacky adjustable following distance + + # Calculates difference in ideal distance vs actual distance to a TR diff in seconds + lead_react_diff_0 = (lead_xv_ideal_0[0][0] - lead_xv_0[0][0]) / lead_xv_0[1][0] + lead_react_diff_1 = (lead_xv_ideal_1[0][0] - lead_xv_1[0][0]) / lead_xv_1[1][0] + + # TODO: some tuning to be had here + # basically the lower the desired TR the more we want to stick near it + # so we come up with a cost to multiply difference in actual TR vs. desired TR by + # and thus the less we change the stopped factor below + react_diff_cost = np.interp(self.desired_TR, [0.9, 1.8, 2.7], [2, 1, 0.5]) + react_diff_mult_0 = np.interp(abs(lead_react_diff_0) * react_diff_cost, [0, 0.9], [1, 0.1]) + react_diff_mult_1 = np.interp(abs(lead_react_diff_1) * react_diff_cost, [0, 0.9], [1, 0.1]) + + t_react_compensation_0 = T_REACT + (T_REACT - self.desired_TR) * react_diff_mult_0 + t_react_compensation_1 = T_REACT + (T_REACT - self.desired_TR) * react_diff_mult_1 + lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], t_react_compensation_0) + lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], t_react_compensation_1) # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. @@ -367,6 +387,6 @@ def run(self): self.reset() -if __name__ == "__main__": - ocp = gen_long_mpc_solver() - AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False) +# if __name__ == "__main__": +# ocp = gen_long_mpc_solver() +# AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False) From b4aad1f18ef903598e0371ecece77e35699fea95 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 13 Oct 2021 02:17:14 -0500 Subject: [PATCH 02/30] distance cost doesn't work, try removing it (may not brake enough for leads!!) --- .../lib/longitudinal_mpc_lib/long_mpc.py | 26 +++++++++++-------- selfdrive/controls/tests/test_alerts.py | 0 .../controls/tests/test_following_distance.py | 3 ++- .../test/longitudinal_maneuvers/plant.py | 6 ++--- 4 files changed, 20 insertions(+), 15 deletions(-) mode change 100755 => 100644 selfdrive/controls/tests/test_alerts.py diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 48f994054d5d7f..9a11158f4b10cf 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -57,8 +57,8 @@ def get_stopped_equivalence_factor(v_lead, t_react=T_REACT): def get_safe_obstacle_distance(v_ego, t_react=T_REACT): return 2 * t_react * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0 -def desired_follow_distance(v_ego, v_lead): - return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead) +def desired_follow_distance(v_ego, v_lead, t_react=T_REACT): + return get_safe_obstacle_distance(v_ego, t_react) - get_stopped_equivalence_factor(v_lead, t_react) def gen_long_model(): @@ -193,7 +193,7 @@ def __init__(self, e2e=False): self.accel_limit_arr[:,0] = -1.2 self.accel_limit_arr[:,1] = 1.2 self.source = SOURCES[2] - self.desired_TR = 1.8 + self.desired_TR = 0.9 def reset(self): self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR) @@ -312,19 +312,23 @@ def update(self, carstate, radarstate, v_cruise): # This may all be wrong, just some tests to enable a hacky adjustable following distance # Calculates difference in ideal distance vs actual distance to a TR diff in seconds - lead_react_diff_0 = (lead_xv_ideal_0[0][0] - lead_xv_0[0][0]) / lead_xv_0[1][0] - lead_react_diff_1 = (lead_xv_ideal_1[0][0] - lead_xv_1[0][0]) / lead_xv_1[1][0] + lead_react_diff_0 = (lead_xv_ideal_0[:,0] - lead_xv_0[:,0]) / lead_xv_0[:,1] + lead_react_diff_1 = (lead_xv_ideal_1[:,0] - lead_xv_1[:,0]) / lead_xv_1[:,1] # TODO: some tuning to be had here # basically the lower the desired TR the more we want to stick near it # so we come up with a cost to multiply difference in actual TR vs. desired TR by # and thus the less we change the stopped factor below - react_diff_cost = np.interp(self.desired_TR, [0.9, 1.8, 2.7], [2, 1, 0.5]) + # react_diff_cost = np.interp(self.desired_TR, [0.9, 1.8, 2.7], [2, 1, 0.5]) + react_diff_cost = 1. + # print(lead_react_diff_0[0]) + lead_react_diff_0 = lead_react_diff_0[0] + lead_react_diff_1 = lead_react_diff_1[0] react_diff_mult_0 = np.interp(abs(lead_react_diff_0) * react_diff_cost, [0, 0.9], [1, 0.1]) react_diff_mult_1 = np.interp(abs(lead_react_diff_1) * react_diff_cost, [0, 0.9], [1, 0.1]) - t_react_compensation_0 = T_REACT + (T_REACT - self.desired_TR) * react_diff_mult_0 - t_react_compensation_1 = T_REACT + (T_REACT - self.desired_TR) * react_diff_mult_1 + t_react_compensation_0 = T_REACT + (T_REACT - self.desired_TR) + t_react_compensation_1 = T_REACT + (T_REACT - self.desired_TR) lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], t_react_compensation_0) lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], t_react_compensation_1) @@ -387,6 +391,6 @@ def run(self): self.reset() -# if __name__ == "__main__": -# ocp = gen_long_mpc_solver() -# AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False) +if __name__ == "__main__": + ocp = gen_long_mpc_solver() + AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False) diff --git a/selfdrive/controls/tests/test_alerts.py b/selfdrive/controls/tests/test_alerts.py old mode 100755 new mode 100644 diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 5b3fa6522841d9..58f444b94fcdff 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -27,8 +27,9 @@ def test_following_distanc(self): v_lead = float(speed) simulation_steady_state = run_following_distance_simulation(v_lead) - correct_steady_state = desired_follow_distance(v_lead, v_lead) + correct_steady_state = desired_follow_distance(v_lead, v_lead, 0.9) + print(simulation_steady_state / speed, correct_steady_state / speed) self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + .3)) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index ac19d2754010c3..9fb153902c9dc1 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -119,9 +119,9 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): v_rel = 0. # print at 5hz - if (self.rk.frame % (self.rate // 5)) == 0: - print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s" - % (self.current_time(), self.distance, self.speed, self.acceleration, d_rel, v_rel)) + # if (self.rk.frame % (self.rate // 5)) == 0: + # print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s" + # % (self.current_time(), self.distance, self.speed, self.acceleration, d_rel, v_rel)) # ******** update prevs ******** From bdf64028d05abafa9463bfc17d01703f344a96c8 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 13 Oct 2021 02:22:03 -0500 Subject: [PATCH 03/30] test it out --- common/colors.py | 36 ++++ common/op_params.py | 186 ++++++++++++++++++ .../lib/longitudinal_mpc_lib/long_mpc.py | 2 +- .../controls/lib/longitudinal_planner.py | 3 + 4 files changed, 226 insertions(+), 1 deletion(-) create mode 100644 common/colors.py create mode 100644 common/op_params.py diff --git a/common/colors.py b/common/colors.py new file mode 100644 index 00000000000000..95aab123a4dc44 --- /dev/null +++ b/common/colors.py @@ -0,0 +1,36 @@ +class COLORS: + def __init__(self): + self.HEADER = '\033[95m' + self.OKBLUE = '\033[94m' + self.CBLUE = '\33[44m' + self.BOLD = '\033[1m' + self.CITALIC = '\33[3m' + self.OKGREEN = '\033[92m' + self.CWHITE = '\33[37m' + self.ENDC = '\033[0m' + self.CWHITE + self.UNDERLINE = '\033[4m' + self.PINK = '\33[38;5;207m' + self.PRETTY_YELLOW = self.BASE(220) + + self.RED = '\033[91m' + self.PURPLE_BG = '\33[45m' + self.YELLOW = '\033[93m' + self.BLUE_GREEN = self.BASE(85) + + self.FAIL = self.RED + # self.INFO = self.PURPLE_BG + self.INFO = self.BASE(207) + self.SUCCESS = self.OKGREEN + self.PROMPT = self.YELLOW + self.DBLUE = '\033[36m' + self.CYAN = self.BASE(39) + self.WARNING = '\033[33m' + + def BASE(self, col): # seems to support more colors + return '\33[38;5;{}m'.format(col) + + def BASEBG(self, col): # seems to support more colors + return '\33[48;5;{}m'.format(col) + + +COLORS = COLORS() diff --git a/common/op_params.py b/common/op_params.py new file mode 100644 index 00000000000000..326753ffd6e2fc --- /dev/null +++ b/common/op_params.py @@ -0,0 +1,186 @@ +#!/usr/bin/env python3 +import os +import json +from atomicwrites import atomic_write +from common.colors import COLORS +from common.basedir import BASEDIR +from selfdrive.hardware import TICI +try: + from common.realtime import sec_since_boot +except ImportError: + import time + sec_since_boot = time.time + +warning = lambda msg: print('{}opParams WARNING: {}{}'.format(COLORS.WARNING, msg, COLORS.ENDC)) +error = lambda msg: print('{}opParams ERROR: {}{}'.format(COLORS.FAIL, msg, COLORS.ENDC)) + +NUMBER = [float, int] # value types +NONE_OR_NUMBER = [type(None), float, int] + +BASEDIR = os.path.dirname(BASEDIR) +PARAMS_DIR = os.path.join(BASEDIR, 'community', 'params') +IMPORTED_PATH = os.path.join(PARAMS_DIR, '.imported') +OLD_PARAMS_FILE = os.path.join(BASEDIR, 'op_params.json') + + +class Param: + def __init__(self, default, allowed_types=[], description=None, *, static=False, live=False, hidden=False): # pylint: disable=dangerous-default-value + self.default_value = default # value first saved and returned if actual value isn't a valid type + if not isinstance(allowed_types, list): + allowed_types = [allowed_types] + self.allowed_types = allowed_types # allowed python value types for opEdit + self.description = description # description to be shown in opEdit + self.hidden = hidden # hide this param to user in opEdit + self.live = live # show under the live menu in opEdit + self.static = static # use cached value, never reads to update + self._create_attrs() + + def is_valid(self, value): + if not self.has_allowed_types: # always valid if no allowed types, otherwise checks to make sure + return True + return type(value) in self.allowed_types + + def _create_attrs(self): # Create attributes and check Param is valid + self.has_allowed_types = isinstance(self.allowed_types, list) and len(self.allowed_types) > 0 + self.has_description = self.description is not None + self.is_list = list in self.allowed_types + self.read_frequency = None if self.static else (1 if self.live else 10) # how often to read param file (sec) + self.last_read = -1 + if self.has_allowed_types: + assert type(self.default_value) in self.allowed_types, 'Default value type must be in specified allowed_types!' + if self.is_list: + self.allowed_types.remove(list) + + +def _read_param(key): # Returns None, False if a json error occurs + try: + with open(os.path.join(PARAMS_DIR, key), 'r') as f: + value = json.loads(f.read()) + return value, True + except json.decoder.JSONDecodeError: + return None, False + + +def _write_param(key, value): + param_path = os.path.join(PARAMS_DIR, key) + with atomic_write(param_path, overwrite=True) as f: + f.write(json.dumps(value)) + os.chmod(param_path, 0o666) + + +def _import_params(): + if os.path.exists(OLD_PARAMS_FILE) and not os.path.exists(IMPORTED_PATH): # if opParams needs to import from old params file + try: + with open(OLD_PARAMS_FILE, 'r') as f: + old_params = json.loads(f.read()) + for key in old_params: + _write_param(key, old_params[key]) + open(IMPORTED_PATH, 'w').close() + except: # pylint: disable=bare-except + pass + + +class opParams: + def __init__(self): + """ + To add your own parameter to opParams in your fork, simply add a new entry in self.fork_params, instancing a new Param class with at minimum a default value. + The allowed_types and description args are not required but highly recommended to help users edit their parameters with opEdit safely. + - The description value will be shown to users when they use opEdit to change the value of the parameter. + - The allowed_types arg is used to restrict what kinds of values can be entered with opEdit so that users can't crash openpilot with unintended behavior. + (setting a param intended to be a number with a boolean, or viceversa for example) + Limiting the range of floats or integers is still recommended when `.get`ting the parameter. + When a None value is allowed, use `type(None)` instead of None, as opEdit checks the type against the values in the arg with `isinstance()`. + - If you want your param to update within a second, specify live=True. If your param is designed to be read once, specify static=True. + Specifying neither will have the param update every 10 seconds if constantly .get() + If the param is not static, call the .get() function on it in the update function of the file you're reading from to use live updating + + Here's an example of a good fork_param entry: + self.fork_params = {'camera_offset': Param(0.06, allowed_types=NUMBER), live=True} # NUMBER allows both floats and ints + """ + + self.fork_params = {'TR': Param(1.8, NUMBER, live=True)} + + self._to_delete = [] # a list of unused params you want to delete from users' params file + self._to_reset = [] # a list of params you want reset to their default values + self._run_init() # restores, reads, and updates params + + def _run_init(self): # does first time initializing of default params + # Two required parameters for opEdit + self.fork_params['username'] = Param(None, [type(None), str, bool], 'Your identifier provided with any crash logs sent to Sentry.\nHelps the developer reach out to you if anything goes wrong') + self.fork_params['op_edit_live_mode'] = Param(False, bool, 'This parameter controls which mode opEdit starts in', hidden=True) + + self.params = self._load_params(can_import=True) + self._add_default_params() # adds missing params and resets values with invalid types to self.params + self._delete_and_reset() # removes old params + + def get(self, key=None, *, force_update=False): # key=None returns dict of all params + if key is None: + return self._get_all_params(to_update=force_update) + self._check_key_exists(key, 'get') + param_info = self.fork_params[key] + rate = param_info.read_frequency # will be None if param is static, so check below + + if (not param_info.static and sec_since_boot() - self.fork_params[key].last_read >= rate) or force_update: + value, success = _read_param(key) + self.fork_params[key].last_read = sec_since_boot() + if not success: # in case of read error, use default and overwrite param + value = param_info.default_value + _write_param(key, value) + self.params[key] = value + + if param_info.is_valid(value := self.params[key]): + return value # all good, returning user's value + print(warning('User\'s value type is not valid! Returning default')) # somehow... it should always be valid + return param_info.default_value # return default value because user's value of key is not in allowed_types to avoid crashing openpilot + + def put(self, key, value): + self._check_key_exists(key, 'put') + if not self.fork_params[key].is_valid(value): + raise Exception('opParams: Tried to put a value of invalid type!') + self.params.update({key: value}) + _write_param(key, value) + + def _load_params(self, can_import=False): + if not os.path.exists(PARAMS_DIR): + os.makedirs(PARAMS_DIR) + if can_import: + _import_params() # just imports old params. below we read them in + + params = {} + for key in os.listdir(PARAMS_DIR): # PARAMS_DIR is guaranteed to exist + if key.startswith('.') or key not in self.fork_params: + continue + value, success = _read_param(key) + if not success: + value = self.fork_params[key].default_value + _write_param(key, value) + params[key] = value + return params + + def _get_all_params(self, to_update=False): + if to_update: + self.params = self._load_params() + return {k: self.params[k] for k, p in self.fork_params.items() if k in self.params and not p.hidden} + + def _check_key_exists(self, key, met): + if key not in self.fork_params: + raise Exception('opParams: Tried to {} an unknown parameter! Key not in fork_params: {}'.format(met, key)) + + def _add_default_params(self): + for key, param in self.fork_params.items(): + if key not in self.params: + self.params[key] = param.default_value + _write_param(key, self.params[key]) + elif not param.is_valid(self.params[key]): + print(warning('Value type of user\'s {} param not in allowed types, replacing with default!'.format(key))) + self.params[key] = param.default_value + _write_param(key, self.params[key]) + + def _delete_and_reset(self): + for key in list(self.params): + if key in self._to_delete: + del self.params[key] + os.remove(os.path.join(PARAMS_DIR, key)) + elif key in self._to_reset and key in self.fork_params: + self.params[key] = self.fork_params[key].default_value + _write_param(key, self.params[key]) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 9a11158f4b10cf..3ecaea8f31f26b 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -193,7 +193,7 @@ def __init__(self, e2e=False): self.accel_limit_arr[:,0] = -1.2 self.accel_limit_arr[:,1] = 1.2 self.source = SOURCES[2] - self.desired_TR = 0.9 + self.desired_TR = 1.8 def reset(self): self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 051a68a7415df9..59cf4790ca9003 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -12,6 +12,7 @@ from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N from selfdrive.swaglog import cloudlog +from common.op_params import opParams LON_MPC_STEP = 0.2 # first step is 0.2s AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted @@ -45,6 +46,7 @@ class Planner(): def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP self.mpc = LongitudinalMpc() + self.op_params = opParams() self.fcw = False @@ -86,6 +88,7 @@ def update(self, sm, CP): # clip limits, cannot init MPC outside of bounds accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) + self.mpc.desired_TR = self.op_params.get('TR') self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired, self.a_desired) self.mpc.update(sm['carState'], sm['radarState'], v_cruise) From 4bd63fef7e6af10a977f08918f14e88207cf05f0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 13 Oct 2021 02:24:00 -0500 Subject: [PATCH 04/30] add opEdit --- op_edit.py | 325 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 325 insertions(+) create mode 100755 op_edit.py diff --git a/op_edit.py b/op_edit.py new file mode 100755 index 00000000000000..212baad7708516 --- /dev/null +++ b/op_edit.py @@ -0,0 +1,325 @@ +#!/usr/bin/env python3 +import time +from common.op_params import opParams +import ast +import difflib +from common.colors import COLORS + + +class opEdit: # use by running `python /data/openpilot/op_edit.py` + def __init__(self): + self.op_params = opParams() + self.params = None + self.sleep_time = 0.5 + self.live_tuning = self.op_params.get('op_edit_live_mode') + self.username = self.op_params.get('username') + self.type_colors = {int: COLORS.BASE(179), float: COLORS.BASE(179), + bool: {False: COLORS.RED, True: COLORS.OKGREEN}, + type(None): COLORS.BASE(177), + str: COLORS.BASE(77)} + + self.last_choice = None + + self.run_init() + + def run_init(self): + if self.username is None: + self.success('\nWelcome to the {}opParams{} command line editor!'.format(COLORS.CYAN, COLORS.SUCCESS), sleep_time=0) + self.prompt('Would you like to add your Discord username for easier crash debugging for the fork owner?') + self.prompt('Your username is only used for reaching out if a crash occurs.') + + username_choice = self.input_with_options(['Y', 'N', 'don\'t ask again'], default='n')[0] + if username_choice == 0: + self.prompt('Enter a unique identifer/Discord username:') + username = '' + while username == '': + username = input('>> ').strip() + self.op_params.put('username', username) + self.username = username + self.success('Thanks! Saved your username\n' + 'Edit the \'username\' parameter at any time to update', sleep_time=1.5) + elif username_choice == 2: + self.op_params.put('username', False) + self.info('Got it, bringing you into opEdit\n' + 'Edit the \'username\' parameter at any time to update', sleep_time=1.0) + else: + self.success('\nWelcome to the {}opParams{} command line editor, {}!'.format(COLORS.CYAN, COLORS.SUCCESS, self.username), sleep_time=0) + + self.run_loop() + + def run_loop(self): + while True: + if not self.live_tuning: + self.info('Here are all your parameters:', sleep_time=0) + self.info('(non-static params update while driving)', end='\n', sleep_time=0) + else: + self.info('Here are your live parameters:', sleep_time=0) + self.info('(changes take effect within a second)', end='\n', sleep_time=0) + self.params = self.op_params.get(force_update=True) + if self.live_tuning: # only display live tunable params + self.params = {k: v for k, v in self.params.items() if self.op_params.fork_params[k].live} + + values_list = [] + for k, v in self.params.items(): + if len(str(v)) < 20: + v = self.color_from_type(v) + else: + v = '{} ... {}'.format(str(v)[:30], str(v)[-15:]) + values_list.append(v) + + static = [COLORS.INFO + '(static)' + COLORS.ENDC if self.op_params.fork_params[k].static else '' for k in self.params] + + to_print = [] + blue_gradient = [33, 39, 45, 51, 87] + for idx, param in enumerate(self.params): + line = '{}. {}: {} {}'.format(idx + 1, param, values_list[idx], static[idx]) + if idx == self.last_choice and self.last_choice is not None: + line = COLORS.OKGREEN + line + else: + _color = blue_gradient[min(round(idx / len(self.params) * len(blue_gradient)), len(blue_gradient) - 1)] + line = COLORS.BASE(_color) + line + to_print.append(line) + + extras = {'l': ('Toggle live params', COLORS.WARNING), + 'e': ('Exit opEdit', COLORS.PINK)} + + to_print += ['---'] + ['{}. {}'.format(ext_col + e, ext_txt + COLORS.ENDC) for e, (ext_txt, ext_col) in extras.items()] + print('\n'.join(to_print)) + self.prompt('\nChoose a parameter to edit (by index or name):') + + choice = input('>> ').strip().lower() + parsed, choice = self.parse_choice(choice, len(to_print) - len(extras)) + if parsed == 'continue': + continue + elif parsed == 'change': + self.last_choice = choice + self.change_parameter(choice) + elif parsed == 'live': + self.last_choice = None + self.live_tuning = not self.live_tuning + self.op_params.put('op_edit_live_mode', self.live_tuning) # for next opEdit startup + elif parsed == 'exit': + return + + def parse_choice(self, choice, opt_len): + if choice.isdigit(): + choice = int(choice) + choice -= 1 + if choice not in range(opt_len): # number of options to choose from + self.error('Not in range!') + return 'continue', choice + return 'change', choice + + if choice in ['l', 'live']: # live tuning mode + return 'live', choice + elif choice in ['exit', 'e', '']: + self.error('Exiting opEdit!', sleep_time=0) + return 'exit', choice + else: # find most similar param to user's input + param_sims = [(idx, self.str_sim(choice, param.lower())) for idx, param in enumerate(self.params)] + param_sims = [param for param in param_sims if param[1] > 0.33] + if len(param_sims) > 0: + chosen_param = sorted(param_sims, key=lambda param: param[1], reverse=True)[0] + return 'change', chosen_param[0] # return idx + + self.error('Invalid choice!') + return 'continue', choice + + def str_sim(self, a, b): + return difflib.SequenceMatcher(a=a, b=b).ratio() + + def change_parameter(self, choice): + while True: + chosen_key = list(self.params)[choice] + param_info = self.op_params.fork_params[chosen_key] + + old_value = self.params[chosen_key] + if not param_info.static: + self.info2('Chosen parameter: {}{} (live!)'.format(chosen_key, COLORS.BASE(207)), sleep_time=0) + else: + self.info2('Chosen parameter: {}{} (static)'.format(chosen_key, COLORS.BASE(207)), sleep_time=0) + + to_print = [] + if param_info.has_description: + to_print.append(COLORS.OKGREEN + '>> Description: {}'.format(param_info.description.replace('\n', '\n > ')) + COLORS.ENDC) + if param_info.static: + to_print.append(COLORS.WARNING + '>> A reboot is required for changes to this parameter!' + COLORS.ENDC) + if not param_info.static and not param_info.live: + to_print.append(COLORS.WARNING + '>> Changes take effect within 10 seconds for this parameter!' + COLORS.ENDC) + if param_info.has_allowed_types: + to_print.append(COLORS.RED + '>> Allowed types: {}'.format(', '.join([at.__name__ for at in param_info.allowed_types])) + COLORS.ENDC) + to_print.append(COLORS.WARNING + '>> Default value: {}'.format(self.color_from_type(param_info.default_value)) + COLORS.ENDC) + + if to_print: + print('\n{}\n'.format('\n'.join(to_print))) + + if param_info.is_list: + self.change_param_list(old_value, param_info, chosen_key) # TODO: need to merge the code in this function with the below to reduce redundant code + return + + self.info('Current value: {}{} (type: {})'.format(self.color_from_type(old_value), COLORS.INFO, type(old_value).__name__), sleep_time=0) + + while True: + self.prompt('\nEnter your new value (enter to exit):') + new_value = input('>> ').strip() + if new_value == '': + self.info('Exiting this parameter...\n') + return + + new_value = self.str_eval(new_value) + if not param_info.is_valid(new_value): + self.error('The type of data you entered ({}) is not allowed with this parameter!'.format(type(new_value).__name__)) + continue + + if not param_info.static: # stay in live tuning interface + self.op_params.put(chosen_key, new_value) + self.success('Saved {} with value: {}{}! (type: {})'.format(chosen_key, self.color_from_type(new_value), COLORS.SUCCESS, type(new_value).__name__)) + else: # else ask to save and break + self.warning('\nOld value: {}{} (type: {})'.format(self.color_from_type(old_value), COLORS.WARNING, type(old_value).__name__)) + self.success('New value: {}{} (type: {})'.format(self.color_from_type(new_value), COLORS.OKGREEN, type(new_value).__name__), sleep_time=0) + self.prompt('\nDo you want to save this?') + if self.input_with_options(['Y', 'N'], 'N')[0] == 0: + self.op_params.put(chosen_key, new_value) + self.success('Saved!') + else: + self.info('Not saved!') + return + + def change_param_list(self, old_value, param_info, chosen_key): + while True: + self.info('Current value: {} (type: {})'.format(old_value, type(old_value).__name__), sleep_time=0) + self.prompt('\nEnter index to edit (0 to {}):'.format(len(old_value) - 1)) + choice_idx = self.str_eval(input('>> ')) + if choice_idx == '': + self.info('Exiting this parameter...') + return + + if not isinstance(choice_idx, int) or choice_idx not in range(len(old_value)): + self.error('Must be an integar within list range!') + continue + + while True: + self.info('Chosen index: {}'.format(choice_idx), sleep_time=0) + self.info('Value: {} (type: {})'.format(old_value[choice_idx], type(old_value[choice_idx]).__name__), sleep_time=0) + self.prompt('\nEnter your new value:') + new_value = input('>> ').strip() + if new_value == '': + self.info('Exiting this list item...') + break + + new_value = self.str_eval(new_value) + if not param_info.is_valid(new_value): + self.error('The type of data you entered ({}) is not allowed with this parameter!'.format(type(new_value).__name__)) + continue + + old_value[choice_idx] = new_value + + self.op_params.put(chosen_key, old_value) + self.success('Saved {} with value: {}{}! (type: {})'.format(chosen_key, self.color_from_type(new_value), COLORS.SUCCESS, type(new_value).__name__), end='\n') + break + + def color_from_type(self, v): + v_color = '' + if type(v) in self.type_colors: + v_color = self.type_colors[type(v)] + if isinstance(v, bool): + v_color = v_color[v] + v = '{}{}{}'.format(v_color, v, COLORS.ENDC) + return v + + def cyan(self, msg, end=''): + msg = self.str_color(msg, style='cyan') + # print(msg, flush=True, end='\n' + end) + return msg + + def prompt(self, msg, end=''): + msg = self.str_color(msg, style='prompt') + print(msg, flush=True, end='\n' + end) + + def warning(self, msg, end=''): + msg = self.str_color(msg, style='warning') + print(msg, flush=True, end='\n' + end) + + def info(self, msg, sleep_time=None, end=''): + if sleep_time is None: + sleep_time = self.sleep_time + msg = self.str_color(msg, style='info') + + print(msg, flush=True, end='\n' + end) + time.sleep(sleep_time) + + def info2(self, msg, sleep_time=None, end=''): + if sleep_time is None: + sleep_time = self.sleep_time + msg = self.str_color(msg, style=86) + + print(msg, flush=True, end='\n' + end) + time.sleep(sleep_time) + + def error(self, msg, sleep_time=None, end='', surround=True): + if sleep_time is None: + sleep_time = self.sleep_time + msg = self.str_color(msg, style='fail', surround=surround) + + print(msg, flush=True, end='\n' + end) + time.sleep(sleep_time) + + def success(self, msg, sleep_time=None, end=''): + if sleep_time is None: + sleep_time = self.sleep_time + msg = self.str_color(msg, style='success') + + print(msg, flush=True, end='\n' + end) + time.sleep(sleep_time) + + @staticmethod + def str_color(msg, style, surround=False): + if style == 'success': + style = COLORS.SUCCESS + elif style == 'fail': + style = COLORS.FAIL + elif style == 'prompt': + style = COLORS.PROMPT + elif style == 'info': + style = COLORS.INFO + elif style == 'cyan': + style = COLORS.CYAN + elif style == 'warning': + style = COLORS.WARNING + elif isinstance(style, int): + style = COLORS.BASE(style) + + if surround: + msg = '{}--------\n{}\n{}--------{}'.format(style, msg, COLORS.ENDC + style, COLORS.ENDC) + else: + msg = '{}{}{}'.format(style, msg, COLORS.ENDC) + + return msg + + def input_with_options(self, options, default=None): + """ + Takes in a list of options and asks user to make a choice. + The most similar option list index is returned along with the similarity percentage from 0 to 1 + """ + user_input = input('[{}]: '.format('/'.join(options))).lower().strip() + if not user_input: + return default, 0.0 + sims = [self.str_sim(i.lower().strip(), user_input) for i in options] + argmax = sims.index(max(sims)) + return argmax, sims[argmax] + + def str_eval(self, dat): + dat = dat.strip() + try: + dat = ast.literal_eval(dat) + except: + if dat.lower() == 'none': + dat = None + elif dat.lower() == 'false': + dat = False + elif dat.lower() == 'true': # else, assume string + dat = True + return dat + + +opEdit() From 27d2fe642cbc8a764e5a545065d94e8eff766be6 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 13 Oct 2021 02:28:44 -0500 Subject: [PATCH 05/30] no static analysis --- .github/workflows/selfdrive_tests.yaml | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 9653a18fdd3743..64a41775da67da 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -163,19 +163,6 @@ jobs: $DOCKER_LOGIN docker push $DOCKER_REGISTRY/$BASE_IMAGE:latest - static_analysis: - name: static analysis - runs-on: ubuntu-20.04 - timeout-minutes: 50 - steps: - - uses: actions/checkout@v2 - with: - submodules: true - - name: Build Docker image - run: eval "$BUILD" - - name: pre-commit - run: ${{ env.RUN }} "git init && git add -A && pre-commit run --all" - valgrind: name: valgrind runs-on: ubuntu-20.04 From 332070c1b4a33002850831a9318f6457cdb93657 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 13 Oct 2021 12:10:05 -0500 Subject: [PATCH 06/30] fix toyota pedal gas --- selfdrive/car/toyota/carcontroller.py | 19 ++++++++++++++++--- selfdrive/car/toyota/interface.py | 7 +------ 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 2d17ee20f821df..5d0a1ceb21ec51 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -25,6 +25,20 @@ def accel_hysteresis(accel, accel_steady, enabled): return accel, accel_steady +def coast_accel(speed): # given a speed, output coasting acceleration + points = [[0.0, 0.03], [.166, .424], [.335, .568], + [.731, .440], [1.886, 0.262], [2.809, -0.207], + [3.443, -0.249], [MIN_ACC_SPEED, -0.145]] + return interp(speed, *zip(*points)) + + +def compute_gb_pedal(accel, speed): + _a3, _a4, _a5, _offset, _e1, _e2, _e3, _e4, _e5, _e6, _e7, _e8 = [-0.07264304340456754, -0.007522016704006004, 0.16234124452228196, 0.0029096574419830296, 1.1674372321165579e-05, -0.008010070095545522, -5.834025253616562e-05, 0.04722441060805912, 0.001887454016549489, -0.0014370672920621269, -0.007577594283906699, 0.01943515032956308] + speed_part = (_e5 * accel + _e6) * speed ** 2 + (_e7 * accel + _e8) * speed + accel_part = ((_e1 * speed + _e2) * accel ** 5 + (_e3 * speed + _e4) * accel ** 4 + _a3 * accel ** 3 + _a4 * accel ** 2 + _a5 * accel) + return speed_part + accel_part + _offset + + class CarController(): def __init__(self, dbc_name, CP, VM): self.last_steer = 0 @@ -56,9 +70,8 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, if self.use_interceptor and enabled: # only send negative accel when using interceptor. gas handles acceleration # +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged - MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED], [0.2, 0.5]) - interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0., MAX_INTERCEPTOR_GAS) - pcm_accel_cmd = 0.18 - max(0, -actuators.accel) + if pcm_accel_cmd > coast_accel(CS.out.vEgo): + interceptor_gas_cmd = clip(compute_gb_pedal(pcm_accel_cmd, CS.out.vEgo), 0., 1.) pcm_accel_cmd, self.accel_steady = accel_hysteresis(pcm_accel_cmd, self.accel_steady, enabled) pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 643876af118a93..dc78cfbf4f9f74 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -315,12 +315,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py # intercepting the DSU is a community feature since it requires unofficial hardware ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu - if ret.enableGasInterceptor: - ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] - ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5] - ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] - ret.longitudinalTuning.kiV = [0.18, 0.165, 0.489, 0.36] - elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]: + if candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]: # Improved longitudinal tune ret.longitudinalTuning.deadzoneBP = [0., 8.05] ret.longitudinalTuning.deadzoneV = [.0, .14] From 5a5afef1a957b4058958aeee56cf225c140e1666 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 13 Oct 2021 12:13:58 -0500 Subject: [PATCH 07/30] fix pedal --- selfdrive/car/toyota/carcontroller.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 5d0a1ceb21ec51..8b39fd830e764f 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -72,6 +72,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, # +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged if pcm_accel_cmd > coast_accel(CS.out.vEgo): interceptor_gas_cmd = clip(compute_gb_pedal(pcm_accel_cmd, CS.out.vEgo), 0., 1.) + pcm_accel_cmd = 0.18 - max(0, -actuators.accel) pcm_accel_cmd, self.accel_steady = accel_hysteresis(pcm_accel_cmd, self.accel_steady, enabled) pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) From 10c70d71c0d72351db77d2a638bfeff450059b8d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 17 Oct 2021 16:08:13 -0500 Subject: [PATCH 08/30] make desired distance a live parameter (is it this easy??) --- .../lib/longitudinal_mpc_lib/long_mpc.py | 39 +++++++++++-------- 1 file changed, 22 insertions(+), 17 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 3ecaea8f31f26b..61d411074f9783 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -83,9 +83,10 @@ def gen_long_model(): # live parameters x_obstacle = SX.sym('x_obstacle') + desired_dist_comfort = SX.sym('desired_dist_comfort') a_min = SX.sym('a_min') a_max = SX.sym('a_max') - model.p = vertcat(a_min, a_max, x_obstacle) + model.p = vertcat(a_min, a_max, x_obstacle, desired_dist_comfort) # dynamics model f_expl = vertcat(v_ego, a_ego, j_ego) @@ -118,17 +119,18 @@ def gen_long_mpc_solver(): a_min, a_max = ocp.model.p[0], ocp.model.p[1] x_obstacle = ocp.model.p[2] + desired_dist_comfort = ocp.model.p[3] ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - desired_dist_comfort = get_safe_obstacle_distance(v_ego) + # desired_dist_comfort = get_safe_obstacle_distance(v_ego) # The main cost in normal operation is how close you are to the "desired" distance # from an obstacle at every timestep. This obstacle can be a lead car # or other object. In e2e mode we can use x_position targets as a cost # instead. - costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.), + costs = [((x_obstacle - x_ego) - (desired_dist_comfort[0])) / (v_ego + 10.), x_ego, v_ego, a_ego, @@ -142,13 +144,13 @@ def gen_long_mpc_solver(): constraints = vertcat((v_ego), (a_ego - a_min), (a_max - a_ego), - ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.)) + ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort[0])) / (v_ego + 10.)) ocp.model.con_h_expr = constraints ocp.model.con_h_expr_e = vertcat(np.zeros(CONSTR_DIM)) x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 - ocp.parameter_values = np.array([-1.2, 1.2, 0.0]) + ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0]) # We put all constraint cost weights to 0 and only set them at runtime cost_weights = np.zeros(CONSTR_DIM) @@ -312,25 +314,27 @@ def update(self, carstate, radarstate, v_cruise): # This may all be wrong, just some tests to enable a hacky adjustable following distance # Calculates difference in ideal distance vs actual distance to a TR diff in seconds - lead_react_diff_0 = (lead_xv_ideal_0[:,0] - lead_xv_0[:,0]) / lead_xv_0[:,1] - lead_react_diff_1 = (lead_xv_ideal_1[:,0] - lead_xv_1[:,0]) / lead_xv_1[:,1] + # lead_react_diff_0 = (lead_xv_ideal_0[:,0] - lead_xv_0[:,0]) / lead_xv_0[:,1] + # lead_react_diff_1 = (lead_xv_ideal_1[:,0] - lead_xv_1[:,0]) / lead_xv_1[:,1] # TODO: some tuning to be had here # basically the lower the desired TR the more we want to stick near it # so we come up with a cost to multiply difference in actual TR vs. desired TR by # and thus the less we change the stopped factor below # react_diff_cost = np.interp(self.desired_TR, [0.9, 1.8, 2.7], [2, 1, 0.5]) - react_diff_cost = 1. + # react_diff_cost = 1. # print(lead_react_diff_0[0]) - lead_react_diff_0 = lead_react_diff_0[0] - lead_react_diff_1 = lead_react_diff_1[0] - react_diff_mult_0 = np.interp(abs(lead_react_diff_0) * react_diff_cost, [0, 0.9], [1, 0.1]) - react_diff_mult_1 = np.interp(abs(lead_react_diff_1) * react_diff_cost, [0, 0.9], [1, 0.1]) - - t_react_compensation_0 = T_REACT + (T_REACT - self.desired_TR) - t_react_compensation_1 = T_REACT + (T_REACT - self.desired_TR) - lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], t_react_compensation_0) - lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], t_react_compensation_1) + # lead_react_diff_0 = lead_react_diff_0[0] + # lead_react_diff_1 = lead_react_diff_1[0] + # react_diff_mult_0 = np.interp(abs(lead_react_diff_0) * react_diff_cost, [0, 0.9], [1, 0.1]) + # react_diff_mult_1 = np.interp(abs(lead_react_diff_1) * react_diff_cost, [0, 0.9], [1, 0.1]) + + # t_react_compensation_0 = T_REACT + (T_REACT - self.desired_TR) + # t_react_compensation_1 = T_REACT + (T_REACT - self.desired_TR) + # lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], t_react_compensation_0) + # lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], t_react_compensation_1) + lead_0_obstacle = lead_xv_0[:, 0] + get_stopped_equivalence_factor(lead_xv_0[:, 1]) + lead_1_obstacle = lead_xv_1[:, 0] + get_stopped_equivalence_factor(lead_xv_1[:, 1]) # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. @@ -344,6 +348,7 @@ def update(self, carstate, radarstate, v_cruise): x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] self.params[:,2] = np.min(x_obstacles, axis=1) + self.params[:,3] = get_safe_obstacle_distance(v_ego, self.desired_TR) self.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and From 88c9946cf01c421375f6c05712ce571c36d75ace Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 17 Oct 2021 16:08:45 -0500 Subject: [PATCH 09/30] probs okay using whole arr --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 61d411074f9783..f8489d2888d775 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -130,7 +130,7 @@ def gen_long_mpc_solver(): # from an obstacle at every timestep. This obstacle can be a lead car # or other object. In e2e mode we can use x_position targets as a cost # instead. - costs = [((x_obstacle - x_ego) - (desired_dist_comfort[0])) / (v_ego + 10.), + costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.), x_ego, v_ego, a_ego, @@ -144,7 +144,7 @@ def gen_long_mpc_solver(): constraints = vertcat((v_ego), (a_ego - a_min), (a_max - a_ego), - ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort[0])) / (v_ego + 10.)) + ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.)) ocp.model.con_h_expr = constraints ocp.model.con_h_expr_e = vertcat(np.zeros(CONSTR_DIM)) From d18368c0674c7bd3964ca2449073917f842079a4 Mon Sep 17 00:00:00 2001 From: Comma Device Date: Sun, 17 Oct 2021 19:39:40 -0500 Subject: [PATCH 10/30] fixes --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index f8489d2888d775..f3bc341ef9b551 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -208,7 +208,7 @@ def reset(self): self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) self.x_sol = np.zeros((N+1, X_DIM)) self.u_sol = np.zeros((N,1)) - self.params = np.zeros((N+1,3)) + self.params = np.zeros((N+1,4)) for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) self.last_cloudlog_t = 0 @@ -333,8 +333,8 @@ def update(self, carstate, radarstate, v_cruise): # t_react_compensation_1 = T_REACT + (T_REACT - self.desired_TR) # lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], t_react_compensation_0) # lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], t_react_compensation_1) - lead_0_obstacle = lead_xv_0[:, 0] + get_stopped_equivalence_factor(lead_xv_0[:, 1]) - lead_1_obstacle = lead_xv_1[:, 0] + get_stopped_equivalence_factor(lead_xv_1[:, 1]) + lead_0_obstacle = lead_xv_0[:, 0] + get_stopped_equivalence_factor(lead_xv_0[:, 1], self.desired_TR) + lead_1_obstacle = lead_xv_1[:, 0] + get_stopped_equivalence_factor(lead_xv_1[:, 1], self.desired_TR) # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. @@ -343,7 +343,7 @@ def update(self, carstate, radarstate, v_cruise): v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), cruise_lower_bound, cruise_upper_bound) - cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped) + cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped, self.desired_TR) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] From 0c28c0de99e67554399622eb9b74d1a88e4aeba2 Mon Sep 17 00:00:00 2001 From: sshane Date: Sun, 17 Oct 2021 22:05:21 -0500 Subject: [PATCH 11/30] Try this --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index f3bc341ef9b551..1c0e9ea8c04202 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -119,12 +119,12 @@ def gen_long_mpc_solver(): a_min, a_max = ocp.model.p[0], ocp.model.p[1] x_obstacle = ocp.model.p[2] - desired_dist_comfort = ocp.model.p[3] + desired_TR = ocp.model.p[3] ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - # desired_dist_comfort = get_safe_obstacle_distance(v_ego) + desired_dist_comfort = get_safe_obstacle_distance(v_ego, desired_TR) # The main cost in normal operation is how close you are to the "desired" distance # from an obstacle at every timestep. This obstacle can be a lead car @@ -150,7 +150,7 @@ def gen_long_mpc_solver(): x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 - ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0]) + ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 1.8]) # We put all constraint cost weights to 0 and only set them at runtime cost_weights = np.zeros(CONSTR_DIM) @@ -348,7 +348,7 @@ def update(self, carstate, radarstate, v_cruise): x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] self.params[:,2] = np.min(x_obstacles, axis=1) - self.params[:,3] = get_safe_obstacle_distance(v_ego, self.desired_TR) + self.params[:,3] = self.desired_TR self.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and From 6fbb1eefcd4e91c24f97af3291bdff0c81b41aa3 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 18 Oct 2021 01:46:52 -0500 Subject: [PATCH 12/30] clean up --- .../lib/longitudinal_mpc_lib/long_mpc.py | 41 ++++--------------- 1 file changed, 9 insertions(+), 32 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 1c0e9ea8c04202..87377186e2a040 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -82,11 +82,11 @@ def gen_long_model(): model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot) # live parameters - x_obstacle = SX.sym('x_obstacle') - desired_dist_comfort = SX.sym('desired_dist_comfort') a_min = SX.sym('a_min') a_max = SX.sym('a_max') - model.p = vertcat(a_min, a_max, x_obstacle, desired_dist_comfort) + x_obstacle = SX.sym('x_obstacle') + desired_TR = SX.sym('desired_TR') + model.p = vertcat(a_min, a_max, x_obstacle, desired_TR) # dynamics model f_expl = vertcat(v_ego, a_ego, j_ego) @@ -150,7 +150,7 @@ def gen_long_mpc_solver(): x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 - ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 1.8]) + ocp.parameter_values = np.array([-1.2, 1.2, 0.0, T_REACT]) # We put all constraint cost weights to 0 and only set them at runtime cost_weights = np.zeros(CONSTR_DIM) @@ -225,6 +225,7 @@ def set_weights(self): self.set_weights_for_lead_policy() def set_weights_for_lead_policy(self): + # TODO: tune x_ego cost based on TR W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, J_EGO_COST])) for i in range(N): self.solver.cost_set(i, 'W', W) @@ -288,9 +289,7 @@ def process_lead(self, lead): v_lead = clip(v_lead, 0.0, 1e8) a_lead = clip(a_lead, -10., 5.) lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) - # TODO: experiment with setting accel to 0 so lead's accel is factored in (only distance is considered now) - lead_xv_ideal = self.extrapolate_lead(get_stopped_equivalence_factor(v_lead, self.desired_TR), v_lead, a_lead, a_lead_tau) - return lead_xv, lead_xv_ideal + return lead_xv def set_accel_limits(self, min_a, max_a): self.cruise_min_a = min_a @@ -300,8 +299,8 @@ def update(self, carstate, radarstate, v_cruise): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status - lead_xv_0, lead_xv_ideal_0 = self.process_lead(radarstate.leadOne) - lead_xv_1, lead_xv_ideal_1 = self.process_lead(radarstate.leadTwo) + lead_xv_0 = self.process_lead(radarstate.leadOne) + lead_xv_1 = self.process_lead(radarstate.leadTwo) # set accel limits in params self.params[:,0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL]) @@ -310,29 +309,6 @@ def update(self, carstate, radarstate, v_cruise): # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. - - # This may all be wrong, just some tests to enable a hacky adjustable following distance - - # Calculates difference in ideal distance vs actual distance to a TR diff in seconds - # lead_react_diff_0 = (lead_xv_ideal_0[:,0] - lead_xv_0[:,0]) / lead_xv_0[:,1] - # lead_react_diff_1 = (lead_xv_ideal_1[:,0] - lead_xv_1[:,0]) / lead_xv_1[:,1] - - # TODO: some tuning to be had here - # basically the lower the desired TR the more we want to stick near it - # so we come up with a cost to multiply difference in actual TR vs. desired TR by - # and thus the less we change the stopped factor below - # react_diff_cost = np.interp(self.desired_TR, [0.9, 1.8, 2.7], [2, 1, 0.5]) - # react_diff_cost = 1. - # print(lead_react_diff_0[0]) - # lead_react_diff_0 = lead_react_diff_0[0] - # lead_react_diff_1 = lead_react_diff_1[0] - # react_diff_mult_0 = np.interp(abs(lead_react_diff_0) * react_diff_cost, [0, 0.9], [1, 0.1]) - # react_diff_mult_1 = np.interp(abs(lead_react_diff_1) * react_diff_cost, [0, 0.9], [1, 0.1]) - - # t_react_compensation_0 = T_REACT + (T_REACT - self.desired_TR) - # t_react_compensation_1 = T_REACT + (T_REACT - self.desired_TR) - # lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], t_react_compensation_0) - # lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], t_react_compensation_1) lead_0_obstacle = lead_xv_0[:, 0] + get_stopped_equivalence_factor(lead_xv_0[:, 1], self.desired_TR) lead_1_obstacle = lead_xv_1[:, 0] + get_stopped_equivalence_factor(lead_xv_1[:, 1], self.desired_TR) @@ -367,6 +343,7 @@ def update_with_xva(self, x, v, a): self.accel_limit_arr[:,0] = -10. self.accel_limit_arr[:,1] = 10. x_obstacle = 1e5*np.ones((N+1)) + # this doesn't consider fourth TR param, set to anything if needed self.params = np.concatenate([self.accel_limit_arr, x_obstacle[:,None]], axis=1) self.run() From d8003dd5cae7b8d6a070820904ac8cc3d91a4869 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 18 Oct 2021 01:51:10 -0500 Subject: [PATCH 13/30] Revert "no static analysis" This reverts commit a05af010309c18d8606ed58319f7129f09140081. --- .github/workflows/selfdrive_tests.yaml | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 64a41775da67da..9653a18fdd3743 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -163,6 +163,19 @@ jobs: $DOCKER_LOGIN docker push $DOCKER_REGISTRY/$BASE_IMAGE:latest + static_analysis: + name: static analysis + runs-on: ubuntu-20.04 + timeout-minutes: 50 + steps: + - uses: actions/checkout@v2 + with: + submodules: true + - name: Build Docker image + run: eval "$BUILD" + - name: pre-commit + run: ${{ env.RUN }} "git init && git add -A && pre-commit run --all" + valgrind: name: valgrind runs-on: ubuntu-20.04 From 5a4bb3ad80d0f23c53834668b3b7b354b032ec2a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 18 Oct 2021 01:51:21 -0500 Subject: [PATCH 14/30] clean up --- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 87377186e2a040..7fd945262e9afb 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -188,14 +188,14 @@ def gen_long_mpc_solver(): class LongitudinalMpc(): - def __init__(self, e2e=False): + def __init__(self, e2e=False, desired_TR=T_REACT): self.e2e = e2e self.reset() self.accel_limit_arr = np.zeros((N+1, 2)) self.accel_limit_arr[:,0] = -1.2 self.accel_limit_arr[:,1] = 1.2 self.source = SOURCES[2] - self.desired_TR = 1.8 + self.desired_TR = desired_TR def reset(self): self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR) @@ -309,8 +309,8 @@ def update(self, carstate, radarstate, v_cruise): # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. - lead_0_obstacle = lead_xv_0[:, 0] + get_stopped_equivalence_factor(lead_xv_0[:, 1], self.desired_TR) - lead_1_obstacle = lead_xv_1[:, 0] + get_stopped_equivalence_factor(lead_xv_1[:, 1], self.desired_TR) + lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], self.desired_TR) + lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.desired_TR) # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. @@ -343,7 +343,7 @@ def update_with_xva(self, x, v, a): self.accel_limit_arr[:,0] = -10. self.accel_limit_arr[:,1] = 10. x_obstacle = 1e5*np.ones((N+1)) - # this doesn't consider fourth TR param, set to anything if needed + # FIXME: this doesn't consider fourth TR param, set to anything if needed self.params = np.concatenate([self.accel_limit_arr, x_obstacle[:,None]], axis=1) self.run() From b8ead6e108f1ceea384b60e46002c66e08c5a33a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 18 Oct 2021 01:51:48 -0500 Subject: [PATCH 15/30] Revert "fix pedal" This reverts commit 54cd7c22c2311c569ca80904bfdd02da1b61972f. --- selfdrive/car/toyota/carcontroller.py | 1 - 1 file changed, 1 deletion(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 8b39fd830e764f..5d0a1ceb21ec51 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -72,7 +72,6 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, # +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged if pcm_accel_cmd > coast_accel(CS.out.vEgo): interceptor_gas_cmd = clip(compute_gb_pedal(pcm_accel_cmd, CS.out.vEgo), 0., 1.) - pcm_accel_cmd = 0.18 - max(0, -actuators.accel) pcm_accel_cmd, self.accel_steady = accel_hysteresis(pcm_accel_cmd, self.accel_steady, enabled) pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) From b16943eae02b01cac0767e75a052e9b3231e7b6a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 18 Oct 2021 01:51:52 -0500 Subject: [PATCH 16/30] Revert "fix toyota pedal gas" This reverts commit d14bdd55220db7a46a93465e70420095e25ed5f1. --- selfdrive/car/toyota/carcontroller.py | 19 +++---------------- selfdrive/car/toyota/interface.py | 7 ++++++- 2 files changed, 9 insertions(+), 17 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 5d0a1ceb21ec51..2d17ee20f821df 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -25,20 +25,6 @@ def accel_hysteresis(accel, accel_steady, enabled): return accel, accel_steady -def coast_accel(speed): # given a speed, output coasting acceleration - points = [[0.0, 0.03], [.166, .424], [.335, .568], - [.731, .440], [1.886, 0.262], [2.809, -0.207], - [3.443, -0.249], [MIN_ACC_SPEED, -0.145]] - return interp(speed, *zip(*points)) - - -def compute_gb_pedal(accel, speed): - _a3, _a4, _a5, _offset, _e1, _e2, _e3, _e4, _e5, _e6, _e7, _e8 = [-0.07264304340456754, -0.007522016704006004, 0.16234124452228196, 0.0029096574419830296, 1.1674372321165579e-05, -0.008010070095545522, -5.834025253616562e-05, 0.04722441060805912, 0.001887454016549489, -0.0014370672920621269, -0.007577594283906699, 0.01943515032956308] - speed_part = (_e5 * accel + _e6) * speed ** 2 + (_e7 * accel + _e8) * speed - accel_part = ((_e1 * speed + _e2) * accel ** 5 + (_e3 * speed + _e4) * accel ** 4 + _a3 * accel ** 3 + _a4 * accel ** 2 + _a5 * accel) - return speed_part + accel_part + _offset - - class CarController(): def __init__(self, dbc_name, CP, VM): self.last_steer = 0 @@ -70,8 +56,9 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, if self.use_interceptor and enabled: # only send negative accel when using interceptor. gas handles acceleration # +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged - if pcm_accel_cmd > coast_accel(CS.out.vEgo): - interceptor_gas_cmd = clip(compute_gb_pedal(pcm_accel_cmd, CS.out.vEgo), 0., 1.) + MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED], [0.2, 0.5]) + interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0., MAX_INTERCEPTOR_GAS) + pcm_accel_cmd = 0.18 - max(0, -actuators.accel) pcm_accel_cmd, self.accel_steady = accel_hysteresis(pcm_accel_cmd, self.accel_steady, enabled) pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index dc78cfbf4f9f74..643876af118a93 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -315,7 +315,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py # intercepting the DSU is a community feature since it requires unofficial hardware ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu - if candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]: + if ret.enableGasInterceptor: + ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5] + ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] + ret.longitudinalTuning.kiV = [0.18, 0.165, 0.489, 0.36] + elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]: # Improved longitudinal tune ret.longitudinalTuning.deadzoneBP = [0., 8.05] ret.longitudinalTuning.deadzoneV = [.0, .14] From e7fd715495679b65f4117a9833414de62000317d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 18 Oct 2021 01:57:19 -0500 Subject: [PATCH 17/30] Revert "add opEdit" This reverts commit ade5cce9503cfd3760ae0f6371a7ba7b9a1d0b0f. --- op_edit.py | 325 ----------------------------------------------------- 1 file changed, 325 deletions(-) delete mode 100755 op_edit.py diff --git a/op_edit.py b/op_edit.py deleted file mode 100755 index 212baad7708516..00000000000000 --- a/op_edit.py +++ /dev/null @@ -1,325 +0,0 @@ -#!/usr/bin/env python3 -import time -from common.op_params import opParams -import ast -import difflib -from common.colors import COLORS - - -class opEdit: # use by running `python /data/openpilot/op_edit.py` - def __init__(self): - self.op_params = opParams() - self.params = None - self.sleep_time = 0.5 - self.live_tuning = self.op_params.get('op_edit_live_mode') - self.username = self.op_params.get('username') - self.type_colors = {int: COLORS.BASE(179), float: COLORS.BASE(179), - bool: {False: COLORS.RED, True: COLORS.OKGREEN}, - type(None): COLORS.BASE(177), - str: COLORS.BASE(77)} - - self.last_choice = None - - self.run_init() - - def run_init(self): - if self.username is None: - self.success('\nWelcome to the {}opParams{} command line editor!'.format(COLORS.CYAN, COLORS.SUCCESS), sleep_time=0) - self.prompt('Would you like to add your Discord username for easier crash debugging for the fork owner?') - self.prompt('Your username is only used for reaching out if a crash occurs.') - - username_choice = self.input_with_options(['Y', 'N', 'don\'t ask again'], default='n')[0] - if username_choice == 0: - self.prompt('Enter a unique identifer/Discord username:') - username = '' - while username == '': - username = input('>> ').strip() - self.op_params.put('username', username) - self.username = username - self.success('Thanks! Saved your username\n' - 'Edit the \'username\' parameter at any time to update', sleep_time=1.5) - elif username_choice == 2: - self.op_params.put('username', False) - self.info('Got it, bringing you into opEdit\n' - 'Edit the \'username\' parameter at any time to update', sleep_time=1.0) - else: - self.success('\nWelcome to the {}opParams{} command line editor, {}!'.format(COLORS.CYAN, COLORS.SUCCESS, self.username), sleep_time=0) - - self.run_loop() - - def run_loop(self): - while True: - if not self.live_tuning: - self.info('Here are all your parameters:', sleep_time=0) - self.info('(non-static params update while driving)', end='\n', sleep_time=0) - else: - self.info('Here are your live parameters:', sleep_time=0) - self.info('(changes take effect within a second)', end='\n', sleep_time=0) - self.params = self.op_params.get(force_update=True) - if self.live_tuning: # only display live tunable params - self.params = {k: v for k, v in self.params.items() if self.op_params.fork_params[k].live} - - values_list = [] - for k, v in self.params.items(): - if len(str(v)) < 20: - v = self.color_from_type(v) - else: - v = '{} ... {}'.format(str(v)[:30], str(v)[-15:]) - values_list.append(v) - - static = [COLORS.INFO + '(static)' + COLORS.ENDC if self.op_params.fork_params[k].static else '' for k in self.params] - - to_print = [] - blue_gradient = [33, 39, 45, 51, 87] - for idx, param in enumerate(self.params): - line = '{}. {}: {} {}'.format(idx + 1, param, values_list[idx], static[idx]) - if idx == self.last_choice and self.last_choice is not None: - line = COLORS.OKGREEN + line - else: - _color = blue_gradient[min(round(idx / len(self.params) * len(blue_gradient)), len(blue_gradient) - 1)] - line = COLORS.BASE(_color) + line - to_print.append(line) - - extras = {'l': ('Toggle live params', COLORS.WARNING), - 'e': ('Exit opEdit', COLORS.PINK)} - - to_print += ['---'] + ['{}. {}'.format(ext_col + e, ext_txt + COLORS.ENDC) for e, (ext_txt, ext_col) in extras.items()] - print('\n'.join(to_print)) - self.prompt('\nChoose a parameter to edit (by index or name):') - - choice = input('>> ').strip().lower() - parsed, choice = self.parse_choice(choice, len(to_print) - len(extras)) - if parsed == 'continue': - continue - elif parsed == 'change': - self.last_choice = choice - self.change_parameter(choice) - elif parsed == 'live': - self.last_choice = None - self.live_tuning = not self.live_tuning - self.op_params.put('op_edit_live_mode', self.live_tuning) # for next opEdit startup - elif parsed == 'exit': - return - - def parse_choice(self, choice, opt_len): - if choice.isdigit(): - choice = int(choice) - choice -= 1 - if choice not in range(opt_len): # number of options to choose from - self.error('Not in range!') - return 'continue', choice - return 'change', choice - - if choice in ['l', 'live']: # live tuning mode - return 'live', choice - elif choice in ['exit', 'e', '']: - self.error('Exiting opEdit!', sleep_time=0) - return 'exit', choice - else: # find most similar param to user's input - param_sims = [(idx, self.str_sim(choice, param.lower())) for idx, param in enumerate(self.params)] - param_sims = [param for param in param_sims if param[1] > 0.33] - if len(param_sims) > 0: - chosen_param = sorted(param_sims, key=lambda param: param[1], reverse=True)[0] - return 'change', chosen_param[0] # return idx - - self.error('Invalid choice!') - return 'continue', choice - - def str_sim(self, a, b): - return difflib.SequenceMatcher(a=a, b=b).ratio() - - def change_parameter(self, choice): - while True: - chosen_key = list(self.params)[choice] - param_info = self.op_params.fork_params[chosen_key] - - old_value = self.params[chosen_key] - if not param_info.static: - self.info2('Chosen parameter: {}{} (live!)'.format(chosen_key, COLORS.BASE(207)), sleep_time=0) - else: - self.info2('Chosen parameter: {}{} (static)'.format(chosen_key, COLORS.BASE(207)), sleep_time=0) - - to_print = [] - if param_info.has_description: - to_print.append(COLORS.OKGREEN + '>> Description: {}'.format(param_info.description.replace('\n', '\n > ')) + COLORS.ENDC) - if param_info.static: - to_print.append(COLORS.WARNING + '>> A reboot is required for changes to this parameter!' + COLORS.ENDC) - if not param_info.static and not param_info.live: - to_print.append(COLORS.WARNING + '>> Changes take effect within 10 seconds for this parameter!' + COLORS.ENDC) - if param_info.has_allowed_types: - to_print.append(COLORS.RED + '>> Allowed types: {}'.format(', '.join([at.__name__ for at in param_info.allowed_types])) + COLORS.ENDC) - to_print.append(COLORS.WARNING + '>> Default value: {}'.format(self.color_from_type(param_info.default_value)) + COLORS.ENDC) - - if to_print: - print('\n{}\n'.format('\n'.join(to_print))) - - if param_info.is_list: - self.change_param_list(old_value, param_info, chosen_key) # TODO: need to merge the code in this function with the below to reduce redundant code - return - - self.info('Current value: {}{} (type: {})'.format(self.color_from_type(old_value), COLORS.INFO, type(old_value).__name__), sleep_time=0) - - while True: - self.prompt('\nEnter your new value (enter to exit):') - new_value = input('>> ').strip() - if new_value == '': - self.info('Exiting this parameter...\n') - return - - new_value = self.str_eval(new_value) - if not param_info.is_valid(new_value): - self.error('The type of data you entered ({}) is not allowed with this parameter!'.format(type(new_value).__name__)) - continue - - if not param_info.static: # stay in live tuning interface - self.op_params.put(chosen_key, new_value) - self.success('Saved {} with value: {}{}! (type: {})'.format(chosen_key, self.color_from_type(new_value), COLORS.SUCCESS, type(new_value).__name__)) - else: # else ask to save and break - self.warning('\nOld value: {}{} (type: {})'.format(self.color_from_type(old_value), COLORS.WARNING, type(old_value).__name__)) - self.success('New value: {}{} (type: {})'.format(self.color_from_type(new_value), COLORS.OKGREEN, type(new_value).__name__), sleep_time=0) - self.prompt('\nDo you want to save this?') - if self.input_with_options(['Y', 'N'], 'N')[0] == 0: - self.op_params.put(chosen_key, new_value) - self.success('Saved!') - else: - self.info('Not saved!') - return - - def change_param_list(self, old_value, param_info, chosen_key): - while True: - self.info('Current value: {} (type: {})'.format(old_value, type(old_value).__name__), sleep_time=0) - self.prompt('\nEnter index to edit (0 to {}):'.format(len(old_value) - 1)) - choice_idx = self.str_eval(input('>> ')) - if choice_idx == '': - self.info('Exiting this parameter...') - return - - if not isinstance(choice_idx, int) or choice_idx not in range(len(old_value)): - self.error('Must be an integar within list range!') - continue - - while True: - self.info('Chosen index: {}'.format(choice_idx), sleep_time=0) - self.info('Value: {} (type: {})'.format(old_value[choice_idx], type(old_value[choice_idx]).__name__), sleep_time=0) - self.prompt('\nEnter your new value:') - new_value = input('>> ').strip() - if new_value == '': - self.info('Exiting this list item...') - break - - new_value = self.str_eval(new_value) - if not param_info.is_valid(new_value): - self.error('The type of data you entered ({}) is not allowed with this parameter!'.format(type(new_value).__name__)) - continue - - old_value[choice_idx] = new_value - - self.op_params.put(chosen_key, old_value) - self.success('Saved {} with value: {}{}! (type: {})'.format(chosen_key, self.color_from_type(new_value), COLORS.SUCCESS, type(new_value).__name__), end='\n') - break - - def color_from_type(self, v): - v_color = '' - if type(v) in self.type_colors: - v_color = self.type_colors[type(v)] - if isinstance(v, bool): - v_color = v_color[v] - v = '{}{}{}'.format(v_color, v, COLORS.ENDC) - return v - - def cyan(self, msg, end=''): - msg = self.str_color(msg, style='cyan') - # print(msg, flush=True, end='\n' + end) - return msg - - def prompt(self, msg, end=''): - msg = self.str_color(msg, style='prompt') - print(msg, flush=True, end='\n' + end) - - def warning(self, msg, end=''): - msg = self.str_color(msg, style='warning') - print(msg, flush=True, end='\n' + end) - - def info(self, msg, sleep_time=None, end=''): - if sleep_time is None: - sleep_time = self.sleep_time - msg = self.str_color(msg, style='info') - - print(msg, flush=True, end='\n' + end) - time.sleep(sleep_time) - - def info2(self, msg, sleep_time=None, end=''): - if sleep_time is None: - sleep_time = self.sleep_time - msg = self.str_color(msg, style=86) - - print(msg, flush=True, end='\n' + end) - time.sleep(sleep_time) - - def error(self, msg, sleep_time=None, end='', surround=True): - if sleep_time is None: - sleep_time = self.sleep_time - msg = self.str_color(msg, style='fail', surround=surround) - - print(msg, flush=True, end='\n' + end) - time.sleep(sleep_time) - - def success(self, msg, sleep_time=None, end=''): - if sleep_time is None: - sleep_time = self.sleep_time - msg = self.str_color(msg, style='success') - - print(msg, flush=True, end='\n' + end) - time.sleep(sleep_time) - - @staticmethod - def str_color(msg, style, surround=False): - if style == 'success': - style = COLORS.SUCCESS - elif style == 'fail': - style = COLORS.FAIL - elif style == 'prompt': - style = COLORS.PROMPT - elif style == 'info': - style = COLORS.INFO - elif style == 'cyan': - style = COLORS.CYAN - elif style == 'warning': - style = COLORS.WARNING - elif isinstance(style, int): - style = COLORS.BASE(style) - - if surround: - msg = '{}--------\n{}\n{}--------{}'.format(style, msg, COLORS.ENDC + style, COLORS.ENDC) - else: - msg = '{}{}{}'.format(style, msg, COLORS.ENDC) - - return msg - - def input_with_options(self, options, default=None): - """ - Takes in a list of options and asks user to make a choice. - The most similar option list index is returned along with the similarity percentage from 0 to 1 - """ - user_input = input('[{}]: '.format('/'.join(options))).lower().strip() - if not user_input: - return default, 0.0 - sims = [self.str_sim(i.lower().strip(), user_input) for i in options] - argmax = sims.index(max(sims)) - return argmax, sims[argmax] - - def str_eval(self, dat): - dat = dat.strip() - try: - dat = ast.literal_eval(dat) - except: - if dat.lower() == 'none': - dat = None - elif dat.lower() == 'false': - dat = False - elif dat.lower() == 'true': # else, assume string - dat = True - return dat - - -opEdit() From efcd55e99fd84a65a7102848d4206ad77719494b Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 18 Oct 2021 01:58:21 -0500 Subject: [PATCH 18/30] clean up --- common/colors.py | 36 ---- common/op_params.py | 186 ------------------ .../controls/lib/longitudinal_planner.py | 3 - 3 files changed, 225 deletions(-) delete mode 100644 common/colors.py delete mode 100644 common/op_params.py diff --git a/common/colors.py b/common/colors.py deleted file mode 100644 index 95aab123a4dc44..00000000000000 --- a/common/colors.py +++ /dev/null @@ -1,36 +0,0 @@ -class COLORS: - def __init__(self): - self.HEADER = '\033[95m' - self.OKBLUE = '\033[94m' - self.CBLUE = '\33[44m' - self.BOLD = '\033[1m' - self.CITALIC = '\33[3m' - self.OKGREEN = '\033[92m' - self.CWHITE = '\33[37m' - self.ENDC = '\033[0m' + self.CWHITE - self.UNDERLINE = '\033[4m' - self.PINK = '\33[38;5;207m' - self.PRETTY_YELLOW = self.BASE(220) - - self.RED = '\033[91m' - self.PURPLE_BG = '\33[45m' - self.YELLOW = '\033[93m' - self.BLUE_GREEN = self.BASE(85) - - self.FAIL = self.RED - # self.INFO = self.PURPLE_BG - self.INFO = self.BASE(207) - self.SUCCESS = self.OKGREEN - self.PROMPT = self.YELLOW - self.DBLUE = '\033[36m' - self.CYAN = self.BASE(39) - self.WARNING = '\033[33m' - - def BASE(self, col): # seems to support more colors - return '\33[38;5;{}m'.format(col) - - def BASEBG(self, col): # seems to support more colors - return '\33[48;5;{}m'.format(col) - - -COLORS = COLORS() diff --git a/common/op_params.py b/common/op_params.py deleted file mode 100644 index 326753ffd6e2fc..00000000000000 --- a/common/op_params.py +++ /dev/null @@ -1,186 +0,0 @@ -#!/usr/bin/env python3 -import os -import json -from atomicwrites import atomic_write -from common.colors import COLORS -from common.basedir import BASEDIR -from selfdrive.hardware import TICI -try: - from common.realtime import sec_since_boot -except ImportError: - import time - sec_since_boot = time.time - -warning = lambda msg: print('{}opParams WARNING: {}{}'.format(COLORS.WARNING, msg, COLORS.ENDC)) -error = lambda msg: print('{}opParams ERROR: {}{}'.format(COLORS.FAIL, msg, COLORS.ENDC)) - -NUMBER = [float, int] # value types -NONE_OR_NUMBER = [type(None), float, int] - -BASEDIR = os.path.dirname(BASEDIR) -PARAMS_DIR = os.path.join(BASEDIR, 'community', 'params') -IMPORTED_PATH = os.path.join(PARAMS_DIR, '.imported') -OLD_PARAMS_FILE = os.path.join(BASEDIR, 'op_params.json') - - -class Param: - def __init__(self, default, allowed_types=[], description=None, *, static=False, live=False, hidden=False): # pylint: disable=dangerous-default-value - self.default_value = default # value first saved and returned if actual value isn't a valid type - if not isinstance(allowed_types, list): - allowed_types = [allowed_types] - self.allowed_types = allowed_types # allowed python value types for opEdit - self.description = description # description to be shown in opEdit - self.hidden = hidden # hide this param to user in opEdit - self.live = live # show under the live menu in opEdit - self.static = static # use cached value, never reads to update - self._create_attrs() - - def is_valid(self, value): - if not self.has_allowed_types: # always valid if no allowed types, otherwise checks to make sure - return True - return type(value) in self.allowed_types - - def _create_attrs(self): # Create attributes and check Param is valid - self.has_allowed_types = isinstance(self.allowed_types, list) and len(self.allowed_types) > 0 - self.has_description = self.description is not None - self.is_list = list in self.allowed_types - self.read_frequency = None if self.static else (1 if self.live else 10) # how often to read param file (sec) - self.last_read = -1 - if self.has_allowed_types: - assert type(self.default_value) in self.allowed_types, 'Default value type must be in specified allowed_types!' - if self.is_list: - self.allowed_types.remove(list) - - -def _read_param(key): # Returns None, False if a json error occurs - try: - with open(os.path.join(PARAMS_DIR, key), 'r') as f: - value = json.loads(f.read()) - return value, True - except json.decoder.JSONDecodeError: - return None, False - - -def _write_param(key, value): - param_path = os.path.join(PARAMS_DIR, key) - with atomic_write(param_path, overwrite=True) as f: - f.write(json.dumps(value)) - os.chmod(param_path, 0o666) - - -def _import_params(): - if os.path.exists(OLD_PARAMS_FILE) and not os.path.exists(IMPORTED_PATH): # if opParams needs to import from old params file - try: - with open(OLD_PARAMS_FILE, 'r') as f: - old_params = json.loads(f.read()) - for key in old_params: - _write_param(key, old_params[key]) - open(IMPORTED_PATH, 'w').close() - except: # pylint: disable=bare-except - pass - - -class opParams: - def __init__(self): - """ - To add your own parameter to opParams in your fork, simply add a new entry in self.fork_params, instancing a new Param class with at minimum a default value. - The allowed_types and description args are not required but highly recommended to help users edit their parameters with opEdit safely. - - The description value will be shown to users when they use opEdit to change the value of the parameter. - - The allowed_types arg is used to restrict what kinds of values can be entered with opEdit so that users can't crash openpilot with unintended behavior. - (setting a param intended to be a number with a boolean, or viceversa for example) - Limiting the range of floats or integers is still recommended when `.get`ting the parameter. - When a None value is allowed, use `type(None)` instead of None, as opEdit checks the type against the values in the arg with `isinstance()`. - - If you want your param to update within a second, specify live=True. If your param is designed to be read once, specify static=True. - Specifying neither will have the param update every 10 seconds if constantly .get() - If the param is not static, call the .get() function on it in the update function of the file you're reading from to use live updating - - Here's an example of a good fork_param entry: - self.fork_params = {'camera_offset': Param(0.06, allowed_types=NUMBER), live=True} # NUMBER allows both floats and ints - """ - - self.fork_params = {'TR': Param(1.8, NUMBER, live=True)} - - self._to_delete = [] # a list of unused params you want to delete from users' params file - self._to_reset = [] # a list of params you want reset to their default values - self._run_init() # restores, reads, and updates params - - def _run_init(self): # does first time initializing of default params - # Two required parameters for opEdit - self.fork_params['username'] = Param(None, [type(None), str, bool], 'Your identifier provided with any crash logs sent to Sentry.\nHelps the developer reach out to you if anything goes wrong') - self.fork_params['op_edit_live_mode'] = Param(False, bool, 'This parameter controls which mode opEdit starts in', hidden=True) - - self.params = self._load_params(can_import=True) - self._add_default_params() # adds missing params and resets values with invalid types to self.params - self._delete_and_reset() # removes old params - - def get(self, key=None, *, force_update=False): # key=None returns dict of all params - if key is None: - return self._get_all_params(to_update=force_update) - self._check_key_exists(key, 'get') - param_info = self.fork_params[key] - rate = param_info.read_frequency # will be None if param is static, so check below - - if (not param_info.static and sec_since_boot() - self.fork_params[key].last_read >= rate) or force_update: - value, success = _read_param(key) - self.fork_params[key].last_read = sec_since_boot() - if not success: # in case of read error, use default and overwrite param - value = param_info.default_value - _write_param(key, value) - self.params[key] = value - - if param_info.is_valid(value := self.params[key]): - return value # all good, returning user's value - print(warning('User\'s value type is not valid! Returning default')) # somehow... it should always be valid - return param_info.default_value # return default value because user's value of key is not in allowed_types to avoid crashing openpilot - - def put(self, key, value): - self._check_key_exists(key, 'put') - if not self.fork_params[key].is_valid(value): - raise Exception('opParams: Tried to put a value of invalid type!') - self.params.update({key: value}) - _write_param(key, value) - - def _load_params(self, can_import=False): - if not os.path.exists(PARAMS_DIR): - os.makedirs(PARAMS_DIR) - if can_import: - _import_params() # just imports old params. below we read them in - - params = {} - for key in os.listdir(PARAMS_DIR): # PARAMS_DIR is guaranteed to exist - if key.startswith('.') or key not in self.fork_params: - continue - value, success = _read_param(key) - if not success: - value = self.fork_params[key].default_value - _write_param(key, value) - params[key] = value - return params - - def _get_all_params(self, to_update=False): - if to_update: - self.params = self._load_params() - return {k: self.params[k] for k, p in self.fork_params.items() if k in self.params and not p.hidden} - - def _check_key_exists(self, key, met): - if key not in self.fork_params: - raise Exception('opParams: Tried to {} an unknown parameter! Key not in fork_params: {}'.format(met, key)) - - def _add_default_params(self): - for key, param in self.fork_params.items(): - if key not in self.params: - self.params[key] = param.default_value - _write_param(key, self.params[key]) - elif not param.is_valid(self.params[key]): - print(warning('Value type of user\'s {} param not in allowed types, replacing with default!'.format(key))) - self.params[key] = param.default_value - _write_param(key, self.params[key]) - - def _delete_and_reset(self): - for key in list(self.params): - if key in self._to_delete: - del self.params[key] - os.remove(os.path.join(PARAMS_DIR, key)) - elif key in self._to_reset and key in self.fork_params: - self.params[key] = self.fork_params[key].default_value - _write_param(key, self.params[key]) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 59cf4790ca9003..051a68a7415df9 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -12,7 +12,6 @@ from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N from selfdrive.swaglog import cloudlog -from common.op_params import opParams LON_MPC_STEP = 0.2 # first step is 0.2s AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted @@ -46,7 +45,6 @@ class Planner(): def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP self.mpc = LongitudinalMpc() - self.op_params = opParams() self.fcw = False @@ -88,7 +86,6 @@ def update(self, sm, CP): # clip limits, cannot init MPC outside of bounds accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.desired_TR = self.op_params.get('TR') self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired, self.a_desired) self.mpc.update(sm['carState'], sm['radarState'], v_cruise) From d4304d3099f9fa882bd07aa73aaa9a46e9abbc0d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 18 Oct 2021 03:26:29 -0500 Subject: [PATCH 19/30] add x_ego cost tuning --- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 14 +++++++++++--- selfdrive/controls/lib/longitudinal_planner.py | 2 +- .../controls/tests/test_following_distance.py | 1 - 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 7fd945262e9afb..97be98b92952a6 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -190,12 +190,12 @@ def gen_long_mpc_solver(): class LongitudinalMpc(): def __init__(self, e2e=False, desired_TR=T_REACT): self.e2e = e2e + self.desired_TR = desired_TR self.reset() self.accel_limit_arr = np.zeros((N+1, 2)) self.accel_limit_arr[:,0] = -1.2 self.accel_limit_arr[:,1] = 1.2 self.source = SOURCES[2] - self.desired_TR = desired_TR def reset(self): self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR) @@ -225,8 +225,16 @@ def set_weights(self): self.set_weights_for_lead_policy() def set_weights_for_lead_policy(self): - # TODO: tune x_ego cost based on TR - W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, J_EGO_COST])) + # WARNING: deceleration tests with these costs: + # 0.9 TR gives FCW at 2 m/s/s test and fails at 3 m/s/s test + # 1.2 TR fails at 3 m/s/s test + # 1.4 TR gives FCW at 3 m/s/s test and fails at 3+ m/s/s test + # 1.6 TR succeeds at 3+ m/s/s test without FCW + TRs = [0.9, 1.8, 2.7] + mults = [10.0, 1.0, 0.1] + x_ego_mult = interp(self.desired_TR, TRs, mults) + + W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST * x_ego_mult, V_EGO_COST, A_EGO_COST, J_EGO_COST])) for i in range(N): self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 051a68a7415df9..693d95083e75d8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -44,7 +44,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class Planner(): def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP - self.mpc = LongitudinalMpc() + self.mpc = LongitudinalMpc(desired_TR=1.8) self.fcw = False diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 58f444b94fcdff..a289381199c198 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -29,7 +29,6 @@ def test_following_distanc(self): simulation_steady_state = run_following_distance_simulation(v_lead) correct_steady_state = desired_follow_distance(v_lead, v_lead, 0.9) - print(simulation_steady_state / speed, correct_steady_state / speed) self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + .3)) From 73e53d4a30223677382a88c557e20f31162c114f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 18 Oct 2021 03:28:37 -0500 Subject: [PATCH 20/30] revert --- selfdrive/controls/tests/test_alerts.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 selfdrive/controls/tests/test_alerts.py diff --git a/selfdrive/controls/tests/test_alerts.py b/selfdrive/controls/tests/test_alerts.py old mode 100644 new mode 100755 From e315b135132f01261c3f6e7db65b1ff0ca14d0b0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 18 Oct 2021 03:30:42 -0500 Subject: [PATCH 21/30] uncomment --- selfdrive/controls/tests/test_following_distance.py | 2 +- selfdrive/test/longitudinal_maneuvers/plant.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index a289381199c198..8cfa1d38011b16 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -27,7 +27,7 @@ def test_following_distanc(self): v_lead = float(speed) simulation_steady_state = run_following_distance_simulation(v_lead) - correct_steady_state = desired_follow_distance(v_lead, v_lead, 0.9) + correct_steady_state = desired_follow_distance(v_lead, v_lead, 1.8) self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + .3)) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 9fb153902c9dc1..ac19d2754010c3 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -119,9 +119,9 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): v_rel = 0. # print at 5hz - # if (self.rk.frame % (self.rate // 5)) == 0: - # print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s" - # % (self.current_time(), self.distance, self.speed, self.acceleration, d_rel, v_rel)) + if (self.rk.frame % (self.rate // 5)) == 0: + print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s" + % (self.current_time(), self.distance, self.speed, self.acceleration, d_rel, v_rel)) # ******** update prevs ******** From 4c956736e714a16f2fcd32a979d7532e970c147f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 18 Oct 2021 03:31:18 -0500 Subject: [PATCH 22/30] clean up --- selfdrive/controls/tests/test_following_distance.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 8cfa1d38011b16..5b3fa6522841d9 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -27,7 +27,7 @@ def test_following_distanc(self): v_lead = float(speed) simulation_steady_state = run_following_distance_simulation(v_lead) - correct_steady_state = desired_follow_distance(v_lead, v_lead, 1.8) + correct_steady_state = desired_follow_distance(v_lead, v_lead) self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + .3)) From 2b5715f3d64bddac333ae3cce60c105036ec9ebd Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 19 Oct 2021 03:48:07 -0500 Subject: [PATCH 23/30] consistent name --- .../lib/longitudinal_mpc_lib/long_mpc.py | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 97be98b92952a6..d184a30c4f0298 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -85,8 +85,8 @@ def gen_long_model(): a_min = SX.sym('a_min') a_max = SX.sym('a_max') x_obstacle = SX.sym('x_obstacle') - desired_TR = SX.sym('desired_TR') - model.p = vertcat(a_min, a_max, x_obstacle, desired_TR) + t_react = SX.sym('t_react') + model.p = vertcat(a_min, a_max, x_obstacle, t_react) # dynamics model f_expl = vertcat(v_ego, a_ego, j_ego) @@ -119,12 +119,12 @@ def gen_long_mpc_solver(): a_min, a_max = ocp.model.p[0], ocp.model.p[1] x_obstacle = ocp.model.p[2] - desired_TR = ocp.model.p[3] + t_react = ocp.model.p[3] ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - desired_dist_comfort = get_safe_obstacle_distance(v_ego, desired_TR) + desired_dist_comfort = get_safe_obstacle_distance(v_ego, t_react) # The main cost in normal operation is how close you are to the "desired" distance # from an obstacle at every timestep. This obstacle can be a lead car @@ -188,9 +188,9 @@ def gen_long_mpc_solver(): class LongitudinalMpc(): - def __init__(self, e2e=False, desired_TR=T_REACT): + def __init__(self, e2e=False, t_react=T_REACT): self.e2e = e2e - self.desired_TR = desired_TR + self.t_react = t_react self.reset() self.accel_limit_arr = np.zeros((N+1, 2)) self.accel_limit_arr[:,0] = -1.2 @@ -232,7 +232,7 @@ def set_weights_for_lead_policy(self): # 1.6 TR succeeds at 3+ m/s/s test without FCW TRs = [0.9, 1.8, 2.7] mults = [10.0, 1.0, 0.1] - x_ego_mult = interp(self.desired_TR, TRs, mults) + x_ego_mult = interp(self.t_react, TRs, mults) W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST * x_ego_mult, V_EGO_COST, A_EGO_COST, J_EGO_COST])) for i in range(N): @@ -303,6 +303,9 @@ def set_accel_limits(self, min_a, max_a): self.cruise_min_a = min_a self.cruise_max_a = max_a + def set_t_react(self, t_react): + self.t_react = t_react + def update(self, carstate, radarstate, v_cruise): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status @@ -317,8 +320,8 @@ def update(self, carstate, radarstate, v_cruise): # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. - lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], self.desired_TR) - lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.desired_TR) + lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], self.t_react) + lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.t_react) # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. @@ -327,12 +330,12 @@ def update(self, carstate, radarstate, v_cruise): v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), cruise_lower_bound, cruise_upper_bound) - cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped, self.desired_TR) + cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped, self.t_react) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] self.params[:,2] = np.min(x_obstacles, axis=1) - self.params[:,3] = self.desired_TR + self.params[:,3] = self.t_react self.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and From a7f0b7ad8297c4f0e3405dbc4d2843e25a71f6a4 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 19 Oct 2021 03:55:33 -0500 Subject: [PATCH 24/30] clean up --- selfdrive/controls/lib/longitudinal_planner.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 693d95083e75d8..3d1e688723eeed 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -44,7 +44,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class Planner(): def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP - self.mpc = LongitudinalMpc(desired_TR=1.8) + self.mpc = LongitudinalMpc() self.fcw = False @@ -88,6 +88,7 @@ def update(self, sm, CP): accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired, self.a_desired) + # self.mpc.set_t_react(1.8) self.mpc.update(sm['carState'], sm['radarState'], v_cruise) self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution) From 6d73c0e930f8bf2d047c99ad11a2a176737fd11d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 19 Oct 2021 03:59:44 -0500 Subject: [PATCH 25/30] better to differentiate --- .../lib/longitudinal_mpc_lib/long_mpc.py | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index d184a30c4f0298..e227a30dd0ff89 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -85,8 +85,8 @@ def gen_long_model(): a_min = SX.sym('a_min') a_max = SX.sym('a_max') x_obstacle = SX.sym('x_obstacle') - t_react = SX.sym('t_react') - model.p = vertcat(a_min, a_max, x_obstacle, t_react) + desired_TR = SX.sym('desired_TR') + model.p = vertcat(a_min, a_max, x_obstacle, desired_TR) # dynamics model f_expl = vertcat(v_ego, a_ego, j_ego) @@ -119,12 +119,12 @@ def gen_long_mpc_solver(): a_min, a_max = ocp.model.p[0], ocp.model.p[1] x_obstacle = ocp.model.p[2] - t_react = ocp.model.p[3] + desired_TR = ocp.model.p[3] ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - desired_dist_comfort = get_safe_obstacle_distance(v_ego, t_react) + desired_dist_comfort = get_safe_obstacle_distance(v_ego, desired_TR) # The main cost in normal operation is how close you are to the "desired" distance # from an obstacle at every timestep. This obstacle can be a lead car @@ -188,9 +188,9 @@ def gen_long_mpc_solver(): class LongitudinalMpc(): - def __init__(self, e2e=False, t_react=T_REACT): + def __init__(self, e2e=False, desired_TR=T_REACT): self.e2e = e2e - self.t_react = t_react + self.desired_TR = desired_TR self.reset() self.accel_limit_arr = np.zeros((N+1, 2)) self.accel_limit_arr[:,0] = -1.2 @@ -232,7 +232,7 @@ def set_weights_for_lead_policy(self): # 1.6 TR succeeds at 3+ m/s/s test without FCW TRs = [0.9, 1.8, 2.7] mults = [10.0, 1.0, 0.1] - x_ego_mult = interp(self.t_react, TRs, mults) + x_ego_mult = interp(self.desired_TR, TRs, mults) W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST * x_ego_mult, V_EGO_COST, A_EGO_COST, J_EGO_COST])) for i in range(N): @@ -303,8 +303,8 @@ def set_accel_limits(self, min_a, max_a): self.cruise_min_a = min_a self.cruise_max_a = max_a - def set_t_react(self, t_react): - self.t_react = t_react + def set_desired_TR(self, desired_TR): + self.desired_TR = desired_TR def update(self, carstate, radarstate, v_cruise): v_ego = self.x0[1] @@ -320,8 +320,8 @@ def update(self, carstate, radarstate, v_cruise): # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. - lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], self.t_react) - lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.t_react) + lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], self.desired_TR) + lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.desired_TR) # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. @@ -330,12 +330,12 @@ def update(self, carstate, radarstate, v_cruise): v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), cruise_lower_bound, cruise_upper_bound) - cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped, self.t_react) + cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped, self.desired_TR) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] self.params[:,2] = np.min(x_obstacles, axis=1) - self.params[:,3] = self.t_react + self.params[:,3] = self.desired_TR self.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and From 05703aba5dc2dff1d6cbae2a010f8f9476f7a133 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 21 Oct 2021 01:21:33 -0500 Subject: [PATCH 26/30] update CamryH tune --- selfdrive/car/toyota/interface.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 643876af118a93..e497d59a7c75db 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -25,7 +25,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop - if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI + if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H, CAR.CAMRYH_TSS2]: # These cars use LQR/INDI ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] @@ -134,8 +134,18 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.init('indi') + ret.lateralTuning.indi.innerLoopGainBP = [20, 24, 30] + ret.lateralTuning.indi.innerLoopGainV = [7.25, 7.5, 9] + ret.lateralTuning.indi.outerLoopGainBP = [20, 24, 30] + ret.lateralTuning.indi.outerLoopGainV = [6, 7.25, 6] + ret.lateralTuning.indi.timeConstantBP = [20, 24] + ret.lateralTuning.indi.timeConstantV = [2.0, 2.2] + ret.lateralTuning.indi.actuatorEffectivenessBP = [20, 24] + ret.lateralTuning.indi.actuatorEffectivenessV = [2, 3] + ret.steerActuatorDelay = 0.3 + ret.steerRateCost = 1.25 + ret.steerLimitTimer = 0.5 elif candidate in [CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]: stop_and_go = True From e6ce5ba46520c4c20ef2a00b999f47c11efeb34e Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 21 Oct 2021 01:47:51 -0500 Subject: [PATCH 27/30] theoretically 0.9 seconds is safe up to -3 m/s/s lead braking --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index e227a30dd0ff89..a1db27f2f76005 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -231,10 +231,11 @@ def set_weights_for_lead_policy(self): # 1.4 TR gives FCW at 3 m/s/s test and fails at 3+ m/s/s test # 1.6 TR succeeds at 3+ m/s/s test without FCW TRs = [0.9, 1.8, 2.7] - mults = [10.0, 1.0, 0.1] - x_ego_mult = interp(self.desired_TR, TRs, mults) + x_ego_cost_multiplier = interp(self.desired_TR, TRs, [15., 1.0, 0.1]) + j_ego_cost_multiplier = interp(self.desired_TR, TRs, [0.5, 1.0, 1.0]) + d_zone_cost_multiplier = interp(self.desired_TR, TRs, [15., 1.0, 1.0]) - W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST * x_ego_mult, V_EGO_COST, A_EGO_COST, J_EGO_COST])) + W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST * x_ego_cost_multiplier, V_EGO_COST, A_EGO_COST, J_EGO_COST * j_ego_cost_multiplier])) for i in range(N): self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, @@ -242,7 +243,7 @@ def set_weights_for_lead_policy(self): self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM])) # Set L2 slack cost on lower bound constraints - Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]) + Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * d_zone_cost_multiplier]) for i in range(N): self.solver.cost_set(i, 'Zl', Zl) From c254ee091fdc2a057273a54f169fa36e080f49f6 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 21 Oct 2021 01:52:10 -0500 Subject: [PATCH 28/30] live tuning --- common/colors.py | 36 ++ common/op_params.py | 185 ++++++++++ op_edit.py | 325 ++++++++++++++++++ .../controls/lib/longitudinal_planner.py | 4 +- 4 files changed, 549 insertions(+), 1 deletion(-) create mode 100644 common/colors.py create mode 100644 common/op_params.py create mode 100755 op_edit.py diff --git a/common/colors.py b/common/colors.py new file mode 100644 index 00000000000000..95aab123a4dc44 --- /dev/null +++ b/common/colors.py @@ -0,0 +1,36 @@ +class COLORS: + def __init__(self): + self.HEADER = '\033[95m' + self.OKBLUE = '\033[94m' + self.CBLUE = '\33[44m' + self.BOLD = '\033[1m' + self.CITALIC = '\33[3m' + self.OKGREEN = '\033[92m' + self.CWHITE = '\33[37m' + self.ENDC = '\033[0m' + self.CWHITE + self.UNDERLINE = '\033[4m' + self.PINK = '\33[38;5;207m' + self.PRETTY_YELLOW = self.BASE(220) + + self.RED = '\033[91m' + self.PURPLE_BG = '\33[45m' + self.YELLOW = '\033[93m' + self.BLUE_GREEN = self.BASE(85) + + self.FAIL = self.RED + # self.INFO = self.PURPLE_BG + self.INFO = self.BASE(207) + self.SUCCESS = self.OKGREEN + self.PROMPT = self.YELLOW + self.DBLUE = '\033[36m' + self.CYAN = self.BASE(39) + self.WARNING = '\033[33m' + + def BASE(self, col): # seems to support more colors + return '\33[38;5;{}m'.format(col) + + def BASEBG(self, col): # seems to support more colors + return '\33[48;5;{}m'.format(col) + + +COLORS = COLORS() diff --git a/common/op_params.py b/common/op_params.py new file mode 100644 index 00000000000000..1e3779fad475cd --- /dev/null +++ b/common/op_params.py @@ -0,0 +1,185 @@ +#!/usr/bin/env python3 +import os +import json +from atomicwrites import atomic_write +from common.colors import COLORS +from common.basedir import BASEDIR +from selfdrive.hardware import TICI +try: + from common.realtime import sec_since_boot +except ImportError: + import time + sec_since_boot = time.time + +warning = lambda msg: print('{}opParams WARNING: {}{}'.format(COLORS.WARNING, msg, COLORS.ENDC)) +error = lambda msg: print('{}opParams ERROR: {}{}'.format(COLORS.FAIL, msg, COLORS.ENDC)) + +NUMBER = [float, int] # value types +NONE_OR_NUMBER = [type(None), float, int] + +BASEDIR = os.path.dirname(BASEDIR) +PARAMS_DIR = os.path.join(BASEDIR, 'community', 'params') +IMPORTED_PATH = os.path.join(PARAMS_DIR, '.imported') +OLD_PARAMS_FILE = os.path.join(BASEDIR, 'op_params.json') + + +class Param: + def __init__(self, default, allowed_types=[], description=None, *, static=False, live=False, hidden=False): # pylint: disable=dangerous-default-value + self.default_value = default # value first saved and returned if actual value isn't a valid type + if not isinstance(allowed_types, list): + allowed_types = [allowed_types] + self.allowed_types = allowed_types # allowed python value types for opEdit + self.description = description # description to be shown in opEdit + self.hidden = hidden # hide this param to user in opEdit + self.live = live # show under the live menu in opEdit + self.static = static # use cached value, never reads to update + self._create_attrs() + + def is_valid(self, value): + if not self.has_allowed_types: # always valid if no allowed types, otherwise checks to make sure + return True + return type(value) in self.allowed_types + + def _create_attrs(self): # Create attributes and check Param is valid + self.has_allowed_types = isinstance(self.allowed_types, list) and len(self.allowed_types) > 0 + self.has_description = self.description is not None + self.is_list = list in self.allowed_types + self.read_frequency = None if self.static else (1 if self.live else 10) # how often to read param file (sec) + self.last_read = -1 + if self.has_allowed_types: + assert type(self.default_value) in self.allowed_types, 'Default value type must be in specified allowed_types!' + if self.is_list: + self.allowed_types.remove(list) + + +def _read_param(key): # Returns None, False if a json error occurs + try: + with open(os.path.join(PARAMS_DIR, key), 'r') as f: + value = json.loads(f.read()) + return value, True + except json.decoder.JSONDecodeError: + return None, False + + +def _write_param(key, value): + param_path = os.path.join(PARAMS_DIR, key) + with atomic_write(param_path, overwrite=True) as f: + f.write(json.dumps(value)) + os.chmod(param_path, 0o666) + + +def _import_params(): + if os.path.exists(OLD_PARAMS_FILE) and not os.path.exists(IMPORTED_PATH): # if opParams needs to import from old params file + try: + with open(OLD_PARAMS_FILE, 'r') as f: + old_params = json.loads(f.read()) + for key in old_params: + _write_param(key, old_params[key]) + open(IMPORTED_PATH, 'w').close() + except: # pylint: disable=bare-except + pass + + +class opParams: + def __init__(self): + """ + To add your own parameter to opParams in your fork, simply add a new entry in self.fork_params, instancing a new Param class with at minimum a default value. + The allowed_types and description args are not required but highly recommended to help users edit their parameters with opEdit safely. + - The description value will be shown to users when they use opEdit to change the value of the parameter. + - The allowed_types arg is used to restrict what kinds of values can be entered with opEdit so that users can't crash openpilot with unintended behavior. + (setting a param intended to be a number with a boolean, or viceversa for example) + Limiting the range of floats or integers is still recommended when `.get`ting the parameter. + When a None value is allowed, use `type(None)` instead of None, as opEdit checks the type against the values in the arg with `isinstance()`. + - If you want your param to update within a second, specify live=True. If your param is designed to be read once, specify static=True. + Specifying neither will have the param update every 10 seconds if constantly .get() + If the param is not static, call the .get() function on it in the update function of the file you're reading from to use live updating + Here's an example of a good fork_param entry: + self.fork_params = {'camera_offset': Param(0.06, allowed_types=NUMBER), live=True} # NUMBER allows both floats and ints + """ + + self.fork_params = {'TR': Param(1.8, NUMBER, live=True)} + + self._to_delete = [] # a list of unused params you want to delete from users' params file + self._to_reset = [] # a list of params you want reset to their default values + self._run_init() # restores, reads, and updates params + + def _run_init(self): # does first time initializing of default params + # Two required parameters for opEdit + self.fork_params['username'] = Param(None, [type(None), str, bool], 'Your identifier provided with any crash logs sent to Sentry.\nHelps the developer reach out to you if anything goes wrong') + self.fork_params['op_edit_live_mode'] = Param(False, bool, 'This parameter controls which mode opEdit starts in', hidden=True) + + self.params = self._load_params(can_import=True) + self._add_default_params() # adds missing params and resets values with invalid types to self.params + self._delete_and_reset() # removes old params + + def get(self, key=None, *, force_update=False): # key=None returns dict of all params + if key is None: + return self._get_all_params(to_update=force_update) + self._check_key_exists(key, 'get') + param_info = self.fork_params[key] + rate = param_info.read_frequency # will be None if param is static, so check below + + if (not param_info.static and sec_since_boot() - self.fork_params[key].last_read >= rate) or force_update: + value, success = _read_param(key) + self.fork_params[key].last_read = sec_since_boot() + if not success: # in case of read error, use default and overwrite param + value = param_info.default_value + _write_param(key, value) + self.params[key] = value + + if param_info.is_valid(value := self.params[key]): + return value # all good, returning user's value + print(warning('User\'s value type is not valid! Returning default')) # somehow... it should always be valid + return param_info.default_value # return default value because user's value of key is not in allowed_types to avoid crashing openpilot + + def put(self, key, value): + self._check_key_exists(key, 'put') + if not self.fork_params[key].is_valid(value): + raise Exception('opParams: Tried to put a value of invalid type!') + self.params.update({key: value}) + _write_param(key, value) + + def _load_params(self, can_import=False): + if not os.path.exists(PARAMS_DIR): + os.makedirs(PARAMS_DIR) + if can_import: + _import_params() # just imports old params. below we read them in + + params = {} + for key in os.listdir(PARAMS_DIR): # PARAMS_DIR is guaranteed to exist + if key.startswith('.') or key not in self.fork_params: + continue + value, success = _read_param(key) + if not success: + value = self.fork_params[key].default_value + _write_param(key, value) + params[key] = value + return params + + def _get_all_params(self, to_update=False): + if to_update: + self.params = self._load_params() + return {k: self.params[k] for k, p in self.fork_params.items() if k in self.params and not p.hidden} + + def _check_key_exists(self, key, met): + if key not in self.fork_params: + raise Exception('opParams: Tried to {} an unknown parameter! Key not in fork_params: {}'.format(met, key)) + + def _add_default_params(self): + for key, param in self.fork_params.items(): + if key not in self.params: + self.params[key] = param.default_value + _write_param(key, self.params[key]) + elif not param.is_valid(self.params[key]): + print(warning('Value type of user\'s {} param not in allowed types, replacing with default!'.format(key))) + self.params[key] = param.default_value + _write_param(key, self.params[key]) + + def _delete_and_reset(self): + for key in list(self.params): + if key in self._to_delete: + del self.params[key] + os.remove(os.path.join(PARAMS_DIR, key)) + elif key in self._to_reset and key in self.fork_params: + self.params[key] = self.fork_params[key].default_value + _write_param(key, self.params[key]) diff --git a/op_edit.py b/op_edit.py new file mode 100755 index 00000000000000..212baad7708516 --- /dev/null +++ b/op_edit.py @@ -0,0 +1,325 @@ +#!/usr/bin/env python3 +import time +from common.op_params import opParams +import ast +import difflib +from common.colors import COLORS + + +class opEdit: # use by running `python /data/openpilot/op_edit.py` + def __init__(self): + self.op_params = opParams() + self.params = None + self.sleep_time = 0.5 + self.live_tuning = self.op_params.get('op_edit_live_mode') + self.username = self.op_params.get('username') + self.type_colors = {int: COLORS.BASE(179), float: COLORS.BASE(179), + bool: {False: COLORS.RED, True: COLORS.OKGREEN}, + type(None): COLORS.BASE(177), + str: COLORS.BASE(77)} + + self.last_choice = None + + self.run_init() + + def run_init(self): + if self.username is None: + self.success('\nWelcome to the {}opParams{} command line editor!'.format(COLORS.CYAN, COLORS.SUCCESS), sleep_time=0) + self.prompt('Would you like to add your Discord username for easier crash debugging for the fork owner?') + self.prompt('Your username is only used for reaching out if a crash occurs.') + + username_choice = self.input_with_options(['Y', 'N', 'don\'t ask again'], default='n')[0] + if username_choice == 0: + self.prompt('Enter a unique identifer/Discord username:') + username = '' + while username == '': + username = input('>> ').strip() + self.op_params.put('username', username) + self.username = username + self.success('Thanks! Saved your username\n' + 'Edit the \'username\' parameter at any time to update', sleep_time=1.5) + elif username_choice == 2: + self.op_params.put('username', False) + self.info('Got it, bringing you into opEdit\n' + 'Edit the \'username\' parameter at any time to update', sleep_time=1.0) + else: + self.success('\nWelcome to the {}opParams{} command line editor, {}!'.format(COLORS.CYAN, COLORS.SUCCESS, self.username), sleep_time=0) + + self.run_loop() + + def run_loop(self): + while True: + if not self.live_tuning: + self.info('Here are all your parameters:', sleep_time=0) + self.info('(non-static params update while driving)', end='\n', sleep_time=0) + else: + self.info('Here are your live parameters:', sleep_time=0) + self.info('(changes take effect within a second)', end='\n', sleep_time=0) + self.params = self.op_params.get(force_update=True) + if self.live_tuning: # only display live tunable params + self.params = {k: v for k, v in self.params.items() if self.op_params.fork_params[k].live} + + values_list = [] + for k, v in self.params.items(): + if len(str(v)) < 20: + v = self.color_from_type(v) + else: + v = '{} ... {}'.format(str(v)[:30], str(v)[-15:]) + values_list.append(v) + + static = [COLORS.INFO + '(static)' + COLORS.ENDC if self.op_params.fork_params[k].static else '' for k in self.params] + + to_print = [] + blue_gradient = [33, 39, 45, 51, 87] + for idx, param in enumerate(self.params): + line = '{}. {}: {} {}'.format(idx + 1, param, values_list[idx], static[idx]) + if idx == self.last_choice and self.last_choice is not None: + line = COLORS.OKGREEN + line + else: + _color = blue_gradient[min(round(idx / len(self.params) * len(blue_gradient)), len(blue_gradient) - 1)] + line = COLORS.BASE(_color) + line + to_print.append(line) + + extras = {'l': ('Toggle live params', COLORS.WARNING), + 'e': ('Exit opEdit', COLORS.PINK)} + + to_print += ['---'] + ['{}. {}'.format(ext_col + e, ext_txt + COLORS.ENDC) for e, (ext_txt, ext_col) in extras.items()] + print('\n'.join(to_print)) + self.prompt('\nChoose a parameter to edit (by index or name):') + + choice = input('>> ').strip().lower() + parsed, choice = self.parse_choice(choice, len(to_print) - len(extras)) + if parsed == 'continue': + continue + elif parsed == 'change': + self.last_choice = choice + self.change_parameter(choice) + elif parsed == 'live': + self.last_choice = None + self.live_tuning = not self.live_tuning + self.op_params.put('op_edit_live_mode', self.live_tuning) # for next opEdit startup + elif parsed == 'exit': + return + + def parse_choice(self, choice, opt_len): + if choice.isdigit(): + choice = int(choice) + choice -= 1 + if choice not in range(opt_len): # number of options to choose from + self.error('Not in range!') + return 'continue', choice + return 'change', choice + + if choice in ['l', 'live']: # live tuning mode + return 'live', choice + elif choice in ['exit', 'e', '']: + self.error('Exiting opEdit!', sleep_time=0) + return 'exit', choice + else: # find most similar param to user's input + param_sims = [(idx, self.str_sim(choice, param.lower())) for idx, param in enumerate(self.params)] + param_sims = [param for param in param_sims if param[1] > 0.33] + if len(param_sims) > 0: + chosen_param = sorted(param_sims, key=lambda param: param[1], reverse=True)[0] + return 'change', chosen_param[0] # return idx + + self.error('Invalid choice!') + return 'continue', choice + + def str_sim(self, a, b): + return difflib.SequenceMatcher(a=a, b=b).ratio() + + def change_parameter(self, choice): + while True: + chosen_key = list(self.params)[choice] + param_info = self.op_params.fork_params[chosen_key] + + old_value = self.params[chosen_key] + if not param_info.static: + self.info2('Chosen parameter: {}{} (live!)'.format(chosen_key, COLORS.BASE(207)), sleep_time=0) + else: + self.info2('Chosen parameter: {}{} (static)'.format(chosen_key, COLORS.BASE(207)), sleep_time=0) + + to_print = [] + if param_info.has_description: + to_print.append(COLORS.OKGREEN + '>> Description: {}'.format(param_info.description.replace('\n', '\n > ')) + COLORS.ENDC) + if param_info.static: + to_print.append(COLORS.WARNING + '>> A reboot is required for changes to this parameter!' + COLORS.ENDC) + if not param_info.static and not param_info.live: + to_print.append(COLORS.WARNING + '>> Changes take effect within 10 seconds for this parameter!' + COLORS.ENDC) + if param_info.has_allowed_types: + to_print.append(COLORS.RED + '>> Allowed types: {}'.format(', '.join([at.__name__ for at in param_info.allowed_types])) + COLORS.ENDC) + to_print.append(COLORS.WARNING + '>> Default value: {}'.format(self.color_from_type(param_info.default_value)) + COLORS.ENDC) + + if to_print: + print('\n{}\n'.format('\n'.join(to_print))) + + if param_info.is_list: + self.change_param_list(old_value, param_info, chosen_key) # TODO: need to merge the code in this function with the below to reduce redundant code + return + + self.info('Current value: {}{} (type: {})'.format(self.color_from_type(old_value), COLORS.INFO, type(old_value).__name__), sleep_time=0) + + while True: + self.prompt('\nEnter your new value (enter to exit):') + new_value = input('>> ').strip() + if new_value == '': + self.info('Exiting this parameter...\n') + return + + new_value = self.str_eval(new_value) + if not param_info.is_valid(new_value): + self.error('The type of data you entered ({}) is not allowed with this parameter!'.format(type(new_value).__name__)) + continue + + if not param_info.static: # stay in live tuning interface + self.op_params.put(chosen_key, new_value) + self.success('Saved {} with value: {}{}! (type: {})'.format(chosen_key, self.color_from_type(new_value), COLORS.SUCCESS, type(new_value).__name__)) + else: # else ask to save and break + self.warning('\nOld value: {}{} (type: {})'.format(self.color_from_type(old_value), COLORS.WARNING, type(old_value).__name__)) + self.success('New value: {}{} (type: {})'.format(self.color_from_type(new_value), COLORS.OKGREEN, type(new_value).__name__), sleep_time=0) + self.prompt('\nDo you want to save this?') + if self.input_with_options(['Y', 'N'], 'N')[0] == 0: + self.op_params.put(chosen_key, new_value) + self.success('Saved!') + else: + self.info('Not saved!') + return + + def change_param_list(self, old_value, param_info, chosen_key): + while True: + self.info('Current value: {} (type: {})'.format(old_value, type(old_value).__name__), sleep_time=0) + self.prompt('\nEnter index to edit (0 to {}):'.format(len(old_value) - 1)) + choice_idx = self.str_eval(input('>> ')) + if choice_idx == '': + self.info('Exiting this parameter...') + return + + if not isinstance(choice_idx, int) or choice_idx not in range(len(old_value)): + self.error('Must be an integar within list range!') + continue + + while True: + self.info('Chosen index: {}'.format(choice_idx), sleep_time=0) + self.info('Value: {} (type: {})'.format(old_value[choice_idx], type(old_value[choice_idx]).__name__), sleep_time=0) + self.prompt('\nEnter your new value:') + new_value = input('>> ').strip() + if new_value == '': + self.info('Exiting this list item...') + break + + new_value = self.str_eval(new_value) + if not param_info.is_valid(new_value): + self.error('The type of data you entered ({}) is not allowed with this parameter!'.format(type(new_value).__name__)) + continue + + old_value[choice_idx] = new_value + + self.op_params.put(chosen_key, old_value) + self.success('Saved {} with value: {}{}! (type: {})'.format(chosen_key, self.color_from_type(new_value), COLORS.SUCCESS, type(new_value).__name__), end='\n') + break + + def color_from_type(self, v): + v_color = '' + if type(v) in self.type_colors: + v_color = self.type_colors[type(v)] + if isinstance(v, bool): + v_color = v_color[v] + v = '{}{}{}'.format(v_color, v, COLORS.ENDC) + return v + + def cyan(self, msg, end=''): + msg = self.str_color(msg, style='cyan') + # print(msg, flush=True, end='\n' + end) + return msg + + def prompt(self, msg, end=''): + msg = self.str_color(msg, style='prompt') + print(msg, flush=True, end='\n' + end) + + def warning(self, msg, end=''): + msg = self.str_color(msg, style='warning') + print(msg, flush=True, end='\n' + end) + + def info(self, msg, sleep_time=None, end=''): + if sleep_time is None: + sleep_time = self.sleep_time + msg = self.str_color(msg, style='info') + + print(msg, flush=True, end='\n' + end) + time.sleep(sleep_time) + + def info2(self, msg, sleep_time=None, end=''): + if sleep_time is None: + sleep_time = self.sleep_time + msg = self.str_color(msg, style=86) + + print(msg, flush=True, end='\n' + end) + time.sleep(sleep_time) + + def error(self, msg, sleep_time=None, end='', surround=True): + if sleep_time is None: + sleep_time = self.sleep_time + msg = self.str_color(msg, style='fail', surround=surround) + + print(msg, flush=True, end='\n' + end) + time.sleep(sleep_time) + + def success(self, msg, sleep_time=None, end=''): + if sleep_time is None: + sleep_time = self.sleep_time + msg = self.str_color(msg, style='success') + + print(msg, flush=True, end='\n' + end) + time.sleep(sleep_time) + + @staticmethod + def str_color(msg, style, surround=False): + if style == 'success': + style = COLORS.SUCCESS + elif style == 'fail': + style = COLORS.FAIL + elif style == 'prompt': + style = COLORS.PROMPT + elif style == 'info': + style = COLORS.INFO + elif style == 'cyan': + style = COLORS.CYAN + elif style == 'warning': + style = COLORS.WARNING + elif isinstance(style, int): + style = COLORS.BASE(style) + + if surround: + msg = '{}--------\n{}\n{}--------{}'.format(style, msg, COLORS.ENDC + style, COLORS.ENDC) + else: + msg = '{}{}{}'.format(style, msg, COLORS.ENDC) + + return msg + + def input_with_options(self, options, default=None): + """ + Takes in a list of options and asks user to make a choice. + The most similar option list index is returned along with the similarity percentage from 0 to 1 + """ + user_input = input('[{}]: '.format('/'.join(options))).lower().strip() + if not user_input: + return default, 0.0 + sims = [self.str_sim(i.lower().strip(), user_input) for i in options] + argmax = sims.index(max(sims)) + return argmax, sims[argmax] + + def str_eval(self, dat): + dat = dat.strip() + try: + dat = ast.literal_eval(dat) + except: + if dat.lower() == 'none': + dat = None + elif dat.lower() == 'false': + dat = False + elif dat.lower() == 'true': # else, assume string + dat = True + return dat + + +opEdit() diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 3d1e688723eeed..88505b7b494357 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -12,6 +12,7 @@ from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N from selfdrive.swaglog import cloudlog +from common.op_params import opParams LON_MPC_STEP = 0.2 # first step is 0.2s AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted @@ -45,6 +46,7 @@ class Planner(): def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP self.mpc = LongitudinalMpc() + self.op_params = opParams() self.fcw = False @@ -88,7 +90,7 @@ def update(self, sm, CP): accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired, self.a_desired) - # self.mpc.set_t_react(1.8) + self.mpc.set_desired_TR(self.op_params.get('TR')) self.mpc.update(sm['carState'], sm['radarState'], v_cruise) self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution) From 5224783bdd65fec96deeed027d4238ee06270d94 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 21 Oct 2021 18:22:21 -0500 Subject: [PATCH 29/30] update tune, rate limits not permanent --- selfdrive/car/toyota/interface.py | 16 +++------------- selfdrive/car/toyota/values.py | 4 ++-- 2 files changed, 5 insertions(+), 15 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index e497d59a7c75db..4bf45c760dc873 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -25,7 +25,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop - if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H, CAR.CAMRYH_TSS2]: # These cars use LQR/INDI + if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] @@ -134,18 +134,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGainBP = [20, 24, 30] - ret.lateralTuning.indi.innerLoopGainV = [7.25, 7.5, 9] - ret.lateralTuning.indi.outerLoopGainBP = [20, 24, 30] - ret.lateralTuning.indi.outerLoopGainV = [6, 7.25, 6] - ret.lateralTuning.indi.timeConstantBP = [20, 24] - ret.lateralTuning.indi.timeConstantV = [2.0, 2.2] - ret.lateralTuning.indi.actuatorEffectivenessBP = [20, 24] - ret.lateralTuning.indi.actuatorEffectivenessV = [2, 3] - ret.steerActuatorDelay = 0.3 - ret.steerRateCost = 1.25 - ret.steerLimitTimer = 0.5 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.4], [0.1]] + ret.lateralTuning.pid.kf = 0.00006 elif candidate in [CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]: stop_and_go = True diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 0cd75d8723b205..925cd7d14da395 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -16,9 +16,9 @@ class CarControllerParams: ACCEL_MIN = -3.5 # m/s2 STEER_MAX = 1500 - STEER_DELTA_UP = 10 # 1.5s time to peak torque + STEER_DELTA_UP = 12 # 1.5s time to peak torque STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) - STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + STEER_ERROR_MAX = 450 # max delta between torque cmd and torque motor class CAR: # Toyota From 0c32b716afcc2bc79b6c94339b003b127be70f00 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 2 Nov 2021 01:52:03 -0500 Subject: [PATCH 30/30] Revert "update tune, rate limits not permanent" This reverts commit 806ffdf5ddf43e47a43e8f7228bde1210ee9e596. --- selfdrive/car/toyota/interface.py | 16 +++++++++++++--- selfdrive/car/toyota/values.py | 4 ++-- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 4bf45c760dc873..e497d59a7c75db 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -25,7 +25,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop - if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI + if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H, CAR.CAMRYH_TSS2]: # These cars use LQR/INDI ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] @@ -134,8 +134,18 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.4], [0.1]] - ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.init('indi') + ret.lateralTuning.indi.innerLoopGainBP = [20, 24, 30] + ret.lateralTuning.indi.innerLoopGainV = [7.25, 7.5, 9] + ret.lateralTuning.indi.outerLoopGainBP = [20, 24, 30] + ret.lateralTuning.indi.outerLoopGainV = [6, 7.25, 6] + ret.lateralTuning.indi.timeConstantBP = [20, 24] + ret.lateralTuning.indi.timeConstantV = [2.0, 2.2] + ret.lateralTuning.indi.actuatorEffectivenessBP = [20, 24] + ret.lateralTuning.indi.actuatorEffectivenessV = [2, 3] + ret.steerActuatorDelay = 0.3 + ret.steerRateCost = 1.25 + ret.steerLimitTimer = 0.5 elif candidate in [CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]: stop_and_go = True diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 925cd7d14da395..0cd75d8723b205 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -16,9 +16,9 @@ class CarControllerParams: ACCEL_MIN = -3.5 # m/s2 STEER_MAX = 1500 - STEER_DELTA_UP = 12 # 1.5s time to peak torque + STEER_DELTA_UP = 10 # 1.5s time to peak torque STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) - STEER_ERROR_MAX = 450 # max delta between torque cmd and torque motor + STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor class CAR: # Toyota