Make LaneSpeed quicker and more efficient (commaai#124)

* add debugging statements

* fix

* run all

* run it manually

* lower priority and affinity

* let's see what cpu usage this gets us

* change to 1

* no realtime

* ls_state

* print len

* so more tracks get grouped

* debug

* debug

* debug

* debug

* debug

* debug

* debug

* debug

* see if this is faster

* better

* so this is slower?

* woah, np.mean can be as low as 400 Hz while sum()/len() is anywhere from 2000 to 5000 hz! wth numpy

* debugging

* better

* better

* don't be random everytime

* remove

* debug

* debug

* debug

* is this any better?

* that was bad

* see which is quicker

* okay, so this is the quickest

* debug which poly val function is faster

* debug which poly val function is faster

* debug which poly val function is faster

* debug which poly val function is faster

* debug which poly val function is faster

* test out faster function

* remove

* print average

* cleaner

* test original function

* test 1st new function

* test 2nd new function

* only measure polyval

* run for 60 sec then quit

* don't run updated

* 2nd function test

* 3rd function test

* now only time grouping loop

* now only time grouping loop

* why not 60

* calc only once instead of up to 2*3 times

* move here

* slightly faster here

* get a baseline of the whole function now

* don't use a np array for lane bounds, no need. might be faster

* add hz

* woah, removing numpy from bounds and offsetting manually make the rate go from 766.2768 to 1299.8735 hz

* use if checks instead of a loop test

* comment old loop

* reverse the if check so that we do speed check first

* move static track object logic to Lane class to remove lines of code in group_tracks. let's see if it's faster

* test loop now

* okay, loop is still slower at 1200 Hz vs 1500 hz with this method (if checks and logic in Lane class for static tracks)

* previous commit was 1562 Hz

* previous commit was 1599 Hz, this commit *should* be faster

* yup, this commit was 1635 Hz. it's not pretty, but it's faster

* debug

* one should be oncoming

* one more

* one more

* one more

* one more

* one more

* one more

* this is good

* get a baseline

* offset y_rel instead of bounds

* offset y_rel instead of bounds

* fix

* calculate on demand instead of all at once test

* old faster method again

* move eval_poly to the same list comp, last commit was 1923 hz

* dang, last commit was 2072 Hz!

* now see if moving it into the main loop will be any faster

* nah, that was 2032.8366 Hz vs 2072

* okay now it's slower again?? what is this then? last was 2048 down from 2079

* let's just use this, it's faster marginally for some reason

* so this should be even faster, right? previous was around 2033 hz

* nope, much slower at 1819 hz. this is good

* get baseline without as many print statements

* last commit was 1958 hz, just want to test this code one more time

* last commit was 1931 hz, so; slower. test loop one last time with new offset_y_rel code

* loop was still slower at 1849 hz, so this looks to be the fastest!

* clean up and print lanes to verify the logic hasn't changed

* remove

* clean up

* clean up

* clean up and use numpy_fast for interpolating

* use numpy fast in DynamicCameraOffset and DynamicFollow

* add comment

* higher corolla gas up to 40 mph

* comment

* revert manager changes

* use LQR for corolla

selfdrive/car/toyota/interface.py

@@ -40,7 +40,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
       ret.longitudinalTuning.kiV = [0.54, 0.36]
 
-    if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]:  # These cars use LQR/INDI
+    if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H, CAR.COROLLA]:  # These cars use LQR/INDI
       ret.lateralTuning.init('pid')
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
 
@@ -88,8 +88,17 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.steerRatio = 18.27
       tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
-      ret.lateralTuning.pid.kf = 0.00003   # full torque for 20 deg at 80mph means 0.00007818594
+      ret.lateralTuning.init('lqr')
+
+      ret.lateralTuning.lqr.scale = 1500.0
+      ret.lateralTuning.lqr.ki = 0.05
+
+      ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
+      ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
+      ret.lateralTuning.lqr.c = [1., 0.]
+      ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
+      ret.lateralTuning.lqr.l = [0.3233671, 0.3185757]
+      ret.lateralTuning.lqr.dcGain = 0.002237852961363602
 
     elif candidate == CAR.LEXUS_RX:
       stop_and_go = True

selfdrive/controls/lib/dynamic_follow/__init__.py

@@ -119,7 +119,7 @@ def _gather_data(self):
 
   def _norm(self, x, name):
     self.x = x
-    return np.interp(x, self.model_scales[name], [0, 1])
+    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:
@@ -141,7 +141,7 @@ def _change_cost(self, libmpc):
       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 *= np.interp(cost_mod, cost_mod_speeds, cost_mods)
+      cost *= interp(cost_mod, cost_mod_speeds, cost_mods)
 
     if self.last_cost != cost:
       libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
