Skip to content

Commit

Permalink
Add real-time tuning functionality
Browse files Browse the repository at this point in the history
  • Loading branch information
James-T1 authored and James-T1 committed Oct 22, 2018
1 parent 5c66101 commit 368dc7f
Show file tree
Hide file tree
Showing 5 changed files with 271 additions and 0 deletions.
119 changes: 119 additions & 0 deletions selfdrive/controls/controlsd.py
100755 → 100644
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,11 @@
from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.driver_monitor import DriverStatus
try: # Real-time tuning
import cPickle as pickle
except ImportError:
import pickle
import os

ThermalStatus = log.ThermalData.ThermalStatus
State = log.Live100Data.ControlState
Expand Down Expand Up @@ -504,6 +509,11 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):

prof = Profiler(False) # off by default

# Setup for real-time tuning
rt_tuning_file = '/data/.openpilot_rtt_params.pkl'
rtt_params = {}
last_mod_time = 0

while 1:

prof.checkpoint("Ratekeeper", ignore=True)
Expand Down Expand Up @@ -533,6 +543,115 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive)
prof.checkpoint("Sent")

###################### Real-Time Tuning Add-on ########################
# TODO: Move this into it's own function to clean things up
# Run this once per second... on frame 29, of course.
if rk.frame % 100 == 29:
# Get the last update time of our real-time tuning file
#print('Real-Time Tuning: Checking tuning file modification time.')
try:
mod_time = os.path.getmtime(rt_tuning_file)
#print('RTT mod_time: {0}'.format(mod_time))
except OSError:
# File doesn't exist or is inaccessible
mod_time = None
print('Real-Time Tuning: RT_TUNING_FILE did not exist or was inaccessible.')

# If rt_tuning_file doesn't exist, then create it from the current CarParams:
if not mod_time:
rtt_params['steerKpBP'] = list(CP.steerKpBP) # Note that the Kp/Ki are lists! But if you reference them directly they are <capnp list builder []>.. oops.
rtt_params['steerKpV'] = list(CP.steerKpV)
rtt_params['steerKiBP'] = list(CP.steerKiBP)
rtt_params['steerKiV'] = list(CP.steerKiV)
rtt_params['steerKf'] = CP.steerKf
# TODO: Give the option to link the front and rear tire stiffness changes together
rtt_params['tireStiffnessFront'] = CP.tireStiffnessFront
rtt_params['tireStiffnessRear'] = CP.tireStiffnessRear
rtt_params['steerRatio'] = CP.steerRatio
rtt_params['steerRateCost'] = CP.steerRateCost
# Write the pickle file
# TODO: try/except the open
with open(rt_tuning_file, "wb") as f_write:
pickle.dump(rtt_params, f_write, -1) # Dump to file with highest protocol (fastest)
# No need to update next time if we just wrote the file out...
last_mod_time = os.path.getmtime(rt_tuning_file)
#print('RTT Last_mod_time: {0}'.format(last_mod_time))

# If file exists and has been updated since the last time we read it in
elif last_mod_time != mod_time:
print('Real-Time Tuning: Reading in the modified tuning file.')
# Read in parameters from file
# TODO: try/except the open
with open(rt_tuning_file, "rb") as f_read:
rtt_params = pickle.load(f_read)
# Sanity check the data before setting it.. format is [min, max, failsafe]
# Failsafe is used if a value is not found or if the value sent is out of the range limits
rt_data_limits = { 'steerKpBP': [ 0.0 , 67.0, 0.0 ],
'steerKpV': [ 0.0 , 1.0, 0.2 ],
'steerKiBP': [ 0.0 , 67.0, 0.0 ],
'steerKiV': [ 0.0 , 1.0, 0.05 ],
'steerKf': [ 0.0, 0.001, 0.00005 ],
'tireStiffnessFront': [ 20000, 1000000, 192150 ],
'tireStiffnessRear': [ 20000, 1000000, 202500 ],
'steerRatio': [ 8.0, 25.0, 14.0 ],
'steerRateCost': [ 0.05, 1.0, 0.5 ]
}
# Do the checks and set the values
for key in rt_data_limits:
rt_val = rtt_params.get(key)
if not rt_val:
# If this key from data limits doesn't exist in our tuning data, then add it as the failsafe
# TODO: Use CP value here instead of failsafe?
rtt_params[key] = rt_data_limits[key][2]
print('Real-Time Tuning: Value did not exist. Key: ' + key)
continue
# If it does exist, then check the values. First see if it's a list
try:
# If it's an iterable list...
for i, val2 in enumerate(rt_val):
# Check each value in the list
if (val2 < rt_data_limits[key][0]) or (val2 > rt_data_limits[key][1]):
rt_val[i] = rt_data_limits[key][2]
print('Real-Time Tuning: Invalid value replaced! Key: ' + key)
except:
# Not interable, compare it and fix if necessary
if (rt_val < rt_data_limits[key][0]) or (rt_val > rt_data_limits[key][1]):
rt_val = rt_data_limits[key][2]
print('Real-Time Tuning: Invalid value replaced! Key: ' + key)
# Set it back so if anything was fixed we have the updated value
rtt_params[key] = rt_val

