From 2bd8a3d94363e8b2b9b76edd5a7ec789f228d5dd Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 4 Apr 2019 23:15:09 -0500 Subject: [PATCH] Speed optimizations for dynamic follow. Changed to logging lead velocity for traffic every half second so only 240 items are required instead of 12000 --- selfdrive/controls/lib/planner.py | 46 ++++++++++++++++++------------- 1 file changed, 27 insertions(+), 19 deletions(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index f4d21c0fb8f8f3..8457d39b4fae8a 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -160,7 +160,8 @@ def __init__(self, mpc_id, live_longitudinal_mpc): self.lastTR = 0 self.last_cloudlog_t = 0.0 self.last_cost = 0 - self.dynamic_follow_dict = {"self_vel": [], "lead_vel": []} + self.dynamic_follow_dict = {"self_vels": [], "lead_vels": [], "traffic_vels": []} + self.mpc_frame = 0 # idea thanks to kegman try: with open("/data/openpilot/gas-interceptor", "r") as f: @@ -199,25 +200,24 @@ def set_cur_state(self, v, a): self.cur_state[0].v_ego = v self.cur_state[0].a_ego = a - def get_traffic_level(self, lead_vels): # generate a value to modify TR by based on fluctuations in traffic speed - if len(lead_vels) < 1000: - return 1.0 # do nothing to TR + def get_traffic_level(self, lead_vels): # generate a value to modify TR by based on fluctuations in lead speed + if len(lead_vels) < 60: + return 1.0 # if less than 30 seconds of traffic data do nothing to TR lead_vel_diffs = [] for idx, vel in enumerate(lead_vels): - if idx != 0: + try: lead_vel_diffs.append(abs(vel - lead_vels[idx - 1])) + except: + pass x = [0, len(lead_vels)] - y = [1.1, .9] # min and max values to modify TR by, need to tune + y = [1.15, .9] # min and max values to modify TR by, need to tune traffic = np.interp(sum(lead_vel_diffs), x, y) return traffic def get_acceleration(self, velocity_list): # calculate car's own acceleration to generate more accurate following distances - try: # try to get value 150 from end - a = (velocity_list[-150] - velocity_list[0]) # first half of acceleration formula - except: - a = (velocity_list[-1] - velocity_list[0]) + a = (velocity_list[-1] - velocity_list[0]) # first half of acceleration formula a = a / (len(velocity_list) / 100.0) # divide difference in velocity by how long in seconds the velocity list has been tracking velocity (2 sec) if abs(a) < 0.11176: #if abs(acceleration) is less than 0.25 mph/s, return 0 return 0.0 @@ -241,14 +241,14 @@ def generateTR(self, velocity): # in m/s x = [-4.4704, -2.2352, -0.89408, 0, 1.34112] # self acceleration values, mph: [-10, -5, -2, 0, 3] y = [(TR + .158), (TR + .062), (TR + .009), TR, (TR - .13)] # modification values - TR = np.interp(self.get_acceleration(self.dynamic_follow_dict["self_vel"]), x, y) # factor in self acceleration + TR = np.interp(self.get_acceleration(self.dynamic_follow_dict["self_vels"]), x, y) # factor in self acceleration x = [-4.4704, -1.77, -.31446, 0, .446, 1.34112] # lead acceleration values, mph: [-10, -5, -2, 0, 3] y = [(TR + .237), (TR + .12), (TR + .027), TR, (TR - .105), (TR - .195)] # modification values - TR = np.interp(self.get_acceleration(self.dynamic_follow_dict["lead_vel"]), x, y) # factor in lead car's acceleration; should perform better + TR = np.interp(self.get_acceleration(self.dynamic_follow_dict["lead_vels"]), x, y) # factor in lead car's acceleration; should perform better - TR = TR * self.get_traffic_level(self.dynamic_follow_dict["lead_vel"]) # modify TR based on last minute of traffic data + TR = TR * self.get_traffic_level(self.dynamic_follow_dict["traffic_vels"]) # modify TR based on last minute of traffic data if TR < 0.5: return 0.5 @@ -264,13 +264,21 @@ def generate_cost(self, distance): return round(float(np.interp(distance, x, y)), 2) # used to cause stuttering, but now we're doing a percentage change check before changing def save_car_data(self, self_vel, lead_vel): - if len(self.dynamic_follow_dict["self_vel"]) > 150: # 100hz, so 150 items is 1.5 seconds - self.dynamic_follow_dict["self_vel"].pop(0) - self.dynamic_follow_dict["self_vel"].append(self_vel) + if len(self.dynamic_follow_dict["self_vels"]) > 150: # 100hz, so 150 items is 1.5 seconds + del self.dynamic_follow_dict["self_vels"][0] + self.dynamic_follow_dict["self_vels"].append(self_vel) + + if len(self.dynamic_follow_dict["lead_vels"]) > 150: # 100hz, so 12000 items is 1.5 seconds + del self.dynamic_follow_dict["lead_vels"][0] + self.dynamic_follow_dict["lead_vels"].append(lead_vel) + + if self.mpc_frame >= 50: # add to traffic list every half second so we're not working with a huge list + if len(self.dynamic_follow_dict["traffic_vels"]) > 240: # 240 half seconds is 2 minutes of traffic logging + del self.dynamic_follow_dict["traffic_vels"][0] + self.dynamic_follow_dict["traffic_vels"].append(lead_vel) + self.mpc_frame = 0 # reset every half second - if len(self.dynamic_follow_dict["lead_vel"]) > 12000: # 100hz, so 12000 items is 2 minutes - self.dynamic_follow_dict["lead_vel"].pop(0) - self.dynamic_follow_dict["lead_vel"].append(lead_vel) + self.mpc_frame += 1 # increment every frame def update(self, CS, lead, v_cruise_setpoint): # Setup current mpc state