From 432d7b4f98809844b027afb49488ed336a7862c3 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 10 Mar 2020 03:07:31 -0500 Subject: [PATCH 1/9] Add dynamic follow, need to find a solution to allow users to choose profiles without op_params --- common/op_params.py | 187 ++++++++++++++++++ common/travis_checker.py | 2 + selfdrive/controls/lib/long_mpc.py | 130 +++++++++++- .../lib_mpc_export/acado_solver.c | 45 +++-- .../lib/longitudinal_mpc/libmpc_py.py | 3 +- .../lib/longitudinal_mpc/longitudinal_mpc.c | 27 ++- 6 files changed, 364 insertions(+), 30 deletions(-) create mode 100644 common/op_params.py create mode 100644 common/travis_checker.py diff --git a/common/op_params.py b/common/op_params.py new file mode 100644 index 000000000..1846f8fd0 --- /dev/null +++ b/common/op_params.py @@ -0,0 +1,187 @@ +#!/usr/bin/env python3 +import os +import json +import time +import string +import random +from common.travis_checker import travis + + +def write_params(params, params_file): + if not travis: + with open(params_file, "w") as f: + json.dump(params, f, indent=2, sort_keys=True) + os.chmod(params_file, 0o764) + + +def read_params(params_file, default_params): + try: + with open(params_file, "r") as f: + params = json.load(f) + return params, True + except Exception as e: + print(e) + params = default_params + return params, False + + +class KeyInfo: + has_allowed_types = False + live = False + has_default = False + has_description = False + + +class opParams: + def __init__(self): + """ + To add your own parameter to opParams in your fork, simply add a new dictionary entry with the name of your parameter and its default value to save to new users' op_params.json file. + The description, allowed_types, and live keys are no longer required but recommended to help users edit their parameters with opEdit and opTune correctly. + - The description value will be shown to users when they use opEdit or opTune to change the value of the parameter. + - The allowed_types key is used to restrict what kinds of values can be entered with opEdit so that users can't reasonably break the fork with unintended behavior. + 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 key with `isinstance()`. + - Finally, the live key tells both opParams and opTune that it's a live parameter that will change. Therefore, you must place the `op_params.get()` call in the update function so that it can update. + Here's an example of the minimum required dictionary: + + self.default_params = {'camera_offset': {'default': 0.06}} + """ + + self.default_params = {'dynamic_follow': {'default': 'relaxed', 'allowed_types': [str], 'description': "Can be: ('traffic', 'relaxed', 'roadtrip'): Left to right increases in following distance.\n" + "All profiles support dynamic follow so you'll get your preferred distance while\n" + "retaining the smoothness and safety of dynamic follow!", 'live': True},} + + self.params = {} + self.params_file = "/data/op_params.json" + self.kegman_file = "/data/kegman.json" + self.last_read_time = time.time() + self.read_frequency = 5.0 # max frequency to read with self.get(...) (sec) + self.force_update = False # replaces values with default params if True, not just add add missing key/value pairs + self.to_delete = ['dynamic_lane_speed', 'longkiV', 'following_distance', 'static_steer_ratio'] + self.run_init() # restores, reads, and updates params + + def create_id(self): # creates unique identifier to send with sentry errors. please update uniqueID with op_edit.py to your username! + need_id = False + if "uniqueID" not in self.params: + need_id = True + if "uniqueID" in self.params and self.params["uniqueID"] is None: + need_id = True + if need_id: + random_id = ''.join([random.choice(string.ascii_lowercase + string.ascii_uppercase + string.digits) for i in range(15)]) + self.params["uniqueID"] = random_id + + def add_default_params(self): + prev_params = dict(self.params) + if not travis: + self.create_id() + for key in self.default_params: + if self.force_update: + self.params[key] = self.default_params[key]['default'] + elif key not in self.params: + self.params[key] = self.default_params[key]['default'] + return prev_params == self.params + + def format_default_params(self): + return {key: self.default_params[key]['default'] for key in self.default_params} + + def run_init(self): # does first time initializing of default params, and/or restoring from kegman.json + if travis: + self.params = self.format_default_params() + return + self.params = self.format_default_params() # in case any file is corrupted + to_write = False + no_params = False + if os.path.isfile(self.params_file): + self.params, read_status = read_params(self.params_file, self.format_default_params()) + if read_status: + to_write = not self.add_default_params() # if new default data has been added + if self.delete_old(): # or if old params have been deleted + to_write = True + else: # don't overwrite corrupted params, just print to screen + print("ERROR: Can't read op_params.json file") + elif os.path.isfile(self.kegman_file): + to_write = True # write no matter what + try: + with open(self.kegman_file, "r") as f: # restore params from kegman + self.params = json.load(f) + self.add_default_params() + except: + print("ERROR: Can't read kegman.json file") + else: + no_params = True # user's first time running a fork with kegman_conf or op_params + if to_write or no_params: + write_params(self.params, self.params_file) + + def delete_old(self): + prev_params = dict(self.params) + for i in self.to_delete: + if i in self.params: + del self.params[i] + return prev_params == self.params + + def put(self, key, value): + self.params.update({key: value}) + write_params(self.params, self.params_file) + + def get(self, key=None, default=None, force_update=False): # can specify a default value if key doesn't exist + self.update_params(key, force_update) + if key is None: + return self.params + + if key in self.params: + key_info = self.get_key_info(key) + if key_info.has_allowed_types: + value = self.params[key] + allowed_types = self.default_params[key]['allowed_types'] + valid_type = type(value) in allowed_types + if not valid_type: + if key_info.has_default: # if value in op_params.json is not correct type, use default + value = self.default_params[key]['default'] + else: # else use a standard value based on type (last resort to keep openpilot running) + value = self.value_from_types(allowed_types) + else: + value = self.params[key] + else: + value = default + + return value + + def get_key_info(self, key): + key_info = KeyInfo() + if key in self.default_params: + if 'allowed_types' in self.default_params[key]: + allowed_types = self.default_params[key]['allowed_types'] + if isinstance(allowed_types, list) and len(allowed_types) > 0: + key_info.has_allowed_types = True + if 'live' in self.default_params[key] and self.default_params[key]['live'] is True: + key_info.live = True + if 'default' in self.default_params[key]: + key_info.has_default = True + if 'description' in self.default_params[key]: + key_info.has_description = True + return key_info + + def value_from_types(self, allowed_types): + if list in allowed_types: + return [] + elif float in allowed_types or int in allowed_types: + return 0 + elif type(None) in allowed_types: + return None + elif str in allowed_types: + return '' + return None # unknown type + + def update_params(self, key, force_update): + if force_update or self.get_key_info(key).live: # if is a live param, we want to get updates while openpilot is running + if not travis and time.time() - self.last_read_time >= self.read_frequency: # make sure we aren't reading file too often + self.params, read_status = read_params(self.params_file, self.format_default_params()) + if not read_status: + time.sleep(1/100.) + self.params, _ = read_params(self.params_file, self.format_default_params()) # if the file was being written to, retry once + self.last_read_time = time.time() + + def delete(self, key): + if key in self.params: + del self.params[key] + write_params(self.params, self.params_file) diff --git a/common/travis_checker.py b/common/travis_checker.py new file mode 100644 index 000000000..74f2d256d --- /dev/null +++ b/common/travis_checker.py @@ -0,0 +1,2 @@ +from common.basedir import BASEDIR +travis = BASEDIR.strip('/').split('/')[0] != 'data' diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index f694af377..593eb7e43 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -1,5 +1,6 @@ import os import math +import time import cereal.messaging as messaging from selfdrive.swaglog import cloudlog @@ -7,6 +8,10 @@ from common.realtime import sec_since_boot from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG +from common.op_params import opParams +from common.numpy_fast import interp, clip +from common.travis_checker import travis +from selfdrive.config import Conversions as CV LOG_MPC = os.environ.get('LOG_MPC', False) @@ -14,6 +19,7 @@ LOG_MPC = os.environ.get('LOG_MPC', False) class LongitudinalMpc(): def __init__(self, mpc_id): self.mpc_id = mpc_id + self.op_params = opParams() self.setup_mpc() self.v_mpc = 0.0 @@ -23,9 +29,15 @@ class LongitudinalMpc(): self.prev_lead_status = False self.prev_lead_x = 0.0 self.new_lead = False - self.last_cloudlog_t = 0.0 + self.car_data = {'v_ego': 0.0, 'a_ego': 0.0} + self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False} + self.df_data = {"v_leads": [], "v_egos": []} # dynamic follow data + self.last_cost = 0.0 + self.df_profile = self.op_params.get('dynamic_follow', 'relaxed').strip().lower() + self.sng = False + def send_mpc_solution(self, pm, qp_iterations, calculation_time): qp_iterations = max(0, qp_iterations) dat = messaging.new_message() @@ -57,8 +69,120 @@ class LongitudinalMpc(): self.cur_state[0].v_ego = v self.cur_state[0].a_ego = a + def get_TR(self, CS): + if not self.lead_data['status'] or travis: + TR = 1.8 + else: + self.store_df_data() + TR = self.dynamic_follow(CS) + + if not travis: + self.change_cost(TR) + return TR + + def change_cost(self, TR): + TRs = [0.9, 1.8, 2.7] + costs = [1.0, 0.1, 0.05] + cost = interp(TR, TRs, costs) + if self.last_cost != cost: + self.libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.last_cost = cost + + def store_df_data(self): + v_lead_retention = 1.9 # keep only last x seconds + v_ego_retention = 2.5 + + cur_time = time.time() + if self.lead_data['status']: + self.df_data['v_leads'] = [sample for sample in self.df_data['v_leads'] if + cur_time - sample['time'] <= v_lead_retention + and not self.new_lead] # reset when new lead + self.df_data['v_leads'].append({'v_lead': self.lead_data['v_lead'], 'time': cur_time}) + + self.df_data['v_egos'] = [sample for sample in self.df_data['v_egos'] if cur_time - sample['time'] <= v_ego_retention] + self.df_data['v_egos'].append({'v_ego': self.car_data['v_ego'], 'time': cur_time}) + + def calculate_lead_accel(self): + min_consider_time = 1.0 # minimum amount of time required to consider calculation + a_lead = self.lead_data['a_lead'] + if len(self.df_data['v_leads']): # if not empty + elapsed = self.df_data['v_leads'][-1]['time'] - self.df_data['v_leads'][0]['time'] + if elapsed > min_consider_time: # if greater than min time (not 0) + a_calculated = (self.df_data['v_leads'][-1]['v_lead'] - self.df_data['v_leads'][0]['v_lead']) / elapsed # delta speed / delta time + # old version: # if abs(a_calculated) > abs(a_lead) and a_lead < 0.33528: # if a_lead is greater than calculated accel (over last 1.5s, use that) and if lead accel is not above 0.75 mph/s + # a_lead = a_calculated + + # long version of below: if (a_calculated < 0 and a_lead >= 0 and a_lead < -a_calculated * 0.5) or (a_calculated > 0 and a_lead <= 0 and -a_lead > a_calculated * 0.5) or (a_lead * a_calculated > 0 and abs(a_calculated) > abs(a_lead)): + if (a_calculated < 0 <= a_lead < -a_calculated * 0.55) or (a_calculated > 0 >= a_lead and -a_lead < a_calculated * 0.45) or ( + a_lead * a_calculated > 0 and abs(a_calculated) > abs(a_lead)): # this is a mess, fix + a_lead = a_calculated + return a_lead # if above doesn't execute, we'll return a_lead from radar + + def dynamic_follow(self, CS): + self.df_profile = self.op_params.get('dynamic_follow', 'relaxed').strip().lower() + x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities + profile_mod_x = [2.2352, 13.4112, 24.5872, 35.7632] # profile mod speeds, mph: [5., 30., 55., 80.] + if self.df_profile == 'roadtrip': + y_dist = [1.3847, 1.3946, 1.4078, 1.4243, 1.4507, 1.4837, 1.5327, 1.553, 1.581, 1.617, 1.653, 1.687, 1.74] # TRs + profile_mod_pos = [0.99, 0.9025, 0.815, 0.55] + profile_mod_neg = [1.0, 1.18, 1.382, 1.787] + elif self.df_profile == 'traffic': # for in congested traffic + x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35] + y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] + profile_mod_pos = [1.05, 1.375, 2.99, 3.8] + profile_mod_neg = [0.79, .1, 0.0, 0.0] + else: # default to relaxed/stock + y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614] + profile_mod_pos = [1.0] * 4 + profile_mod_neg = [1.0] * 4 + + sng_TR = 1.7 # reacceleration stop and go TR + sng_speed = 15.0 * CV.MPH_TO_MS + + if self.car_data['v_ego'] > sng_speed: # keep sng distance until we're above sng speed again + self.sng = False + + if (self.car_data['v_ego'] >= sng_speed or self.df_data['v_egos'][0]['v_ego'] >= self.car_data[ + 'v_ego']) and not self.sng: # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease + TR = interp(self.car_data['v_ego'], x_vel, y_dist) + else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating + self.sng = True + x = [sng_speed / 3.0, sng_speed] # decrease TR between 5 and 15 mph from 1.8s to defined TR above at 15mph while accelerating + y = [sng_TR, interp(sng_speed, x_vel, y_dist)] + TR = interp(self.car_data['v_ego'], x, y) + + TR_mod = [] + # Dynamic follow modifications (the secret sauce) + x = [-20.0383, -15.6978, -11.2053, -7.8781, -5.0407, -3.2167, -1.6122, 0.0, 0.6847, 1.3772, 1.9007, 2.7452] # relative velocity values + y = [0.641, 0.506, 0.418, 0.334, 0.24, 0.115, 0.065, 0.0, -0.049, -0.068, -0.142, -0.221] # modification values + TR_mod.append(interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y)) + + x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466, 0.5144, 0.6903, 0.9302] # lead acceleration values + y = [0.265, 0.187, 0.096, 0.057, 0.033, 0.024, 0.0, -0.009, -0.042, -0.053, -0.059] # modification values + TR_mod.append(interp(self.calculate_lead_accel(), x, y)) + + profile_mod_pos = interp(self.car_data['v_ego'], profile_mod_x, profile_mod_pos) + profile_mod_neg = interp(self.car_data['v_ego'], profile_mod_x, profile_mod_neg) + + TR_mod = sum([mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos for mod in TR_mod]) # alter TR modification according to profile + TR += TR_mod + + if CS.leftBlinker or CS.rightBlinker and self.df_profile != 'traffic': + x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph + y = [1.0, .75, .65] # reduce TR when changing lanes + TR *= interp(self.car_data['v_ego'], x, y) + + return clip(TR, 0.9, 2.7) + + def process_lead(self, v_lead, a_lead, x_lead, status): + self.lead_data['v_lead'] = v_lead + self.lead_data['a_lead'] = a_lead + self.lead_data['x_lead'] = x_lead + self.lead_data['status'] = status + def update(self, pm, CS, lead, v_cruise_setpoint): v_ego = CS.vEgo + self.car_data = {'v_ego': CS.vEgo, 'a_ego': CS.aEgo} # Setup current mpc state self.cur_state[0].x_ego = 0.0 @@ -71,6 +195,7 @@ class LongitudinalMpc(): if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): v_lead = 0.0 a_lead = 0.0 + self.process_lead(v_lead, a_lead, x_lead, lead.status) self.a_lead_tau = lead.aLeadTau self.new_lead = False @@ -83,6 +208,7 @@ class LongitudinalMpc(): self.cur_state[0].x_l = x_lead self.cur_state[0].v_l = v_lead else: + self.process_lead(None, None, None, False) self.prev_lead_status = False # Fake a fast lead car, so mpc keeps running self.cur_state[0].x_l = 50.0 @@ -92,7 +218,7 @@ class LongitudinalMpc(): # Calculate mpc t = sec_since_boot() - n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR(CS)) duration = int((sec_since_boot() - t) * 1e9) if LOG_MPC: diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c index 8cfc06f3b..2adeb4397 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c @@ -68,7 +68,7 @@ acadoWorkspace.evGu[lRun1 * 3 + 2] = acadoWorkspace.state[14]; return ret; } -void acado_evaluateLSQ(const real_t* in, real_t* out) +void acado_evaluateLSQ(const real_t* in, real_t* out, double TR) { const real_t* xd = in; const real_t* u = in + 3; @@ -78,29 +78,29 @@ real_t* a = acadoWorkspace.objAuxVar; /* Compute intermediate quantities: */ a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]); a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[7] = (a[6]*(real_t)(5.0000000000000000e-01)); a[8] = (a[2]*a[2]); -a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); +a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[12] = (a[10]*a[10]); /* Compute outputs: */ out[0] = (a[1]-(real_t)(1.0000000000000000e+00)); -out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[3] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[4] = a[4]; out[5] = a[9]; out[6] = (real_t)(0.0000000000000000e+00); out[7] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]); -out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); +out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); out[9] = (real_t)(0.0000000000000000e+00); out[10] = (real_t)(0.0000000000000000e+00); out[11] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -114,7 +114,7 @@ out[18] = (real_t)(0.0000000000000000e+00); out[19] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)); } -void acado_evaluateLSQEndTerm(const real_t* in, real_t* out) +void acado_evaluateLSQEndTerm(const real_t* in, real_t* out, double TR) { const real_t* xd = in; const real_t* od = in + 3; @@ -123,28 +123,28 @@ real_t* a = acadoWorkspace.objAuxVar; /* Compute intermediate quantities: */ a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]); a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[7] = (a[6]*(real_t)(5.0000000000000000e-01)); a[8] = (a[2]*a[2]); -a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); +a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[12] = (a[10]*a[10]); /* Compute outputs: */ out[0] = (a[1]-(real_t)(1.0000000000000000e+00)); -out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[3] = a[4]; out[4] = a[9]; out[5] = (real_t)(0.0000000000000000e+00); out[6] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]); -out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); +out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); out[8] = (real_t)(0.0000000000000000e+00); out[9] = (real_t)(0.0000000000000000e+00); out[10] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -207,7 +207,7 @@ tmpQN1[7] = + tmpQN2[6]*tmpFx[1] + tmpQN2[7]*tmpFx[4] + tmpQN2[8]*tmpFx[7]; tmpQN1[8] = + tmpQN2[6]*tmpFx[2] + tmpQN2[7]*tmpFx[5] + tmpQN2[8]*tmpFx[8]; } -void acado_evaluateObjective( ) +void acado_evaluateObjective( double TR ) { int runObj; for (runObj = 0; runObj < 20; ++runObj) @@ -219,7 +219,7 @@ acadoWorkspace.objValueIn[3] = acadoVariables.u[runObj]; acadoWorkspace.objValueIn[4] = acadoVariables.od[runObj * 2]; acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 2 + 1]; -acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0]; acadoWorkspace.Dy[runObj * 4 + 1] = acadoWorkspace.objValueOut[1]; acadoWorkspace.Dy[runObj * 4 + 2] = acadoWorkspace.objValueOut[2]; @@ -235,7 +235,7 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[61]; acadoWorkspace.objValueIn[2] = acadoVariables.x[62]; acadoWorkspace.objValueIn[3] = acadoVariables.od[40]; acadoWorkspace.objValueIn[4] = acadoVariables.od[41]; -acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1]; @@ -412,7 +412,7 @@ int lRun3; int lRun4; int lRun5; /** Row vector of size: 20 */ -static const int xBoundIndices[ 20 ] = +static const int xBoundIndices[ 20 ] = { 4, 7, 10, 13, 16, 19, 22, 25, 28, 31, 34, 37, 40, 43, 46, 49, 52, 55, 58, 61 }; acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E ); acado_moveGxT( &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.T ); @@ -4589,12 +4589,12 @@ acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVar acado_multEDu( &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 60 ]) ); } -int acado_preparationStep( ) +int acado_preparationStep( double TR ) { int ret; ret = acado_modelSimulation(); -acado_evaluateObjective( ); +acado_evaluateObjective( TR ); acado_condensePrep( ); return ret; } @@ -4660,7 +4660,7 @@ acadoVariables.x[60] = xEnd[0]; acadoVariables.x[61] = xEnd[1]; acadoVariables.x[62] = xEnd[2]; } -else if (strategy == 2) +else if (strategy == 2) { acadoWorkspace.state[0] = acadoVariables.x[60]; acadoWorkspace.state[1] = acadoVariables.x[61]; @@ -4726,7 +4726,7 @@ kkt += fabs(acadoWorkspace.ubA[index] * prd); return kkt; } -real_t acado_getObjective( ) +real_t acado_getObjective( TR ) { real_t objVal; @@ -4746,7 +4746,7 @@ acadoWorkspace.objValueIn[3] = acadoVariables.u[lRun1]; acadoWorkspace.objValueIn[4] = acadoVariables.od[lRun1 * 2]; acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 2 + 1]; -acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.Dy[lRun1 * 4] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 4]; acadoWorkspace.Dy[lRun1 * 4 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 4 + 1]; acadoWorkspace.Dy[lRun1 * 4 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 4 + 2]; @@ -4757,7 +4757,7 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[61]; acadoWorkspace.objValueIn[2] = acadoVariables.x[62]; acadoWorkspace.objValueIn[3] = acadoVariables.od[40]; acadoWorkspace.objValueIn[4] = acadoVariables.od[41]; -acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1]; acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2] - acadoVariables.yN[2]; @@ -4779,4 +4779,3 @@ objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1] + objVal *= 0.5; return objVal; } - diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py index c40b4e071..fd8947ff0 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -29,8 +29,9 @@ def _get_libmpc(mpc_id): void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l); + void change_tr(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); int run_mpc(state_t * x0, log_t * solution, - double l, double a_l_0); + double l, double a_l_0, double TR); """) return (ffi, ffi.dlopen(libmpc_fn)) diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c index d4bfee8c8..f0c6cca85 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c +++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c @@ -68,6 +68,25 @@ void init(double ttcCost, double distanceCost, double accelerationCost, double j } +void change_tr(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){ + int i; + const int STEP_MULTIPLIER = 3; + + for (i = 0; i < N; i++) { + int f = 1; + if (i > 4){ + f = STEP_MULTIPLIER; + } + acadoVariables.W[16 * i + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc) + acadoVariables.W[16 * i + 5] = distanceCost * f; // desired distance + acadoVariables.W[16 * i + 10] = accelerationCost * f; // acceleration + acadoVariables.W[16 * i + 15] = jerkCost * f; // jerk + } + acadoVariables.WN[0] = ttcCost * STEP_MULTIPLIER; // exponential cost for danger zone + acadoVariables.WN[4] = distanceCost * STEP_MULTIPLIER; // desired distance + acadoVariables.WN[8] = accelerationCost * STEP_MULTIPLIER; // acceleration +} + void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0, double l){ int i; @@ -112,7 +131,7 @@ void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0 for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; } -int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ +int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0, double TR){ // Calculate lead vehicle predictions int i; double t = 0.; @@ -152,7 +171,7 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego; acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego; - acado_preparationStep(); + acado_preparationStep(TR); acado_feedbackStep(); for (i = 0; i <= N; i++){ @@ -164,10 +183,10 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ solution->j_ego[i] = acadoVariables.u[i]; } } - solution->cost = acado_getObjective(); + solution->cost = acado_getObjective(TR); // Dont shift states here. Current solution is closer to next timestep than if // we shift by 0.2 seconds. return acado_getNWSR(); -} +} \ No newline at end of file From adffbc9fd4529946101b3d0653cad929d9e7120c Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 10 Mar 2020 03:10:21 -0500 Subject: [PATCH 2/9] add opEdit to easily change parameter --- op_edit.py | 220 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 220 insertions(+) create mode 100644 op_edit.py diff --git a/op_edit.py b/op_edit.py new file mode 100644 index 000000000..5faaae994 --- /dev/null +++ b/op_edit.py @@ -0,0 +1,220 @@ +#!/usr/bin/env python3 +from common.op_params import opParams +import time +import ast + + +class opEdit: # use by running `python /data/openpilot/op_edit.py` + def __init__(self): + self.op_params = opParams() + self.params = None + self.sleep_time = 1.0 + self.live_tuning = False + self.welcome() + + def welcome(self): + print('Welcome to the opParams command line editor!') + while True: + print('Would you like to enter opEdit\'s live-tuning mode?') + choice = input('[Y/n]: ').lower().strip() + if choice in ['y', 'ye', 'yes']: + self.live_tuning = True + self.run_loop() + break + elif choice in ['n', 'no', '']: + self.run_loop() + break + elif choice in ['e', 'exit']: + break + + def run_loop(self): + if not self.live_tuning: + print('Here are your parameters:\n') + else: + print('Here are your live parameters:\n') + while True: + 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.get_key_info(k).live} + + values_list = [self.params[i] if len(str(self.params[i])) < 20 else '{} ... {}'.format(str(self.params[i])[:30], str(self.params[i])[-15:]) for i in self.params] + live = ['(live!)' if self.op_params.get_key_info(i).live else '' for i in self.params] + + to_print = ['{}. {}: {} {}'.format(idx + 1, i, values_list[idx], live[idx]) for idx, i in enumerate(self.params)] + to_print.append('\n{}. Add new parameter!'.format(len(self.params) + 1)) + to_print.append('{}. Delete parameter!'.format(len(self.params) + 2)) + + print('\n'.join(to_print)) + print('\nChoose a parameter to explore (by integer index): ') + + choice = input('>> ').strip() + parsed, choice = self.parse_choice(choice) + if parsed == 'continue': + continue + elif parsed == 'add': + self.add_parameter() + elif parsed == 'change': + self.change_parameter(choice) + elif parsed == 'delete': + self.delete_parameter() + elif parsed == 'error': + return + + def parse_choice(self, choice): + if choice.isdigit(): + choice = int(choice) + choice -= 1 + elif choice == '': + print('Exiting opEdit!') + return 'error', choice + else: + self.message('Not an integer!') + return 'retry', choice + if choice not in range(0, len(self.params) + 2): # three for add/delete parameter + self.message('Not in range!') + return 'continue', choice + + if choice == len(self.params): # add new parameter + return 'add', choice + + if choice == len(self.params) + 1: # delete parameter + return 'delete', choice + + return 'change', choice + + def change_parameter(self, choice): + while True: + chosen_key = list(self.params)[choice] + key_info = self.op_params.get_key_info(chosen_key) + + old_value = self.params[chosen_key] + print('Chosen parameter: {}'.format(chosen_key)) + + to_print = [] + if key_info.has_description: + to_print.append('>> Description: {}'.format(self.op_params.default_params[chosen_key]['description'].replace('\n', '\n > '))) + if key_info.has_allowed_types: + allowed_types = self.op_params.default_params[chosen_key]['allowed_types'] + to_print.append('>> Allowed types: {}'.format(', '.join([str(i).split("'")[1] for i in allowed_types]))) + if key_info.live: + to_print.append('>> This parameter supports live tuning! Updates should take affect within 5 seconds.\n') + + if to_print: + print('\n{}\n'.format('\n'.join(to_print))) + + print('Current value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) + if key_info.live or self.live_tuning: # similar to opTune + while True: + print('Enter your new value:') + new_value = input('>> ').strip() + if new_value == '': + self.message('Exiting this parameter...') + return + + new_value = self.parse_input(new_value) + if key_info.has_allowed_types and type(new_value) not in allowed_types: + self.message('The type of data you entered ({}) is not allowed with this parameter!'.format(str(type(new_value)).split("'")[1])) + continue + + self.op_params.put(chosen_key, new_value) + print('Saved {} with value: {}! (type: {})\n'.format(chosen_key, new_value, str(type(new_value)).split("'")[1])) + else: + print('Enter your new value:') + new_value = input('>> ').strip() + if new_value == '': + self.message('Exiting this parameter...') + return + + new_value = self.parse_input(new_value) + if key_info.has_allowed_types and type(new_value) not in allowed_types: + self.message('The type of data you entered ({}) is not allowed with this parameter!'.format(str(type(new_value)).split("'")[1])) + continue + + print('\nOld value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) + print('New value: {} (type: {})'.format(new_value, str(type(new_value)).split("'")[1])) + print('Do you want to save this?') + choice = input('[Y/n]: ').lower().strip() + if choice == 'y': + self.op_params.put(chosen_key, new_value) + self.message('Saved!') + else: + self.message('Not saved!') + return + + def parse_input(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 + + def delete_parameter(self): + while True: + print('Enter the name of the parameter to delete:') + key = input('>> ').lower() + key = self.parse_input(key) + + if key == '': + return + if not isinstance(key, str): + self.message('Input must be a string!') + continue + if key not in self.params: + self.message("Parameter doesn't exist!") + continue + + value = self.params.get(key) + print('Parameter name: {}'.format(key)) + print('Parameter value: {} (type: {})'.format(value, str(type(value)).split("'")[1])) + print('Do you want to delete this?') + + choice = input('[Y/n]: ').lower().strip() + if choice == 'y': + self.op_params.delete(key) + self.message('Deleted!') + else: + self.message('Not saved!') + return + + def add_parameter(self): + while True: + print('Type the name of your new parameter:') + key = input('>> ').strip() + if key == '': + return + + key = self.parse_input(key) + + if not isinstance(key, str): + self.message('Input must be a string!') + continue + + print("Enter the data you'd like to save with this parameter:") + value = input('>> ').strip() + value = self.parse_input(value) + + print('Parameter name: {}'.format(key)) + print('Parameter value: {} (type: {})'.format(value, str(type(value)).split("'")[1])) + print('Do you want to save this?') + + choice = input('[Y/n]: ').lower().strip() + if choice == 'y': + self.op_params.put(key, value) + self.message('Saved!') + else: + self.message('Not saved!') + return + + def message(self, msg): + print('--------\n{}\n--------'.format(msg), flush=True) + time.sleep(self.sleep_time) + print() + + +opEdit() From e3015eccd427651fe26fba469dd37d63b677cfe0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 10 Mar 2020 04:24:16 -0500 Subject: [PATCH 3/9] better live tuning implementation with opEdit --- common/op_params.py | 51 +++++++++++++++++++++++++---------------- op_edit.py | 55 +++++++++++++++++++-------------------------- 2 files changed, 55 insertions(+), 51 deletions(-) diff --git a/common/op_params.py b/common/op_params.py index 1846f8fd0..be6dc1ec5 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -4,6 +4,7 @@ import json import time import string import random +from selfdrive.swaglog import cloudlog from common.travis_checker import travis @@ -30,6 +31,7 @@ class KeyInfo: live = False has_default = False has_description = False + hidden = False class opParams: @@ -49,7 +51,8 @@ class opParams: self.default_params = {'dynamic_follow': {'default': 'relaxed', 'allowed_types': [str], 'description': "Can be: ('traffic', 'relaxed', 'roadtrip'): Left to right increases in following distance.\n" "All profiles support dynamic follow so you'll get your preferred distance while\n" - "retaining the smoothness and safety of dynamic follow!", 'live': True},} + "retaining the smoothness and safety of dynamic follow!", 'live': True}, + 'op_edit_live_mode': {'default': False, 'description': 'This parameter controls which mode opEdit starts in. It should be hidden from the user with the hide key', 'hide': True}} self.params = {} self.params_file = "/data/op_params.json" @@ -126,39 +129,49 @@ class opParams: def get(self, key=None, default=None, force_update=False): # can specify a default value if key doesn't exist self.update_params(key, force_update) if key is None: - return self.params + return self.get_all() if key in self.params: - key_info = self.get_key_info(key) + key_info = self.key_info(key) if key_info.has_allowed_types: value = self.params[key] allowed_types = self.default_params[key]['allowed_types'] - valid_type = type(value) in allowed_types - if not valid_type: - if key_info.has_default: # if value in op_params.json is not correct type, use default - value = self.default_params[key]['default'] - else: # else use a standard value based on type (last resort to keep openpilot running) - value = self.value_from_types(allowed_types) + if type(value) not in allowed_types: + cloudlog.warning('op_params: User\'s value is not valid!') + if key_info.has_default: # invalid value type, try to use default value + default_value = self.default_params[key]['default'] + if type(default_value) in allowed_types: # actually check if the default is valid + # return default value because user's value of key is not in the allowed_types to avoid crashing openpilot + return default_value + else: # else use a standard value based on type (last resort to keep openpilot running if user's value is of invalid type) + return self.value_from_types(allowed_types) + else: + return value # all good, returning user's value else: - value = self.params[key] - else: - value = default + return self.params[key] # no defined allowed types, returning user's value - return value + return default # not in params - def get_key_info(self, key): + def get_all(self): # returns all non-hidden params + return {k: v for k, v in self.params.items() if not self.key_info(k).hidden} + + def key_info(self, key): key_info = KeyInfo() + if key is None: + return key_info if key in self.default_params: if 'allowed_types' in self.default_params[key]: allowed_types = self.default_params[key]['allowed_types'] if isinstance(allowed_types, list) and len(allowed_types) > 0: key_info.has_allowed_types = True - if 'live' in self.default_params[key] and self.default_params[key]['live'] is True: - key_info.live = True + if 'live' in self.default_params[key]: + key_info.live = self.default_params[key]['live'] if 'default' in self.default_params[key]: key_info.has_default = True if 'description' in self.default_params[key]: key_info.has_description = True + if 'hide' in self.default_params[key]: + key_info.hidden = self.default_params[key]['hide'] return key_info def value_from_types(self, allowed_types): @@ -173,8 +186,8 @@ class opParams: return None # unknown type def update_params(self, key, force_update): - if force_update or self.get_key_info(key).live: # if is a live param, we want to get updates while openpilot is running - if not travis and time.time() - self.last_read_time >= self.read_frequency: # make sure we aren't reading file too often + if force_update or self.key_info(key).live: # if is a live param, we want to get updates while openpilot is running + if not travis and (time.time() - self.last_read_time >= self.read_frequency or force_update): # make sure we aren't reading file too often self.params, read_status = read_params(self.params_file, self.format_default_params()) if not read_status: time.sleep(1/100.) @@ -184,4 +197,4 @@ class opParams: def delete(self, key): if key in self.params: del self.params[key] - write_params(self.params, self.params_file) + write_params(self.params, self.params_file) \ No newline at end of file diff --git a/op_edit.py b/op_edit.py index 5faaae994..617165e94 100644 --- a/op_edit.py +++ b/op_edit.py @@ -9,46 +9,33 @@ class opEdit: # use by running `python /data/openpilot/op_edit.py` self.op_params = opParams() self.params = None self.sleep_time = 1.0 - self.live_tuning = False - self.welcome() - - def welcome(self): - print('Welcome to the opParams command line editor!') - while True: - print('Would you like to enter opEdit\'s live-tuning mode?') - choice = input('[Y/n]: ').lower().strip() - if choice in ['y', 'ye', 'yes']: - self.live_tuning = True - self.run_loop() - break - elif choice in ['n', 'no', '']: - self.run_loop() - break - elif choice in ['e', 'exit']: - break + self.live_tuning = self.op_params.get('op_edit_live_mode', False) + self.run_loop() def run_loop(self): - if not self.live_tuning: - print('Here are your parameters:\n') - else: - print('Here are your live parameters:\n') + print('Welcome to the opParams command line editor!') while True: + if not self.live_tuning: + print('Here are your parameters:\n') + else: + print('Here are your live parameters:\n') 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.get_key_info(k).live} + self.params = {k: v for k, v in self.params.items() if self.op_params.key_info(k).live} values_list = [self.params[i] if len(str(self.params[i])) < 20 else '{} ... {}'.format(str(self.params[i])[:30], str(self.params[i])[-15:]) for i in self.params] - live = ['(live!)' if self.op_params.get_key_info(i).live else '' for i in self.params] + live = ['(live!)' if self.op_params.key_info(i).live else '' for i in self.params] to_print = ['{}. {}: {} {}'.format(idx + 1, i, values_list[idx], live[idx]) for idx, i in enumerate(self.params)] - to_print.append('\n{}. Add new parameter!'.format(len(self.params) + 1)) - to_print.append('{}. Delete parameter!'.format(len(self.params) + 2)) + to_print.append('---\n{}. Add new parameter'.format(len(to_print) + 1)) + to_print.append('{}. Delete parameter'.format(len(to_print) + 1)) + to_print.append('{}. Toggle live tuning'.format(len(to_print) + 1)) print('\n'.join(to_print)) print('\nChoose a parameter to explore (by integer index): ') choice = input('>> ').strip() - parsed, choice = self.parse_choice(choice) + parsed, choice = self.parse_choice(choice, len(to_print)) if parsed == 'continue': continue elif parsed == 'add': @@ -57,10 +44,13 @@ class opEdit: # use by running `python /data/openpilot/op_edit.py` self.change_parameter(choice) elif parsed == 'delete': self.delete_parameter() + elif parsed == 'live': + self.live_tuning = not self.live_tuning + self.op_params.put('op_edit_live_mode', self.live_tuning) # for next opEdit startup elif parsed == 'error': return - def parse_choice(self, choice): + def parse_choice(self, choice, opt_len): if choice.isdigit(): choice = int(choice) choice -= 1 @@ -70,22 +60,23 @@ class opEdit: # use by running `python /data/openpilot/op_edit.py` else: self.message('Not an integer!') return 'retry', choice - if choice not in range(0, len(self.params) + 2): # three for add/delete parameter + if choice not in range(opt_len): # number of options to choose from self.message('Not in range!') return 'continue', choice - if choice == len(self.params): # add new parameter + if choice == opt_len - 3: # add new parameter return 'add', choice - - if choice == len(self.params) + 1: # delete parameter + elif choice == opt_len - 2: # delete parameter return 'delete', choice + elif choice == opt_len - 1: # live tuning mode + return 'live', choice return 'change', choice def change_parameter(self, choice): while True: chosen_key = list(self.params)[choice] - key_info = self.op_params.get_key_info(chosen_key) + key_info = self.op_params.key_info(chosen_key) old_value = self.params[chosen_key] print('Chosen parameter: {}'.format(chosen_key)) From 3fab068a176d65e6c487c7f28e867ab57757a522 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Thu, 12 Mar 2020 14:26:53 +1000 Subject: [PATCH 4/9] required mods for dp --- common/op_params.py | 200 ------------------ common/params.py | 1 + common/travis_checker.py | 2 - op_edit.py | 211 ------------------- selfdrive/controls/lib/long_mpc.py | 42 +++- selfdrive/dragonpilot/dragonconf/__init__.py | 3 +- 6 files changed, 34 insertions(+), 425 deletions(-) delete mode 100644 common/op_params.py delete mode 100644 common/travis_checker.py delete mode 100644 op_edit.py diff --git a/common/op_params.py b/common/op_params.py deleted file mode 100644 index be6dc1ec5..000000000 --- a/common/op_params.py +++ /dev/null @@ -1,200 +0,0 @@ -#!/usr/bin/env python3 -import os -import json -import time -import string -import random -from selfdrive.swaglog import cloudlog -from common.travis_checker import travis - - -def write_params(params, params_file): - if not travis: - with open(params_file, "w") as f: - json.dump(params, f, indent=2, sort_keys=True) - os.chmod(params_file, 0o764) - - -def read_params(params_file, default_params): - try: - with open(params_file, "r") as f: - params = json.load(f) - return params, True - except Exception as e: - print(e) - params = default_params - return params, False - - -class KeyInfo: - has_allowed_types = False - live = False - has_default = False - has_description = False - hidden = False - - -class opParams: - def __init__(self): - """ - To add your own parameter to opParams in your fork, simply add a new dictionary entry with the name of your parameter and its default value to save to new users' op_params.json file. - The description, allowed_types, and live keys are no longer required but recommended to help users edit their parameters with opEdit and opTune correctly. - - The description value will be shown to users when they use opEdit or opTune to change the value of the parameter. - - The allowed_types key is used to restrict what kinds of values can be entered with opEdit so that users can't reasonably break the fork with unintended behavior. - 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 key with `isinstance()`. - - Finally, the live key tells both opParams and opTune that it's a live parameter that will change. Therefore, you must place the `op_params.get()` call in the update function so that it can update. - Here's an example of the minimum required dictionary: - - self.default_params = {'camera_offset': {'default': 0.06}} - """ - - self.default_params = {'dynamic_follow': {'default': 'relaxed', 'allowed_types': [str], 'description': "Can be: ('traffic', 'relaxed', 'roadtrip'): Left to right increases in following distance.\n" - "All profiles support dynamic follow so you'll get your preferred distance while\n" - "retaining the smoothness and safety of dynamic follow!", 'live': True}, - 'op_edit_live_mode': {'default': False, 'description': 'This parameter controls which mode opEdit starts in. It should be hidden from the user with the hide key', 'hide': True}} - - self.params = {} - self.params_file = "/data/op_params.json" - self.kegman_file = "/data/kegman.json" - self.last_read_time = time.time() - self.read_frequency = 5.0 # max frequency to read with self.get(...) (sec) - self.force_update = False # replaces values with default params if True, not just add add missing key/value pairs - self.to_delete = ['dynamic_lane_speed', 'longkiV', 'following_distance', 'static_steer_ratio'] - self.run_init() # restores, reads, and updates params - - def create_id(self): # creates unique identifier to send with sentry errors. please update uniqueID with op_edit.py to your username! - need_id = False - if "uniqueID" not in self.params: - need_id = True - if "uniqueID" in self.params and self.params["uniqueID"] is None: - need_id = True - if need_id: - random_id = ''.join([random.choice(string.ascii_lowercase + string.ascii_uppercase + string.digits) for i in range(15)]) - self.params["uniqueID"] = random_id - - def add_default_params(self): - prev_params = dict(self.params) - if not travis: - self.create_id() - for key in self.default_params: - if self.force_update: - self.params[key] = self.default_params[key]['default'] - elif key not in self.params: - self.params[key] = self.default_params[key]['default'] - return prev_params == self.params - - def format_default_params(self): - return {key: self.default_params[key]['default'] for key in self.default_params} - - def run_init(self): # does first time initializing of default params, and/or restoring from kegman.json - if travis: - self.params = self.format_default_params() - return - self.params = self.format_default_params() # in case any file is corrupted - to_write = False - no_params = False - if os.path.isfile(self.params_file): - self.params, read_status = read_params(self.params_file, self.format_default_params()) - if read_status: - to_write = not self.add_default_params() # if new default data has been added - if self.delete_old(): # or if old params have been deleted - to_write = True - else: # don't overwrite corrupted params, just print to screen - print("ERROR: Can't read op_params.json file") - elif os.path.isfile(self.kegman_file): - to_write = True # write no matter what - try: - with open(self.kegman_file, "r") as f: # restore params from kegman - self.params = json.load(f) - self.add_default_params() - except: - print("ERROR: Can't read kegman.json file") - else: - no_params = True # user's first time running a fork with kegman_conf or op_params - if to_write or no_params: - write_params(self.params, self.params_file) - - def delete_old(self): - prev_params = dict(self.params) - for i in self.to_delete: - if i in self.params: - del self.params[i] - return prev_params == self.params - - def put(self, key, value): - self.params.update({key: value}) - write_params(self.params, self.params_file) - - def get(self, key=None, default=None, force_update=False): # can specify a default value if key doesn't exist - self.update_params(key, force_update) - if key is None: - return self.get_all() - - if key in self.params: - key_info = self.key_info(key) - if key_info.has_allowed_types: - value = self.params[key] - allowed_types = self.default_params[key]['allowed_types'] - if type(value) not in allowed_types: - cloudlog.warning('op_params: User\'s value is not valid!') - if key_info.has_default: # invalid value type, try to use default value - default_value = self.default_params[key]['default'] - if type(default_value) in allowed_types: # actually check if the default is valid - # return default value because user's value of key is not in the allowed_types to avoid crashing openpilot - return default_value - else: # else use a standard value based on type (last resort to keep openpilot running if user's value is of invalid type) - return self.value_from_types(allowed_types) - else: - return value # all good, returning user's value - else: - return self.params[key] # no defined allowed types, returning user's value - - return default # not in params - - def get_all(self): # returns all non-hidden params - return {k: v for k, v in self.params.items() if not self.key_info(k).hidden} - - def key_info(self, key): - key_info = KeyInfo() - if key is None: - return key_info - if key in self.default_params: - if 'allowed_types' in self.default_params[key]: - allowed_types = self.default_params[key]['allowed_types'] - if isinstance(allowed_types, list) and len(allowed_types) > 0: - key_info.has_allowed_types = True - if 'live' in self.default_params[key]: - key_info.live = self.default_params[key]['live'] - if 'default' in self.default_params[key]: - key_info.has_default = True - if 'description' in self.default_params[key]: - key_info.has_description = True - if 'hide' in self.default_params[key]: - key_info.hidden = self.default_params[key]['hide'] - return key_info - - def value_from_types(self, allowed_types): - if list in allowed_types: - return [] - elif float in allowed_types or int in allowed_types: - return 0 - elif type(None) in allowed_types: - return None - elif str in allowed_types: - return '' - return None # unknown type - - def update_params(self, key, force_update): - if force_update or self.key_info(key).live: # if is a live param, we want to get updates while openpilot is running - if not travis and (time.time() - self.last_read_time >= self.read_frequency or force_update): # make sure we aren't reading file too often - self.params, read_status = read_params(self.params_file, self.format_default_params()) - if not read_status: - time.sleep(1/100.) - self.params, _ = read_params(self.params_file, self.format_default_params()) # if the file was being written to, retry once - self.last_read_time = time.time() - - def delete(self, key): - if key in self.params: - del self.params[key] - write_params(self.params, self.params_file) \ No newline at end of file diff --git a/common/params.py b/common/params.py index fa93520ee..65101d2f5 100755 --- a/common/params.py +++ b/common/params.py @@ -169,6 +169,7 @@ keys = { "DragonBootHotspot": [TxType.PERSISTENT], "DragonAccelProfile": [TxType.PERSISTENT], "DragonLastModified": [TxType.PERSISTENT], + "DragonDynamicFollow": [TxType.PERSISTENT], } diff --git a/common/travis_checker.py b/common/travis_checker.py deleted file mode 100644 index 74f2d256d..000000000 --- a/common/travis_checker.py +++ /dev/null @@ -1,2 +0,0 @@ -from common.basedir import BASEDIR -travis = BASEDIR.strip('/').split('/')[0] != 'data' diff --git a/op_edit.py b/op_edit.py deleted file mode 100644 index 617165e94..000000000 --- a/op_edit.py +++ /dev/null @@ -1,211 +0,0 @@ -#!/usr/bin/env python3 -from common.op_params import opParams -import time -import ast - - -class opEdit: # use by running `python /data/openpilot/op_edit.py` - def __init__(self): - self.op_params = opParams() - self.params = None - self.sleep_time = 1.0 - self.live_tuning = self.op_params.get('op_edit_live_mode', False) - self.run_loop() - - def run_loop(self): - print('Welcome to the opParams command line editor!') - while True: - if not self.live_tuning: - print('Here are your parameters:\n') - else: - print('Here are your live parameters:\n') - 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.key_info(k).live} - - values_list = [self.params[i] if len(str(self.params[i])) < 20 else '{} ... {}'.format(str(self.params[i])[:30], str(self.params[i])[-15:]) for i in self.params] - live = ['(live!)' if self.op_params.key_info(i).live else '' for i in self.params] - - to_print = ['{}. {}: {} {}'.format(idx + 1, i, values_list[idx], live[idx]) for idx, i in enumerate(self.params)] - to_print.append('---\n{}. Add new parameter'.format(len(to_print) + 1)) - to_print.append('{}. Delete parameter'.format(len(to_print) + 1)) - to_print.append('{}. Toggle live tuning'.format(len(to_print) + 1)) - - print('\n'.join(to_print)) - print('\nChoose a parameter to explore (by integer index): ') - - choice = input('>> ').strip() - parsed, choice = self.parse_choice(choice, len(to_print)) - if parsed == 'continue': - continue - elif parsed == 'add': - self.add_parameter() - elif parsed == 'change': - self.change_parameter(choice) - elif parsed == 'delete': - self.delete_parameter() - elif parsed == 'live': - self.live_tuning = not self.live_tuning - self.op_params.put('op_edit_live_mode', self.live_tuning) # for next opEdit startup - elif parsed == 'error': - return - - def parse_choice(self, choice, opt_len): - if choice.isdigit(): - choice = int(choice) - choice -= 1 - elif choice == '': - print('Exiting opEdit!') - return 'error', choice - else: - self.message('Not an integer!') - return 'retry', choice - if choice not in range(opt_len): # number of options to choose from - self.message('Not in range!') - return 'continue', choice - - if choice == opt_len - 3: # add new parameter - return 'add', choice - elif choice == opt_len - 2: # delete parameter - return 'delete', choice - elif choice == opt_len - 1: # live tuning mode - return 'live', choice - - return 'change', choice - - def change_parameter(self, choice): - while True: - chosen_key = list(self.params)[choice] - key_info = self.op_params.key_info(chosen_key) - - old_value = self.params[chosen_key] - print('Chosen parameter: {}'.format(chosen_key)) - - to_print = [] - if key_info.has_description: - to_print.append('>> Description: {}'.format(self.op_params.default_params[chosen_key]['description'].replace('\n', '\n > '))) - if key_info.has_allowed_types: - allowed_types = self.op_params.default_params[chosen_key]['allowed_types'] - to_print.append('>> Allowed types: {}'.format(', '.join([str(i).split("'")[1] for i in allowed_types]))) - if key_info.live: - to_print.append('>> This parameter supports live tuning! Updates should take affect within 5 seconds.\n') - - if to_print: - print('\n{}\n'.format('\n'.join(to_print))) - - print('Current value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) - if key_info.live or self.live_tuning: # similar to opTune - while True: - print('Enter your new value:') - new_value = input('>> ').strip() - if new_value == '': - self.message('Exiting this parameter...') - return - - new_value = self.parse_input(new_value) - if key_info.has_allowed_types and type(new_value) not in allowed_types: - self.message('The type of data you entered ({}) is not allowed with this parameter!'.format(str(type(new_value)).split("'")[1])) - continue - - self.op_params.put(chosen_key, new_value) - print('Saved {} with value: {}! (type: {})\n'.format(chosen_key, new_value, str(type(new_value)).split("'")[1])) - else: - print('Enter your new value:') - new_value = input('>> ').strip() - if new_value == '': - self.message('Exiting this parameter...') - return - - new_value = self.parse_input(new_value) - if key_info.has_allowed_types and type(new_value) not in allowed_types: - self.message('The type of data you entered ({}) is not allowed with this parameter!'.format(str(type(new_value)).split("'")[1])) - continue - - print('\nOld value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) - print('New value: {} (type: {})'.format(new_value, str(type(new_value)).split("'")[1])) - print('Do you want to save this?') - choice = input('[Y/n]: ').lower().strip() - if choice == 'y': - self.op_params.put(chosen_key, new_value) - self.message('Saved!') - else: - self.message('Not saved!') - return - - def parse_input(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 - - def delete_parameter(self): - while True: - print('Enter the name of the parameter to delete:') - key = input('>> ').lower() - key = self.parse_input(key) - - if key == '': - return - if not isinstance(key, str): - self.message('Input must be a string!') - continue - if key not in self.params: - self.message("Parameter doesn't exist!") - continue - - value = self.params.get(key) - print('Parameter name: {}'.format(key)) - print('Parameter value: {} (type: {})'.format(value, str(type(value)).split("'")[1])) - print('Do you want to delete this?') - - choice = input('[Y/n]: ').lower().strip() - if choice == 'y': - self.op_params.delete(key) - self.message('Deleted!') - else: - self.message('Not saved!') - return - - def add_parameter(self): - while True: - print('Type the name of your new parameter:') - key = input('>> ').strip() - if key == '': - return - - key = self.parse_input(key) - - if not isinstance(key, str): - self.message('Input must be a string!') - continue - - print("Enter the data you'd like to save with this parameter:") - value = input('>> ').strip() - value = self.parse_input(value) - - print('Parameter name: {}'.format(key)) - print('Parameter value: {} (type: {})'.format(value, str(type(value)).split("'")[1])) - print('Do you want to save this?') - - choice = input('[Y/n]: ').lower().strip() - if choice == 'y': - self.op_params.put(key, value) - self.message('Saved!') - else: - self.message('Not saved!') - return - - def message(self, msg): - print('--------\n{}\n--------'.format(msg), flush=True) - time.sleep(self.sleep_time) - print() - - -opEdit() diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 593eb7e43..a4ca84feb 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -8,18 +8,22 @@ from common.realtime import sec_since_boot from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG -from common.op_params import opParams +# df + dp from common.numpy_fast import interp, clip -from common.travis_checker import travis from selfdrive.config import Conversions as CV +from common.params import Params +from selfdrive.dragonpilot.dragonconf import dp_get_last_modified LOG_MPC = os.environ.get('LOG_MPC', False) +# df profile +DF_PROFILE_ROADTRIP = -1 +DF_PROFILE_STANDARD = 0 +DF_PROFILE_TRAFFIC = 1 class LongitudinalMpc(): def __init__(self, mpc_id): self.mpc_id = mpc_id - self.op_params = opParams() self.setup_mpc() self.v_mpc = 0.0 @@ -31,13 +35,19 @@ class LongitudinalMpc(): self.new_lead = False self.last_cloudlog_t = 0.0 + # df from Shane Smiskol self.car_data = {'v_ego': 0.0, 'a_ego': 0.0} self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False} self.df_data = {"v_leads": [], "v_egos": []} # dynamic follow data self.last_cost = 0.0 - self.df_profile = self.op_params.get('dynamic_follow', 'relaxed').strip().lower() + self.df_profile = DF_PROFILE_STANDARD self.sng = False + # dragonpilot + self.params = Params() + self.last_ts = 0 + self.last_modified = 0 + def send_mpc_solution(self, pm, qp_iterations, calculation_time): qp_iterations = max(0, qp_iterations) dat = messaging.new_message() @@ -70,14 +80,13 @@ class LongitudinalMpc(): self.cur_state[0].a_ego = a def get_TR(self, CS): - if not self.lead_data['status'] or travis: + if not self.lead_data['status']: TR = 1.8 else: self.store_df_data() TR = self.dynamic_follow(CS) - if not travis: - self.change_cost(TR) + self.change_cost(TR) return TR def change_cost(self, TR): @@ -119,14 +128,25 @@ class LongitudinalMpc(): return a_lead # if above doesn't execute, we'll return a_lead from radar def dynamic_follow(self, CS): - self.df_profile = self.op_params.get('dynamic_follow', 'relaxed').strip().lower() + ts = sec_since_boot() + if ts - self.last_ts >= 10.: + modified = dp_get_last_modified() + if self.last_modified != modified: + try: + self.df_profile = int(self.params.get('DragonDynamicFollow', encoding='utf8')) + self.df_profile = self.df_profile if self.df_profile in [DF_PROFILE_STANDARD, DF_PROFILE_ROADTRIP, DF_PROFILE_TRAFFIC] else DF_PROFILE_STANDARD + except TypeError: + self.df_profile = DF_PROFILE_STANDARD + self.last_modified = modified + self.last_ts = ts + x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities profile_mod_x = [2.2352, 13.4112, 24.5872, 35.7632] # profile mod speeds, mph: [5., 30., 55., 80.] - if self.df_profile == 'roadtrip': + if self.df_profile == DF_PROFILE_ROADTRIP: y_dist = [1.3847, 1.3946, 1.4078, 1.4243, 1.4507, 1.4837, 1.5327, 1.553, 1.581, 1.617, 1.653, 1.687, 1.74] # TRs profile_mod_pos = [0.99, 0.9025, 0.815, 0.55] profile_mod_neg = [1.0, 1.18, 1.382, 1.787] - elif self.df_profile == 'traffic': # for in congested traffic + elif self.df_profile == DF_PROFILE_TRAFFIC: # for in congested traffic x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35] y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] profile_mod_pos = [1.05, 1.375, 2.99, 3.8] @@ -167,7 +187,7 @@ class LongitudinalMpc(): TR_mod = sum([mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos for mod in TR_mod]) # alter TR modification according to profile TR += TR_mod - if CS.leftBlinker or CS.rightBlinker and self.df_profile != 'traffic': + if CS.leftBlinker or CS.rightBlinker and self.df_profile != DF_PROFILE_TRAFFIC: x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph y = [1.0, .75, .65] # reduce TR when changing lanes TR *= interp(self.car_data['v_ego'], x, y) diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 9437308ba..2c199378f 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -69,7 +69,8 @@ default_conf = { 'DragonBTG': 0, 'DragonBootHotspot': 0, 'DragonAccelProfile': '0', - 'DragonLastModified': str(floor(time.time())) + 'DragonLastModified': str(floor(time.time())), + 'DragonDynamicFollow': '0', # traffic = -1, stock = 0, roadtrip = 1 } deprecated_conf = { From 38002aa6c9e7f4a50626d0c50924d8ac63df2733 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Thu, 12 Mar 2020 15:03:59 +1000 Subject: [PATCH 5/9] fix apk From 27a45992c5c92093f6504bcbd261e055b9cf4678 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Fri, 13 Mar 2020 12:03:06 +1000 Subject: [PATCH 6/9] fix up df language From 5ba53538d8f76f863ba4d0f5fd7cf5854c3dc08f Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Sun, 15 Mar 2020 18:29:14 +1000 Subject: [PATCH 7/9] bump df offroad apk to 0.7.4 From c39a1e3e40415b5d5bed1e9adefc034b607db297 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Mon, 16 Mar 2020 16:57:37 +1000 Subject: [PATCH 8/9] add 'OFF' option to dynamic follow profile (stock mode) --- selfdrive/controls/lib/long_mpc.py | 46 +++++++++++--------- selfdrive/dragonpilot/dragonconf/__init__.py | 2 +- 2 files changed, 26 insertions(+), 22 deletions(-) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 278550ed1..35cfde668 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -17,9 +17,10 @@ from selfdrive.dragonpilot.dragonconf import dp_get_last_modified LOG_MPC = os.environ.get('LOG_MPC', False) # df profile -DF_PROFILE_ROADTRIP = -1 -DF_PROFILE_STANDARD = 0 -DF_PROFILE_TRAFFIC = 1 +DF_PROFILE_OFF = -2 +DF_PROFILE_LONG = -1 +DF_PROFILE_NORMAL = 0 +DF_PROFILE_SHORT = 1 class LongitudinalMpc(): def __init__(self, mpc_id): @@ -40,7 +41,7 @@ class LongitudinalMpc(): self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False} self.df_data = {"v_leads": [], "v_egos": []} # dynamic follow data self.last_cost = 0.0 - self.df_profile = DF_PROFILE_STANDARD + self.df_profile = DF_PROFILE_OFF self.sng = False # dragonpilot @@ -79,13 +80,28 @@ class LongitudinalMpc(): self.cur_state[0].a_ego = a def get_TR(self, CS): - if not self.lead_data['status']: + # load profile + ts = sec_since_boot() + if ts - self.last_ts >= 10.: + modified = dp_get_last_modified() + if self.last_modified != modified: + try: + self.df_profile = int(self.params.get('DragonDynamicFollow', encoding='utf8')) + self.df_profile = self.df_profile if self.df_profile in [DF_PROFILE_OFF, DF_PROFILE_LONG, DF_PROFILE_NORMAL, DF_PROFILE_SHORT] else DF_PROFILE_OFF + except TypeError: + self.df_profile = DF_PROFILE_OFF + self.last_modified = modified + self.last_ts = ts + + if not self.lead_data['status'] or self.df_profile == DF_PROFILE_OFF: TR = 1.8 else: self.store_df_data() TR = self.dynamic_follow(CS) - self.change_cost(TR) + # only change cost when profile is not off + if not self.df_profile == DF_PROFILE_OFF: + self.change_cost(TR) return TR def change_cost(self, TR): @@ -127,25 +143,13 @@ class LongitudinalMpc(): return a_lead # if above doesn't execute, we'll return a_lead from radar def dynamic_follow(self, CS): - ts = sec_since_boot() - if ts - self.last_ts >= 10.: - modified = dp_get_last_modified() - if self.last_modified != modified: - try: - self.df_profile = int(self.params.get('DragonDynamicFollow', encoding='utf8')) - self.df_profile = self.df_profile if self.df_profile in [DF_PROFILE_STANDARD, DF_PROFILE_ROADTRIP, DF_PROFILE_TRAFFIC] else DF_PROFILE_STANDARD - except TypeError: - self.df_profile = DF_PROFILE_STANDARD - self.last_modified = modified - self.last_ts = ts - x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities profile_mod_x = [2.2352, 13.4112, 24.5872, 35.7632] # profile mod speeds, mph: [5., 30., 55., 80.] - if self.df_profile == DF_PROFILE_ROADTRIP: + if self.df_profile == DF_PROFILE_LONG: y_dist = [1.3847, 1.3946, 1.4078, 1.4243, 1.4507, 1.4837, 1.5327, 1.553, 1.581, 1.617, 1.653, 1.687, 1.74] # TRs profile_mod_pos = [0.99, 0.9025, 0.815, 0.55] profile_mod_neg = [1.0, 1.18, 1.382, 1.787] - elif self.df_profile == DF_PROFILE_TRAFFIC: # for in congested traffic + elif self.df_profile == DF_PROFILE_SHORT: # for in congested traffic x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35] y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] profile_mod_pos = [1.05, 1.375, 2.99, 3.8] @@ -186,7 +190,7 @@ class LongitudinalMpc(): TR_mod = sum([mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos for mod in TR_mod]) # alter TR modification according to profile TR += TR_mod - if CS.leftBlinker or CS.rightBlinker and self.df_profile != DF_PROFILE_TRAFFIC: + if CS.leftBlinker or CS.rightBlinker and self.df_profile != DF_PROFILE_SHORT: x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph y = [1.0, .75, .65] # reduce TR when changing lanes TR *= interp(self.car_data['v_ego'], x, y) diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index a789e5e30..9bd27b33d 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -71,7 +71,7 @@ default_conf = { 'DragonAccelProfile': '0', 'DragonLastModified': str(floor(time.time())), 'DragonEnableRegistration': '1', - 'DragonDynamicFollow': '0', # traffic = -1, stock = 0, roadtrip = 1 + 'DragonDynamicFollow': '-2', # OFF = -2, LONG = -1, NORMAL = 0, SHORT = 1 } deprecated_conf = { From ae6a923d4df832a9b5b24de09a2aff8e0c952ac1 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Mon, 16 Mar 2020 17:13:54 +1000 Subject: [PATCH 9/9] remove alexnoop's fp for now --- selfdrive/car/honda/values.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 766733212..bf4786f98 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -76,10 +76,10 @@ FINGERPRINTS = { # 2017 Civic Hatchback EX, 2019 Civic Sedan Touring Canadian, and 2018 Civic Hatchback Executive Premium 1.0L CVT European 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 460: 3, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1625: 5, 1629: 5, 1633: 8, }, - # Manual CIVIC from AlexNoop - { - 57: 3, 148: 8, 228: 5, 274: 3, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 460: 3, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1029: 8, 1036: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1625: 5, 1633: 8, - }, + # # Manual CIVIC from AlexNoop + # { + # 57: 3, 148: 8, 228: 5, 274: 3, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 460: 3, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1029: 8, 1036: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1625: 5, 1633: 8, + # }, # 2017 Civic Hatchback LX { 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 815: 8, 825: 4, 829: 5, 846: 8, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 892: 8, 918: 7, 927: 8, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1092: 1, 1108: 8, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1633: 8