# Update CP with the new params
CP.steerKpBP = rtt_params['steerKpBP']
CP.steerKpV = rtt_params['steerKpV']
CP.steerKiBP = rtt_params['steerKiBP']
CP.steerKiV = rtt_params['steerKiV']
CP.steerKf = rtt_params['steerKf']
CP.tireStiffnessFront = rtt_params['tireStiffnessFront']
CP.tireStiffnessRear = rtt_params['tireStiffnessRear']
CP.steerRatio = rtt_params['steerRatio']
if CP.steerRateCost != rtt_params['steerRateCost']:
print(CP.steerRateCost)
print(rtt_params['steerRateCost'])
CP.steerRateCost = rtt_params['steerRateCost']
rt_mpc_flag = True
print('Real-Time Tuning: CP.steerRateCost changed - Re-initializing lateral MPC.')
else:
rt_mpc_flag = False
# TODO: try/except the open
# Write the pickle file back so if we fixed any data errors the revised values will show up on the client-side
with open(rt_tuning_file, "wb") as f_write:
pickle.dump(rtt_params, f_write, -1) # Dump to file with highest protocol (fastest)
# Set the last modified time to this write.... we don't need to read back in what we just wrote out
# Only set this if we were able to successfully make the write (once the try/except is added)
last_mod_time = os.path.getmtime(rt_tuning_file)
# Make updates in latcontrol, etc. I'm not sure if this is actually necessary, depends on if the objects are referenced or not. Anyway, one less thing to debug atm.
VM.update_rt_params(CP)
LaC.update_rt_params(VM, rt_mpc_flag)
#print('RTT Last_mod_time: {0}'.format(last_mod_time))

####### END OF REAL-TIME TUNING ADD-ON #######

# *** run loop at fixed rate ***
rk.keep_time()

Expand Down
18 changes: 18 additions & 0 deletions selfdrive/controls/lib/latcontrol.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,16 @@ def __init__(self, VM):
self.last_cloudlog_t = 0.0
self.setup_mpc(VM.CP.steerRateCost)

def update_rt_params(self, VM, rt_mpc_flag):
# TODO: Is this really necessary, or is the original reference preserved through the cap n' proto setup?
# Real-time tuning: Update these values from the CP if called from real-time tuning logic in controlsd
self.pid._k_p = (VM.CP.steerKpBP, VM.CP.steerKpV) # proportional gain
self.pid._k_i = (VM.CP.steerKiBP, VM.CP.steerKiV) # integral gain
self.pid.k_f = VM.CP.steerKf # feedforward gain
# Re-init the MPC with the new steerRateCost if it changed
if rt_mpc_flag:
self.rtt_reset_mpc = True

def setup_mpc(self, steer_rate_cost):
self.libmpc = libmpc_py.libmpc
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost)
Expand All @@ -48,6 +58,7 @@ def setup_mpc(self, steer_rate_cost):
self.angle_steers_des_mpc = 0.0
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
self.rtt_reset_mpc = False

def reset(self):
self.pid.reset()
Expand Down Expand Up @@ -86,6 +97,13 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs
self.angle_steers_des_time = cur_time
self.mpc_updated = True

# Real-Time Tuning: Reset MPC if steerRateCost changed
# TODO: Figure out if this is the best way to accomplish the real-time change to steerRateCost
if self.rtt_reset_mpc:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, VM.CP.steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio
self.rtt_reset_mpc = False

