def _setup_collector(self):
self.sm_collector = SubMaster(['liveTracks'])
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', 'profile',
'time'
],
log_data=self.log_auto_df)
def send_request(url, request):
print('Size of request: ' + str(request.image.size))
start_time = time.time()
# Send full image to edge device for further processing
pickle.dump(request.image, open(PICKLED_TEMP_LOC, 'wb'))
resp = requests.post(url,
files={'file': open(PICKLED_TEMP_LOC, 'rb')},
verify=False)
status_code = resp.status_code
size_of_resp = len(resp.content)
total_size = str(request.image.size + size_of_resp)
resp = resp.json()
print('Size of response: ' + str(size_of_resp))
print('Total transfer size: ' + total_size)
duration = str(time.time() - start_time)
prediction = 'Count: ' + str(len(resp['prediction'])) + ', ' + str(
resp['prediction'])
print('Data Transfer Time: ' + duration)
print('Results: ' + prediction)
data_collector = DataCollector()
data_collector.write(DATA_TYPE_TRANSFER_SIZE, total_size)
data_collector.write(DATA_TYPE_DATA_TRANSFER_TIME, duration)
data_collector.write(DATA_TYPE_DETECTION, prediction)
return NodeResponse(cc.get_status(status_code),
cc.get_image(request.image, resp['prediction']),
resp['prediction'])
def run_camera(camera_chain, video_file_loc):
# buffers for demo in batch
buffer_inp = list()
# Loop through frames
elapsed = 0
start = timer()
data_collector = DataCollector()
print('Video file to open: ' + str(video_file_loc))
while True:
frame_num = 0
while frame_num < FRAME_LIMIT:
print('Press [ESC] to quit demo')
camera = cv2.VideoCapture(video_file_loc)
cam_num = UserParameters().get_cam_num()
while camera.isOpened():
elapsed += 1
_, frame = camera.read()
frame_num = frame_num + 1
if frame is None:
print('\nEnd of Video')
break
buffer_inp.append(frame)
# Only process and imshow when queue is full
if elapsed % QUEUE == 0:
for image in buffer_inp:
collector_start_time = timer()
process_image_response = process_img_request(
image, cam_num, frame_num,
camera_chain.get_processor())
cv2.imshow('', process_image_response.boxed_image)
#cv2.imshow('', image)
duration = str(timer() - collector_start_time)
print('Processing Time: ' + duration)
data_collector.write(DATA_TYPE_PROCESSING_TIME,
duration)
# Clear Buffers
buffer_inp = list()
if elapsed % 5 == 0:
sys.stdout.write('\r')
fps = elapsed / (timer() - start)
sys.stdout.write('{0:3.3f} FPS\n'.format(fps))
data_collector.write(DATA_TYPE_FPS, str(fps))
sys.stdout.flush()
choice = cv2.waitKey(1)
if choice == 27:
break
sys.stdout.write('\n')
camera.release()
data_collector.close()
cv2.destroyAllWindows()
#run_camera()
def predict(self, image):
boxes = self.model.predict(image)
image_annotated = draw_boxes(image, boxes, self.label_array)
boxes_str = 'Count: ' + str(len(boxes)) + ', ' + str([create_box_resp_field(box) for box in boxes])
print('Results: ' + boxes_str)
DataCollector().write(DATA_TYPE_DETECTION, boxes_str)
return image_annotated, boxes
def post_to_multi_view(request):
data = {
'cam':
request.cam_num,
'frame':
request.frame_num,
'prediction':
[create_box_resp_field(pred) for pred in request.predicted_box_loc]
}
size_of_request = asizeof.asizeof(data)
print('Size of request: ' + str(size_of_request))
start_time = time.time()
resp = requests.post(str(UserParameters().get_edge_ip()) +
MULTI_VIEW_RELATIVE_URL,
headers={'Content-Type': 'application/json'},
data=json.dumps(data),
verify=False)
status_code = resp.status_code
size_of_resp = asizeof.asizeof(resp.content)
total_size = str(size_of_request + size_of_resp)
resp = resp.json()
print('Size of response: ' + str(size_of_resp))
print('Total transfer size: ' + total_size)
duration = str(time.time() - start_time)
prediction = 'Count: ' + str(len(resp['prediction'])) + ', ' + str(
resp['prediction'])
print('Data Transfer Time: ' + duration)
print('Results: ' + prediction)
data_collector = DataCollector()
data_collector.write(DATA_TYPE_TRANSFER_SIZE, total_size)
data_collector.write(DATA_TYPE_DATA_TRANSFER_TIME, duration)
data_collector.write(DATA_TYPE_DETECTION, prediction)
return NodeResponse(cc.get_status(status_code),
cc.get_image(request.image, resp['prediction']),
resp['prediction'])
def predict(self, image):
class_to_color = {self.class_mapping[v]: np.random.randint(0, 255, 3) for v in self.class_mapping}
bbox_threshold = 0.8
X, ratio = format_img(image, self.C)
X = np.transpose(X, (0, 2, 3, 1))
[Y1, Y2, F] = self.model_rpn.predict(X)
R = roi_helpers.rpn_to_roi(Y1, Y2, K.image_dim_ordering(), overlap_thresh=0.7)
R[:, 2] -= R[:, 0]
R[:, 3] -= R[:, 1]
# apply the spatial pyramid pooling to the proposed regions
bboxes = {}
probs = {}
for jk in range(R.shape[0] // self.C.num_rois + 1):
ROIs = np.expand_dims(R[self.C.num_rois * jk:self.C.num_rois * (jk + 1), :], axis=0)
if ROIs.shape[1] == 0:
break
if jk == R.shape[0] // self.C.num_rois:
# pad R
curr_shape = ROIs.shape
target_shape = (curr_shape[0], self.C.num_rois, curr_shape[2])
ROIs_padded = np.zeros(target_shape).astype(ROIs.dtype)
ROIs_padded[:, :curr_shape[1], :] = ROIs
ROIs_padded[0, curr_shape[1]:, :] = ROIs[0, 0, :]
ROIs = ROIs_padded
[P_cls, P_regr] = self.model_classifier_only.predict([F, ROIs])
for ii in range(P_cls.shape[1]):
if np.max(P_cls[0, ii, :]) < bbox_threshold or np.argmax(P_cls[0, ii, :]) == (P_cls.shape[2] - 1):
continue
cls_name = self.class_mapping[np.argmax(P_cls[0, ii, :])]
if cls_name not in bboxes:
bboxes[cls_name] = []
probs[cls_name] = []
(x, y, w, h) = ROIs[0, ii, :]
cls_num = np.argmax(P_cls[0, ii, :])
try:
(tx, ty, tw, th) = P_regr[0, ii, 4 * cls_num:4 * (cls_num + 1)]
tx /= self.C.classifier_regr_std[0]
ty /= self.C.classifier_regr_std[1]
tw /= self.C.classifier_regr_std[2]
th /= self.C.classifier_regr_std[3]
x, y, w, h = roi_helpers.apply_regr(x, y, w, h, tx, ty, tw, th)
except:
pass
bboxes[cls_name].append(
[self.C.rpn_stride * x, self.C.rpn_stride * y, self.C.rpn_stride * (x + w), self.C.rpn_stride * (y + h)])
probs[cls_name].append(np.max(P_cls[0, ii, :]))
all_dets = []
for key in bboxes:
bbox = np.array(bboxes[key])
new_boxes, new_probs = roi_helpers.non_max_suppression_fast(bbox, np.array(probs[key]), overlap_thresh=0.5)
for jk in range(new_boxes.shape[0]):
(x1, y1, x2, y2) = new_boxes[jk, :]
(real_x1, real_y1, real_x2, real_y2) = get_real_coordinates(ratio, x1, y1, x2, y2)
cv2.rectangle(image, (real_x1, real_y1), (real_x2, real_y2),
(int(class_to_color[key][0]), int(class_to_color[key][1]), int(class_to_color[key][2])),
2)
textLabel = '{}: {}'.format(key, int(100 * new_probs[jk]))
all_dets.append(create_box_resp_field(self.class_mapping_rev[key], new_probs[jk], real_x2/255, real_x1/255, real_y2/255, real_y1/255))
(retval, baseLine) = cv2.getTextSize(textLabel, cv2.FONT_HERSHEY_COMPLEX, 1, 1)
textOrg = (real_x1, real_y1 - 0)
cv2.rectangle(image, (textOrg[0] - 5, textOrg[1] + baseLine - 5),
(textOrg[0] + retval[0] + 5, textOrg[1] - retval[1] - 5), (0, 0, 0), 2)
cv2.rectangle(image, (textOrg[0] - 5, textOrg[1] + baseLine - 5),
(textOrg[0] + retval[0] + 5, textOrg[1] - retval[1] - 5), (255, 255, 255), -1)
cv2.putText(image, textLabel, textOrg, cv2.FONT_HERSHEY_DUPLEX, 1, (0, 0, 0), 1)
boxes_str = 'Count: ' + str(len(all_dets)) + ', ' + str(all_dets)
print('Results: ' + boxes_str)
DataCollector().write(DATA_TYPE_DETECTION, boxes_str)
return image, all_dets
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.op_params)
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()
dat.init('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.15, 0.15, 0.05]
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))
def _remove_old_entries(self, 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, profile_mod_x, profile_mod_pos,
profile_mod_neg, x_vel, y_dist):
"""
This function modifies the y_dist list used by dynamic follow in accordance with global_df_mod
It also intelligently adjusts the profile mods at each breakpoint based on the change in TR
"""
if self.global_df_mod == 1.:
return profile_mod_pos, profile_mod_neg, y_dist
global_df_mod = 1 - self.global_df_mod
# Calculate new TRs
speeds = [
0, self.sng_speed, 18, x_vel[-1]
] # [0, 18 mph, ~40 mph, highest profile mod speed (~78 mph)]
mods = [
0, 0.1, 0.7, 1
] # how much to limit global_df_mod at each speed, 1 is full effect
y_dist_new = [
y - (y * global_df_mod * interp(x, speeds, mods))
for x, y in zip(x_vel, y_dist)
]
# Calculate how to change profile mods based on change in TR
# eg. if df mod is 0.7, then increase positive mod and decrease negative mod
calc_profile_mods = [(interp(mod_x, x_vel, y_dist) -
interp(mod_x, x_vel, y_dist_new) + 1)
for mod_x in profile_mod_x]
profile_mod_pos = [
mod_pos * mod
for mod_pos, mod in zip(profile_mod_pos, calc_profile_mods)
]
profile_mod_neg = [
mod_neg * ((1 - mod) + 1)
for mod_neg, mod in zip(profile_mod_neg, calc_profile_mods)
]
return profile_mod_pos, profile_mod_neg, y_dist_new
def _get_TR(self):
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 = [
5 * CV.MPH_TO_MS, 30 * CV.MPH_TO_MS, 55 * CV.MPH_TO_MS,
80 * CV.MPH_TO_MS
] # profile mod speeds
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
if df_profile == self.df_profiles.roadtrip:
y_dist = [
1.5486, 1.556, 1.5655, 1.5773, 1.5964, 1.6246, 1.6715, 1.7057,
1.7859, 1.8542, 1.8697, 1.8833, 1.8961
] # TRs
profile_mod_pos = [0.5, 0.35, 0.1, 0.03]
profile_mod_neg = [1.3, 1.4, 1.8, 2.0]
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
]
profile_mod_pos = [1.075, 1.55, 2.6, 3.75]
profile_mod_neg = [0.95, .275, 0.1, 0.05]
elif df_profile == self.df_profiles.relaxed: # default to relaxed/stock
y_dist = [
1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.521, 1.544, 1.568,
1.588, 1.599, 1.613, 1.634
]
profile_mod_pos = [1.0, 0.955, 0.898, 0.905]
profile_mod_neg = [1.0, 1.0825, 1.1877, 1.174]
else:
raise Exception('Unknown profile type: {}'.format(df_profile))
# Global df mod
profile_mod_pos, profile_mod_neg, y_dist = self.global_profile_mod(
profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist)
# Profile modifications - Designed so that each profile reacts similarly to changing lead dynamics
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)
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)
TR_mods = []
# Dynamic follow modifications (the secret sauce)
x = [
-26.8224, -20.0288, -15.6871, -11.1965, -7.8645, -4.9472, -3.0541,
-2.2244, -1.5045, -0.7908, -0.3196, 0.0, 0.5588, 1.3682, 1.898,
2.7316, 4.4704
] # relative velocity values
y = [
.76, 0.62323, 0.49488, 0.40656, 0.32227, 0.23914 * 1.025,
0.12269 * 1.05, 0.10483 * 1.075, 0.08074 * 1.15, 0.04886 * 1.25,
0.0072 * 1.075, 0.0, -0.05648, -0.0792, -0.15675, -0.23289, -0.315
] # modification values
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 = [
0.24, 0.16, 0.092, 0.0515, 0.0305, 0.022, 0.0, -0.0153, -0.042,
-0.053, -0.059
] # modification values
TR_mods.append(interp(self.lead_data.a_lead, x, y))
# 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]
profile_mod_pos *= interp(self.car_data.v_ego, x,
y) # but only for currently positive mods
TR_mod = sum([
mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos
for mod in TR_mods
]) # alter TR modification according to profile
TR += TR_mod
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, 1.2)
self.min_TR = self.op_params.get('min_TR')
if self.min_TR != 1.:
self.min_TR = clip(self.min_TR, 0.85, 1.6)
# hidden-layer. To train the network:
# - Use the training dataset of MNIST digits
# - Corrupt the input data using a binomial distribution at 10% corruption level.
# - Use cross-entropy as the cost function
#
# Plot
#
# - a. learning curves for training each layer
# - b. Plot 100 samples of weights (as images) learned at each layer
# - c. For 100 representative test images plot:
# - reconstructed images by the network.
# - Hidden layer activation
# a. Learning curve for training each layer.
data_collector = DataCollector("../mnist.pkl")
train_x, train_y = data_collector.get_train_data()
test_x, test_y = data_collector.get_test_data()
validate_x, validate_y = data_collector.get_validation_data()
num_feature = len(train_x[0])
list_hidden_layer = [900, 625, 400]
epochs = 3
batch_size = 128
num_output = 10
softmax = SoftmaxAutoEncoder(num_features=num_feature,
num_outputs=num_output,
list_hidden_layer=list_hidden_layer,
learning_rate=0.1)
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 #Orig
#self.sng_TR = 2.3 # Last value
#self.sng_TR = 1.5 # Try to decrease stop and start TR so would start move faster from stop
self.sng_TR = 1. # Try to decrease stop and start TR so would start move faster from stop
self.sng_TR = 2.5 # Maybe I thought this wrong
self.sng_speed = 18.0 * CV.MPH_TO_MS #Orig
#self.sng_speed = 12.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] #Original values
#costs = [1., 0.5, 0.1] # Last values
costs = [1., 0.75, 0.5]
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] # Original values
#y_dist = [1.6, 1.6, 1.6, 1.6, 1.6, 1.6, 1.6, 1.6, 1.6, 1.612, 1.621, 1.632, 1.648]
#y_dist = [1.411, 1.418, 1.8, 1.9, 2.1, 2.0, 1.8, 1.6, 1.6, 1.612, 1.621, 1.632, 1.648]
#y_dist = [1.8, 1.8, 1.8, 1.9, 2.1, 2.0, 1.8, 1.6, 1.6, 1.55, 1.5, 1.45, 1.4] # Last used
#y_dist = [3., 2.6, 2.2, 2.1, 2.1, 2.0, 1.8, 1.6, 1.6, 1.55, 1.5, 1.45, 1.4]
y_dist = [
3., 2.6, 2.2, 2.1, 2.1, 2.0, 1.8, 1.7, 1.7, 1.65, 1.55, 1.45,
1.4
]
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)
#TR = TR
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
#x = [self.sng_speed * 0.55, self.sng_speed]
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)