various cleanup (#22289)

old-commit-hash: cc6af379ce0930ec3b635fda9d441654d2ac0c03

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
+        status = True
+      elif prob > .5:
+        status = True
       else:
-        prob = 1.0
-      status = True
+        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,
   ),