# Check for infeasable MPC solution
self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
Expand Down
13 changes: 13 additions & 0 deletions selfdrive/controls/lib/vehicle_model.py
100755 → 100644
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,19 @@ def __init__(self, CP, init_state=np.asarray([[0.], [0.]])):
self.sR = CP.steerRatio
self.chi = CP.steerRatioRear

def update_rt_params(self, CP):
# Update parameters used in real-time tuning if called from real-time tuning logic in controlsd
# TODO: Determine if really necessary
#self.m = CP.mass
#self.j = CP.rotationalInertia
#self.l = CP.wheelbase
#self.aF = CP.centerToFront
#self.aR = CP.wheelbase - CP.centerToFront
self.cF = CP.tireStiffnessFront
self.cR = CP.tireStiffnessRear
self.sR = CP.steerRatio
#self.chi = CP.steerRatioRear

def update_state(self, state):
self.state = state

Expand Down
108 changes: 108 additions & 0 deletions selfdrive/rtt_cl_interface.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,108 @@
# Quick and dirty real-time tuning interface from the command-line
# Will only work once controlsd has had a chance to write the real-time tuning carparams the first time.
#
# v0.01 James-T1: Initial code

import pickle
import sys

def isDigit(x):
try:
float(x)
return True
except (ValueError, TypeError) as e:
return False

rt_tuning_file = '/data/.openpilot_rtt_params.pkl'

# Loop forever (ctrl-C and then Enter key to end script)
while 1:
with open(rt_tuning_file, "rb") as f_read:
rtt_params = pickle.load(f_read)

key_list = []

print('')
cnt = 0
for key in sorted(rtt_params.keys()):
print('{0}: {1}'.format(cnt, key))
key_list.append(key)
cnt += 1

print('')
sys.stdin = open('/dev/tty')
entry = raw_input('Enter parameter number to modify: ')

# Data checking
try:
int(entry)
except ValueError:
print ('Please re-enter a valid parameter number.')
continue
param_num = int(entry)
if param_num < 0 or param_num >= len(key_list):
print('Please re-enter a valid parameter number.')
continue

print('')
print('Old value:')
key = key_list[param_num]
original_param_is_list = False
if isDigit(rtt_params[key]):
print(' {0}: {1:.6f}'.format(key, rtt_params[key]))
else:
print(' {0}: {1}'.format(key, rtt_params[key]))
original_param_is_list = True
print('')
entry = raw_input('Enter new value: ')
print('')
#print(entry)
# Check to see if a list was entered... basically anything with a comma.
if ',' in entry or ('[' in entry and ']' in entry):
if not original_param_is_list:
print('Original value was float, new value entered is a list. Try again.')
print('')
continue
entry = entry.replace('[','').replace(']','')
processed_entry = [float(s) for s in entry.split(',') if isDigit(s)]
if len(processed_entry) == 0:
print('Invalid list entry. Try again.')
print('')
continue
if len(processed_entry) != len(rtt_params[key]):
print('New list length does not match length of original list. Try again.')
print('')
continue
elif isDigit(entry):
if original_param_is_list:
print('Original value was list, new value entered is a float. Try again.')
print('')
continue
processed_entry = float(entry)
else:
print('Invalid value entered. Try again.')
print('')
continue

print('New value:')
if isDigit(processed_entry):
print(' {0}: {1:.6f}'.format(key, processed_entry))
else:
# must be a list.
print(' {0}: {1}'.format(key, processed_entry))

print('')
confirm = raw_input('Type "y" to confirm + save or any other key to escape: ')
if confirm.lower() == 'y':
print('Confirmed. Writing to real-time tuning file.')
print('')
# Set it to this value
rtt_params[key] = processed_entry
# Save the file
with open(rt_tuning_file, "wb") as f_write:
pickle.dump(rtt_params, f_write, -1) # Dump to file with highest protocol (fastest)
else:
print('Escaped!')
print('')


13 changes: 13 additions & 0 deletions selfdrive/rtt_pickle_print.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
# Quick and dirty tool to print the real-time tuning data from the command line
# Will only work once controlsd has had a chance to write the real-time tuning carparams the first time.
#
# v0.01 James-T1: Initial code

import pickle

rt_tuning_file = '/data/.openpilot_rtt_params.pkl'

with open(rt_tuning_file, "rb") as f_read:
rtt_params = pickle.load(f_read)

print(rtt_params)

0 comments on commit 368dc7f

Please sign in to comment.