Merge pull request #177 from ShaneSmiskol/dynamic-follow

Speed optimizations for dynamic follow. Changed to logging lead veloc…

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