various cleanup (#22289)

selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py

@@ -21,10 +21,10 @@ MPC_T = list(np.arange(0,1.,.2)) + list(np.arange(1.,10.6,.6))
 N = len(MPC_T) - 1
 
 
-def RW(v_ego, v_l):
+def desired_follow_distance(v_ego, v_lead):
   TR = 1.8
   G = 9.81
-  return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
+  return (v_ego * TR - (v_lead - v_ego) * TR + v_ego * v_ego / (2 * G) - v_lead * v_lead / (2 * G)) + 4.0
 
 
 def gen_lead_model():
@@ -85,21 +85,16 @@ def gen_lead_mpc_solver():
   ocp.cost.yref_e = np.zeros((3, ))
 
   x_lead, v_lead = ocp.model.p[0], ocp.model.p[1]
-  G = 9.81
+  desired_dist = desired_follow_distance(v_ego, v_lead)
-  TR = 1.8
+  dist_err = (desired_dist - (x_lead - x_ego))/(sqrt(v_ego + 0.5) + 0.1)
-  desired_dist = (v_ego * TR
-                  - (v_lead - v_ego) * TR
-                  + v_ego*v_ego/(2*G)
-                  - v_lead * v_lead / (2*G))
-  dist_err = (desired_dist + 4.0 - (x_lead - x_ego))/(sqrt(v_ego + 0.5) + 0.1)
 
   # TODO hacky weights to keep behavior the same
   ocp.model.cost_y_expr = vertcat(exp(.3 * dist_err) - 1.,
-                                  ((x_lead - x_ego) - (desired_dist + 4.0)) / (0.05 * v_ego + 0.5),
+                                  ((x_lead - x_ego) - (desired_dist)) / (0.05 * v_ego + 0.5),
                                   a_ego * (.1 * v_ego + 1.0),
                                   j_ego * (.1 * v_ego + 1.0))
   ocp.model.cost_y_expr_e = vertcat(exp(.3 * dist_err) - 1.,
-                                  ((x_lead - x_ego) - (desired_dist + 4.0)) / (0.05 * v_ego + 0.5),
+                                  ((x_lead - x_ego) - (desired_dist)) / (0.05 * v_ego + 0.5),
                                   a_ego * (.1 * v_ego + 1.0))
   ocp.parameter_values = np.array([0., .0])
 

selfdrive/controls/tests/test_following_distance.py

@@ -5,7 +5,7 @@ import numpy as np
 from cereal import log
 import cereal.messaging as messaging
 from selfdrive.config import Conversions as CV
-from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import RW, LeadMpc
+from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import desired_follow_distance, LeadMpc
 
 
 class FakePubMaster():
@@ -68,7 +68,7 @@ class TestFollowingDistance(unittest.TestCase):
       v_lead = float(speed_mph * CV.MPH_TO_MS)
 
       simulation_steady_state = run_following_distance_simulation(v_lead)
-      correct_steady_state = RW(v_lead, v_lead) + 4.0
+      correct_steady_state = desired_follow_distance(v_lead, v_lead)
 
       self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.2)
 

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -9,8 +9,9 @@ class Maneuver():
     self.speed = kwargs.get("initial_speed", 0.0)
     self.lead_relevancy = kwargs.get("lead_relevancy", 0)
 
-    self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0])
     self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration])
+    self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0])
+    self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.speed_lead_breakpoints))])
 
     self.only_lead2 = kwargs.get("only_lead2", False)
     self.only_radar = kwargs.get("only_radar", False)
@@ -31,13 +32,19 @@ class Maneuver():
     logs = []
     while plant.current_time() < self.duration:
       speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
-      log = plant.step(speed_lead)
+      prob = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.prob_lead_values)
+      log = plant.step(speed_lead, prob)
 
       d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200.
       v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
       log['d_rel'] = d_rel
       log['v_rel'] = v_rel
-      logs.append(np.array([plant.current_time(), log['distance'], log['distance_lead'], log['speed'], speed_lead, log['acceleration']]))
+      logs.append(np.array([plant.current_time(),
+                            log['distance'],
+                            log['distance_lead'],
+                            log['speed'],
+                            speed_lead,
+                            log['acceleration']]))
 
       if d_rel < 1.0:
         print("Crashed!!!!")

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -48,7 +48,7 @@ class Plant():
   def current_time(self):
     return float(self.rk.frame) / self.rate
 
-  def step(self, v_lead=0.0):
+  def step(self, v_lead=0.0, prob=1.0):
     # ******** publish a fake model going straight and fake calibration ********
     # note that this is worst case for MPC, since model will delay long mpc by one time step
     radar = messaging.new_message('radarState')
@@ -61,10 +61,11 @@ class Plant():
       d_rel = np.maximum(0., self.distance_lead - self.distance)
       v_rel = v_lead - self.speed
       if self.only_radar:
-        prob = 0.0
-      else:
-        prob = 1.0
         status = True
+      elif prob > .5:
+        status = True
+      else:
+        status = False
     else:
       d_rel = 200.
       v_rel = 0.
@@ -81,7 +82,7 @@ class Plant():
     lead.aLeadK = float(a_lead)
     lead.aLeadTau = float(1.5)
     lead.status = status
-    lead.modelProb = prob
+    lead.modelProb = float(prob)
     if not self.only_lead2:
       radar.radarState.leadOne = lead
     radar.radarState.leadTwo = lead

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -88,6 +88,7 @@ maneuvers = [
     lead_relevancy=True,
     initial_distance_lead=10.,
     speed_lead_values=[0., 0.],
+    prob_lead_values=[0., 0.],
     speed_lead_breakpoints=[1., 11.],
     only_radar=True,
   ),