forked from commaai/openpilot
-
-
Notifications
You must be signed in to change notification settings - Fork 88
/
__init__.py
367 lines (297 loc) · 16.2 KB
/
__init__.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
import math
import numpy as np
import cereal.messaging as messaging
from common.realtime import sec_since_boot
from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
from common.op_params import opParams
from common.numpy_fast import interp, clip, mean
from selfdrive.config import Conversions as CV
from cereal.messaging import SubMaster
from selfdrive.controls.lib.dynamic_follow.auto_df import predict
from selfdrive.controls.lib.dynamic_follow.df_manager import dfManager
from selfdrive.controls.lib.dynamic_follow.support import LeadData, CarData, dfData, dfProfiles
from common.data_collector import DataCollector
travis = False
class DistanceModController:
def __init__(self, k_i, k_d, x_clip, mods):
self._rate = 1 / 20.
self._k_i = k_i
self._k_d = k_d
self._to_clip = x_clip # reaches this with v_rel=3.5 mph for 4 seconds
self._mods = mods
self.i = 0 # never resets, even when new lead
self.last_error = 0
def update(self, error):
"""
Relative velocity is a good starting point
Returns: Multiplier for final y_dist output
"""
if (d := self._k_d * (error - self.last_error)) < 0: # only add if it will add distance
self.i += d
self.i += error * self._rate * self._k_i
self.i = clip(self.i, self._to_clip[0], self._to_clip[-1]) # clip to reasonable range
self._slow_reset() # slowly reset from max to 0
fact = interp(self.i, self._to_clip, self._mods)
self.last_error = float(error)
# print("I: {}, FACT: {}".format(round(self.i, 4), round(fact, 3)))
return fact
def _slow_reset(self):
if abs(self.i) > 0.01: # oscillation starts around 0.006
reset_time = 15 # in x seconds i goes from max to 0
sign = 1 if self.i > 0 else -1
self.i -= sign * max(self._to_clip) / (reset_time / self._rate)
class DynamicFollow:
def __init__(self, mpc_id):
self.mpc_id = mpc_id
self.op_params = opParams()
self.df_profiles = dfProfiles()
self.df_manager = dfManager()
self.dmc_v_rel = DistanceModController(k_i=0.042, k_d=0.08, x_clip=[-1, 0, 0.66], mods=[1.15, 1., 0.95])
self.dmc_a_rel = DistanceModController(k_i=0.042 * 1.05, k_d=0.08, x_clip=[-1, 0, 0.33], mods=[1.15, 1., 0.98]) # a_lead loop is 5% faster
if not travis and mpc_id == 1:
self.pm = messaging.PubMaster(['dynamicFollowData'])
else:
self.pm = None
# Model variables
mpc_rate = 1 / 20.
self.model_scales = {'v_ego': [-0.06112159043550491, 37.96522521972656], 'a_lead': [-3.109330892562866, 3.3612186908721924], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 141.44000244140625]}
self.predict_rate = 1 / 4.
self.skip_every = round(0.25 / mpc_rate)
self.model_input_len = round(45 / mpc_rate)
# Dynamic follow variables
self.default_TR = 1.8
self.TR = 1.8
self.v_ego_retention = 2.5
self.v_rel_retention = 1.75
self.sng_TR = 1.8 # reacceleration stop and go TR
self.sng_speed = 18.0 * CV.MPH_TO_MS
self._setup_collector()
self._setup_changing_variables()
def _setup_collector(self):
self.sm_collector = SubMaster(['liveTracks', 'laneSpeed'])
self.log_auto_df = self.op_params.get('log_auto_df')
if not isinstance(self.log_auto_df, bool):
self.log_auto_df = False
self.data_collector = DataCollector(file_path='/data/df_data', keys=['v_ego', 'a_ego', 'a_lead', 'v_lead', 'x_lead', 'left_lane_speeds', 'middle_lane_speeds', 'right_lane_speeds', 'left_lane_distances', 'middle_lane_distances', 'right_lane_distances', 'profile', 'time'], log_data=self.log_auto_df)
def _setup_changing_variables(self):
self.TR = self.default_TR
self.user_profile = self.df_profiles.relaxed # just a starting point
self.model_profile = self.df_profiles.relaxed
self.last_effective_profile = self.user_profile
self.profile_change_time = 0
self.sng = False
self.car_data = CarData()
self.lead_data = LeadData()
self.df_data = dfData() # dynamic follow data
self.last_cost = 0.0
self.last_predict_time = 0.0
self.auto_df_model_data = []
self._get_live_params() # so they're defined just in case
def update(self, CS, libmpc):
self._get_live_params()
self._update_car(CS)
self._get_profiles()
if self.mpc_id == 1 and self.log_auto_df:
self._gather_data()
if not self.lead_data.status:
self.TR = self.default_TR
else:
self._store_df_data()
self.TR = self._get_TR()
if not travis:
self._change_cost(libmpc)
self._send_cur_state()
return self.TR
def _get_profiles(self):
"""This receives profile change updates from dfManager and runs the auto-df prediction if auto mode"""
df_out = self.df_manager.update()
self.user_profile = df_out.user_profile
if df_out.is_auto: # todo: find some way to share prediction between the two mpcs to reduce processing overhead
self._get_pred() # sets self.model_profile, all other checks are inside function
def _gather_data(self):
self.sm_collector.update(0)
# live_tracks = [[i.dRel, i.vRel, i.aRel, i.yRel] for i in self.sm_collector['liveTracks']]
if self.car_data.cruise_enabled:
self.data_collector.update([self.car_data.v_ego,
self.car_data.a_ego,
self.lead_data.a_lead,
self.lead_data.v_lead,
self.lead_data.x_lead,
list(self.sm_collector['laneSpeed'].leftLaneSpeeds),
list(self.sm_collector['laneSpeed'].middleLaneSpeeds),
list(self.sm_collector['laneSpeed'].rightLaneSpeeds),
list(self.sm_collector['laneSpeed'].leftLaneDistances),
list(self.sm_collector['laneSpeed'].middleLaneDistances),
list(self.sm_collector['laneSpeed'].rightLaneDistances),
self.user_profile,
sec_since_boot()])
def _norm(self, x, name):
self.x = x
return interp(x, self.model_scales[name], [0, 1])
def _send_cur_state(self):
if self.mpc_id == 1 and self.pm is not None:
dat = messaging.new_message('dynamicFollowData')
dat.dynamicFollowData.mpcTR = self.TR
dat.dynamicFollowData.profilePred = self.model_profile
self.pm.send('dynamicFollowData', dat)
def _change_cost(self, libmpc):
TRs = [0.9, 1.8, 2.7]
costs = [1., 0.1, 0.01]
cost = interp(self.TR, TRs, costs)
# change_time = sec_since_boot() - self.profile_change_time
# change_time_x = [0, 0.5, 4] # for three seconds after effective profile has changed
# change_mod_y = [3, 6, 1] # multiply cost by multiplier to quickly change distance
# if change_time < change_time_x[-1]: # if profile changed in last 3 seconds
# cost_mod = interp(change_time, change_time_x, change_mod_y)
# cost_mod_speeds = [0, 20 * CV.MPH_TO_MS] # don't change cost too much under 20 mph
# cost_mods = [cost_mod * 0.01, cost_mod]
# cost *= interp(cost_mod, cost_mod_speeds, cost_mods)
if self.last_cost != cost:
libmpc.change_costs(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) # todo: jerk is the derivative of acceleration, could tune that
self.last_cost = cost
def _store_df_data(self):
cur_time = sec_since_boot()
# Store custom relative accel over time
if self.lead_data.status:
if self.lead_data.new_lead:
self.df_data.v_rels = [] # reset when new lead
else:
self.df_data.v_rels = self._remove_old_entries(self.df_data.v_rels, cur_time, self.v_rel_retention)
self.df_data.v_rels.append({'v_ego': self.car_data.v_ego, 'v_lead': self.lead_data.v_lead, 'time': cur_time})
# Store our velocity for better sng
self.df_data.v_egos = self._remove_old_entries(self.df_data.v_egos, cur_time, self.v_ego_retention)
self.df_data.v_egos.append({'v_ego': self.car_data.v_ego, 'time': cur_time})
# Store data for auto-df model
self.auto_df_model_data.append([self._norm(self.car_data.v_ego, 'v_ego'),
self._norm(self.lead_data.v_lead, 'v_lead'),
self._norm(self.lead_data.a_lead, 'a_lead'),
self._norm(self.lead_data.x_lead, 'x_lead')])
while len(self.auto_df_model_data) > self.model_input_len:
del self.auto_df_model_data[0]
def _get_pred(self):
cur_time = sec_since_boot()
if self.car_data.cruise_enabled and self.lead_data.status:
if cur_time - self.last_predict_time > self.predict_rate:
if len(self.auto_df_model_data) == self.model_input_len:
pred = predict(np.array(self.auto_df_model_data[::self.skip_every], dtype=np.float32).flatten())
self.last_predict_time = cur_time
self.model_profile = int(np.argmax(pred))
@staticmethod
def _remove_old_entries(lst, cur_time, retention):
return [sample for sample in lst if cur_time - sample['time'] <= retention]
def _relative_accel_mod(self):
"""
Returns relative acceleration mod calculated from list of lead and ego velocities over time (longer than 1s)
If min_consider_time has not been reached, uses lead accel and ego accel from openpilot (kalman filtered)
"""
a_ego = self.car_data.a_ego
a_lead = self.lead_data.a_lead
min_consider_time = 0.75 # minimum amount of time required to consider calculation
if len(self.df_data.v_rels) > 0: # if not empty
elapsed_time = self.df_data.v_rels[-1]['time'] - self.df_data.v_rels[0]['time']
if elapsed_time > min_consider_time:
a_ego = (self.df_data.v_rels[-1]['v_ego'] - self.df_data.v_rels[0]['v_ego']) / elapsed_time
a_lead = (self.df_data.v_rels[-1]['v_lead'] - self.df_data.v_rels[0]['v_lead']) / elapsed_time
mods_x = [-1.5, -.75, 0]
mods_y = [1, 1.25, 1.3]
if a_lead < 0: # more weight to slight lead decel
a_lead *= interp(a_lead, mods_x, mods_y)
if a_lead - a_ego > 0: # return only if adding distance
return 0
rel_x = [-2.6822, -1.7882, -0.8941, -0.447, -0.2235, 0.0, 0.2235, 0.447, 0.8941, 1.7882, 2.6822]
mod_y = [0.3245 * 1.1, 0.277 * 1.08, 0.11075 * 1.06, 0.08106 * 1.045, 0.06325 * 1.035, 0.0, -0.09, -0.09375, -0.125, -0.3, -0.35]
return interp(a_lead - a_ego, rel_x, mod_y)
def global_profile_mod(self, x_vel, y_dist):
"""
This function modifies the y_dist list used by dynamic follow in accordance with global_df_mod
"""
if self.global_df_mod == 1.:
return y_dist
global_df_mod = 1 - self.global_df_mod
# Calculate new TRs
speeds, mods = [0.], [1.] # if increasing don't limit
if self.global_df_mod < 1: # if reducing distance
speeds = [0, self.sng_speed, 18, x_vel[-1]] # [0, 18 mph, ~40 mph, highest profile mod speed (~78 mph)]
mods = [0, 0.25, 0.75, 1] # how much to limit global_df_mod at each speed, 1 is full effect
return [y - (y * global_df_mod * interp(x, speeds, mods)) for x, y in zip(x_vel, y_dist)]
def _get_TR(self):
if self.df_manager.is_auto: # decide which profile to use, model profile will be updated before this
df_profile = self.model_profile
else:
df_profile = self.user_profile
if df_profile != self.last_effective_profile:
self.profile_change_time = sec_since_boot()
self.last_effective_profile = df_profile
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
if df_profile == self.df_profiles.stock:
return 1.8
elif df_profile == self.df_profiles.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.3457, 1.3134, 1.3145, 1.318, 1.3485, 1.257, 1.144, 0.979, 0.9461, 0.9156]
elif df_profile == self.df_profiles.relaxed: # default to relaxed/stock
y_dist = [1.411, 1.418, 1.428, 1.441, 1.461, 1.49, 1.535, 1.561, 1.589, 1.612, 1.621, 1.632, 1.648]
else:
raise Exception('Unknown profile type: {}'.format(df_profile))
# Global df mod
y_dist = self.global_profile_mod(x_vel, y_dist)
v_rel_dist_factor = self.dmc_v_rel.update(self.lead_data.v_lead - self.car_data.v_ego)
a_lead_dist_factor = self.dmc_a_rel.update(self.lead_data.a_lead - self.car_data.a_ego)
TR = interp(self.car_data.v_ego, x_vel, y_dist)
TR *= v_rel_dist_factor
TR *= a_lead_dist_factor
if self.car_data.v_ego > self.sng_speed: # keep sng distance until we're above sng speed again
self.sng = False
if (self.car_data.v_ego >= self.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 = [self.sng_speed * 0.7, self.sng_speed] # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating
y = [self.sng_TR, interp(self.sng_speed, x_vel, y_dist)]
TR = interp(self.car_data.v_ego, x, y)
return float(clip(TR, self.min_TR, 2.7))
TR_mods = []
# Dynamic follow modifications (the secret sauce)
x = [-26, -15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68, 4.4704] # relative velocity values
y = [1.76, 1.504, 1.34, 1.29, 1.25, 1.22, 1.19, 1.13, 1.053, 1.017, 1.0, 0.985, 0.958, 0.87, 0.81, 0.685] # multiplier values
y = np.array(y) - 1 # converts back to original abs mod
y *= 1.1 # multiplier for how much to mod
y = y / TR + 1 # converts back to multipliers
TR_mods.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 = [1.16, 1.1067, 1.0613, 1.0343, 1.0203, 1.0147, 1.0, 0.9898, 0.972, 0.9647, 0.9607] # multiplier values
converted_with_TR = 1.5 # todo: do without numpy and simplify by converting with TR of 1, so only subtract
absolute_y_TR_mod = np.array(y) * converted_with_TR - converted_with_TR # converts back to original abs mod
absolute_y_TR_mod *= 1.2 # multiplier for how much to mod
y = absolute_y_TR_mod / TR + 1 # converts back to multipliers with accel mod of 1.4 taking current TR into account
TR_mods.append(interp(self.get_rel_accel(), x, y)) # todo: make this over more than 1 sec
# deadzone = self.car_data.v_ego / 3 # 10 mph at 30 mph # todo: tune pedal to react similarly to without before adding/testing this
# if self.lead_data.v_lead - deadzone > self.car_data.v_ego:
# TR_mods.append(self._relative_accel_mod())
# x = [self.sng_speed, self.sng_speed / 5.0] # as we approach 0, apply x% more distance
# y = [1.0, 1.05]
TR *= mean(TR_mods) # with mods as multipliers, profile mods shouldn't be needed
# if (self.car_data.left_blinker or self.car_data.right_blinker) and df_profile != self.df_profiles.traffic:
# x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph
# y = [1.0, .75, .65]
# TR *= interp(self.car_data.v_ego, x, y) # reduce TR when changing lanes
return float(clip(TR, self.min_TR, 2.7))
def update_lead(self, v_lead=None, a_lead=None, x_lead=None, status=False, new_lead=False):
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
self.lead_data.new_lead = new_lead
def _update_car(self, CS):
self.car_data.v_ego = CS.vEgo
self.car_data.a_ego = CS.aEgo
self.car_data.left_blinker = CS.leftBlinker
self.car_data.right_blinker = CS.rightBlinker
self.car_data.cruise_enabled = CS.cruiseState.enabled
def _get_live_params(self):
self.global_df_mod = self.op_params.get('global_df_mod')
if self.global_df_mod != 1.:
self.global_df_mod = clip(self.global_df_mod, 0.85, 2.5)
self.min_TR = self.op_params.get('min_TR')
if self.min_TR != 1.:
self.min_TR = clip(self.min_TR, 0.85, 2.7)