@@ -198,11 +198,11 @@ def _relative_accel_mod(self):
     mods_x = [0, -.75, -1.5]
     mods_y = [1.5, 1.25, 1]
     if a_lead < 0:  # more weight to slight lead decel
-      a_lead *= np.interp(a_lead, mods_x, mods_y)
+      a_lead *= interp(a_lead, mods_x, mods_y)
 
     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.25, 0.277 * 1.2, 0.11075 * 1.15, 0.08106 * 1.075, 0.06325 * 1.05, 0.0, -0.09, -0.09375, -0.125, -0.3, -0.35]
-    return np.interp(a_lead - a_ego, rel_x, mod_y)
+    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):
     """
@@ -216,11 +216,11 @@ def global_profile_mod(self, profile_mod_x, profile_mod_pos, profile_mod_neg, x_
     # 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 * np.interp(x, speeds, mods)) for x, y in zip(x_vel, y_dist)]
+    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 = [(np.interp(mod_x, x_vel, y_dist) - np.interp(mod_x, x_vel, y_dist_new) + 1) for mod_x in profile_mod_x]
+    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)]
 

selfdrive/controls/lib/dynamic_gas.py

@@ -74,6 +74,7 @@ def set_profile(self):
       if self.candidate == CAR_TOYOTA.COROLLA:
         x = [0.0, 1.4082, 2.8031, 4.2266, 5.3827, 6.1656, 7.2478, 8.2831, 10.2447, 12.964, 15.423, 18.119, 20.117, 24.4661, 29.0581, 32.7101, 35.7633]
         y = [0.218, 0.222, 0.233, 0.25, 0.273, 0.294, 0.337, 0.362, 0.38, 0.389, 0.398, 0.41, 0.421, 0.459, 0.512, 0.564, 0.621]
+        y = [interp(i, [0.218, (0.218 + 0.398) / 2, 0.398], [1.075 * i, i * 1.05, i]) for i in y]  # more gas at lower speeds up until ~40 mph
         self.supported_car = True
       elif self.candidate == CAR_TOYOTA.PRIUS:
         x = [0.0, 1.4082, 2.8031, 4.2266, 5.3827, 6.1656, 7.2478, 8.2831, 10.2447, 12.964, 15.423, 18.119, 20.117, 24.4661, 29.0581, 32.7101, 35.7633]
@@ -89,7 +90,7 @@ def set_profile(self):
         self.supported_car = True
     else:
       y = [0.35, 0.47, 0.43, 0.35, 0.3, 0.3, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7]
-      y = [np.interp(i, [.35, 0.7], [1.15, 1.0]) * i for i in y]
+      y = [interp(i, [y[0], y[-1]], [1.15, 1.0]) * i for i in y]  # more gas at lower speeds
       self.supported_car = True
 
     self.gasMaxBP, self.gasMaxV = x, y

selfdrive/controls/lib/lane_planner.py

@@ -118,7 +118,7 @@ def _get_camera_offset(self, v_ego, active, angle_steers):
     right_lane_oncoming = self.right_lane_oncoming
 
     if self.have_oncoming:
-      _min_poly_prob = np.interp(v_ego, self._poly_prob_speeds, self._poly_probs)
+      _min_poly_prob = interp(v_ego, self._poly_prob_speeds, self._poly_probs)
       if self.l_prob < _min_poly_prob and self.r_prob < _min_poly_prob:  # we only need one line and an accurate current lane width
         return
       if self.lane_width_certainty < self._min_lane_width_certainty:
@@ -134,7 +134,7 @@ def _get_camera_offset(self, v_ego, active, angle_steers):
 
     estimated_lane_position = self._get_camera_position()
 
-    hug_modifier = np.interp(abs(angle_steers), self._ramp_angles, self._ramp_angle_mods)  # don't offset as much when angle is high
+    hug_modifier = interp(abs(angle_steers), self._ramp_angles, self._ramp_angle_mods)  # don't offset as much when angle is high
     if left_lane_oncoming:
       self.keeping_right = True
       hug_ratio = (self._hug_right_ratio * hug_modifier) + (self._center_ratio * (1 - hug_modifier))  # weighted average
@@ -149,7 +149,7 @@ def _get_camera_offset(self, v_ego, active, angle_steers):
     offset = self.i + error * self._k_p
 
     if time_since_oncoming <= self._keep_offset_for and not self.have_oncoming:  # not yet 3 seconds after last oncoming, ramp down from 1.5 second
-      offset *= np.interp(time_since_oncoming, self._ramp_down_times, self._ramp_down_multipliers)  # ramp down offset
+      offset *= interp(time_since_oncoming, self._ramp_down_times, self._ramp_down_multipliers)  # ramp down offset
 
     return offset
 

selfdrive/controls/lib/lane_speed.py

@@ -1,22 +1,23 @@
 from common.op_params import opParams
-
+from common.realtime import set_core_affinity
 from selfdrive.config import Conversions as CV
-# from common.numpy_fast import clip, interp
+from selfdrive.controls.lib.lane_planner import eval_poly
+from common.numpy_fast import interp
 import numpy as np
 import time
 try:
   from common.realtime import sec_since_boot
   import cereal.messaging as messaging
 except:
   pass
+
 # try:
 #   from common.realtime import sec_since_boot
 # except ImportError:
 #   import matplotlib.pyplot as plt
 #   import time
 #   sec_since_boot = time.time
 
-
 def cluster(data, maxgap):
   data.sort(key=lambda _trk: _trk.dRel)
   groups = [[data[0]]]
@@ -55,6 +56,7 @@ def set_fastest(self):
 
 class LaneSpeed:
   def __init__(self):
+    set_core_affinity(1)  # use up to 1 core?
     self.op_params = opParams()
 
     self._track_speed_margin = 0.05  # track has to be above X% of v_ego (excludes oncoming and stopped)
@@ -63,6 +65,7 @@ def __init__(self):
     self._min_fastest_time = 3 / LANE_SPEED_RATE  # how long should we wait for a specific lane to be faster than middle before alerting
     self._max_steer_angle = 100  # max supported steering angle
     self._extra_wait_time = 5  # in seconds, how long to wait after last alert finished before allowed to show next alert
+    self._min_track_speed = 5 * CV.MPH_TO_MS  # tracks must be traveling faster than this speed to be added to a lane (- or +)
 
     self.fastest_lane = 'none'  # always will be either left, right, or none as a string, never middle or NoneType
     self.last_fastest_lane = 'none'
@@ -123,18 +126,19 @@ def update(self):
     else:
       self.reset(reset_fastest=True)
 
-  def update_lane_bounds(self):
+  def update_lane_bounds(self):  # todo: run this at half the rate of lane_speed
+    # todo 2: add dPoly offsetting to lane bounds here as well, from group_tracks
     lane_width = self.sm['pathPlan'].laneWidth
     if isinstance(lane_width, float) and lane_width > 1:
-      self.lane_width = min(lane_width, 4)  # LanePlanner uses 4 as max width for dPoly calculation
+      self.lane_width = min(lane_width, 4.5)  # LanePlanner uses 4 as max width for dPoly calculation
 
     self.lanes['left'].pos = self.lane_width  # update with new lane center positions
     self.lanes['right'].pos = -self.lane_width
 
     # and now update bounds
-    self.lanes['left'].bounds = np.array([self.lanes['left'].pos * 1.5, self.lanes['left'].pos / 2])
-    self.lanes['middle'].bounds = np.array([self.lanes['left'].pos / 2, self.lanes['right'].pos / 2])
-    self.lanes['right'].bounds = np.array([self.lanes['right'].pos / 2, self.lanes['right'].pos * 1.5])
+    self.lanes['left'].bounds = [self.lanes['left'].pos * 1.5, self.lanes['left'].pos / 2]
+    self.lanes['middle'].bounds = [self.lanes['left'].pos / 2, self.lanes['right'].pos / 2]
+    self.lanes['right'].bounds = [self.lanes['right'].pos / 2, self.lanes['right'].pos * 1.5]
 
   # def filter_tracks(self):  # todo: make cluster() return indexes of live_tracks instead
   #   print(type(self.live_tracks))
@@ -147,16 +151,29 @@ def update_lane_bounds(self):
 
   def group_tracks(self):
     """Groups tracks based on lateral position, dPoly offset, and lane width"""
-    y_offsets = np.polyval(self.d_poly, [trk.dRel for trk in self.live_tracks])  # it's faster to calculate all at once
-    for track, y_offset in zip(self.live_tracks, y_offsets):
-      for lane_name in self.lanes:
-        lane_bounds = self.lanes[lane_name].bounds + y_offset  # offset lane bounds based on our future lateral position (dPoly) and track's distance
-        if lane_bounds[0] >= track.yRel >= lane_bounds[1]:  # track is in a lane
-          if track.vRel + self.v_ego >= 2.24:
-            self.lanes[lane_name].tracks.append(track)
-          elif track.vRel + self.v_ego <= -2.24:  # make sure we don't add stopped tracks at high speeds
-            self.lanes[lane_name].oncoming_tracks.append(track)
-          break  # skip to next track
+    offset_y_rels = [trk.yRel - eval_poly(self.d_poly, trk.dRel) for trk in self.live_tracks]  # eval_poly: 4109.0476 Hz vs np.polyval's 2483.2956 Hz
+    for track, offset_y_rel in zip(self.live_tracks, offset_y_rels):
+      # it's not pretty, but this code is the fastest. even when looping through tracks and then lanes for each track
+      # (and breaking when a lane has been found for the track)
+      # this is also faster than having the speed if check first
+      track_vel = track.vRel + self.v_ego
+      if self.lanes['left'].bounds[0] >= offset_y_rel >= self.lanes['left'].bounds[1]:
+        if track_vel >= self._min_track_speed:  # ongoing track
+          self.lanes['left'].tracks.append(track)
+        elif track_vel <= -self._min_track_speed:  # oncoming track
+          self.lanes['left'].oncoming_tracks.append(track)
+
+      elif self.lanes['middle'].bounds[0] >= offset_y_rel >= self.lanes['middle'].bounds[1]:
+        if track_vel >= self._min_track_speed:
+          self.lanes['middle'].tracks.append(track)
+        elif track_vel <= -self._min_track_speed:
+          self.lanes['middle'].oncoming_tracks.append(track)
+
+      elif self.lanes['right'].bounds[0] >= offset_y_rel >= self.lanes['right'].bounds[1]:
+        if track_vel >= self._min_track_speed:
+          self.lanes['right'].tracks.append(track)
+        elif track_vel <= -self._min_track_speed:
+          self.lanes['right'].oncoming_tracks.append(track)
 
   def find_oncoming_lanes(self):
     """If number of oncoming tracks is greater than tracks going our direction, set lane to oncoming"""
@@ -180,7 +197,8 @@ def get_fastest_lane(self):
       track_speeds = [track.vRel + self.v_ego for track in lane.tracks]
       track_speeds = [speed for speed in track_speeds if self.v_ego * self._track_speed_margin < speed <= v_cruise_setpoint]
       if len(track_speeds):  # filters out very slow tracks
-        lane.avg_speed = np.mean(track_speeds)  # todo: something with std?
+        # np.mean was much slower than sum() / len()
+        lane.avg_speed = sum(track_speeds) / len(track_speeds)  # todo: something with std?
 
     lanes_with_avg_speeds = self.lanes_with_avg_speeds()
     if 'middle' not in lanes_with_avg_speeds or len(lanes_with_avg_speeds) < 2:
@@ -202,7 +220,7 @@ def get_fastest_lane(self):
 
     _f_time_x = [1, 4, 12]  # change the minimum time for fastest based on how many tracks are in fastest lane
     _f_time_y = [1.5, 1, 0.5]  # this is multiplied by base fastest time todo: probably need to tune this
-    min_fastest_time = np.interp(len(fastest_lane.tracks), _f_time_x, _f_time_y)  # get multiplier
+    min_fastest_time = interp(len(fastest_lane.tracks), _f_time_x, _f_time_y)  # get multiplier
     min_fastest_time = int(min_fastest_time * self._min_fastest_time)  # now get final min_fastest_time
 
     if fastest_lane.fastest_count < min_fastest_time:
@@ -287,8 +305,11 @@ def reset(self, reset_tracks=False, reset_fastest=False, reset_avg_speed=False):
 #     self.vRel = vRel
 #     self.yRel = yRel
 #     self.dRel = dRel
-#
-# TEMP_LIVE_TRACKS = [Track(10, 4, 20), Track(10, 4, 20), Track(20, 0, 20), Track(25, 0, 20)]
+# v_rels = [7.027988825101453, -35, -2.0073281329557595, -38, -42, -0.4124279188166433, -4.864017389464086, -31.5, -9.684282305020197, -9.979187599100587, -8.036672540886896, -3.025854705185946, -6.347005348508485, -2.502134724290814, 3.8857648270182743, 5.3016772854121115]
+# y_rels = [-3.7392238915910396, -4.947102125963248, -3.099776764519531, -5.399104990417248, 5.278053706824695, 3.8991116187949793, -0.9252016611001208, 0.4527911313949229, 4.606432638329704, -1.9683618473307751, -3.6920577990810357, -0.9243886066458202, 4.765879225624099, 5.310588490331199, -2.073362080174996, -0.787692913730746]
+# d_rels = [47.816299530243484, 1.0937590342875225, 45.83286354330341, 44.79009263149329, 15.721120725763347, 48.974408204844835, 10.538985749858739, 50.379159253222355, 27.746917826360942, 24.410420872880284, 1.605961587171345, 23.89657990345233, 30.219941981980615, 50.31621564718719, 35.654178681545176, 34.980565736019585]
+# TEMP_LIVE_TRACKS = [Track(v, y, d) for v, y, d in zip(v_rels, y_rels, d_rels)]
+# TEMP_D_POLY = np.array([1.3839008e-06/10, 0, 0, 0.05])
 
 def main():
   lane_speed = LaneSpeed()