From 432d7b4f98809844b027afb49488ed336a7862c3 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 10 Mar 2020 03:07:31 -0500 Subject: [PATCH 1/3] 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/3] 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/3] 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))