revert debugging code

revert debugging code

Author: sshane

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -1,9 +1,5 @@
 import numpy as np
 from selfdrive.test.longitudinal_maneuvers.plant import Plant
-import matplotlib
-matplotlib.rcParams['figure.raise_window'] = False
-import matplotlib.pyplot as plt
-import time
 
 
 class Maneuver():
@@ -35,9 +31,7 @@ def evaluate(self):
 
     valid = True
     logs = []
-    i = 0
     while plant.current_time() < self.duration:
-      i += 1
       speed_lead = np.interp(plant.current_time(), self.breakpoints, self.speed_lead_values)
       prob = np.interp(plant.current_time(), self.breakpoints, self.prob_lead_values)
       cruise = np.interp(plant.current_time(), self.breakpoints, self.cruise_values)
@@ -53,20 +47,10 @@ def evaluate(self):
                             log['speed'],
                             speed_lead,
                             log['acceleration']]))
-      if i % 20 == 0:
-        plt.clf()
-        plt.title('rel dist: {} m'.format(round(logs[-1][2] - logs[-1][1], 3)))
-        plt.plot([i[1] for i in logs], label='ego')
-        plt.plot([i[2] for i in logs], label='lead')
-        plt.legend()
-        plt.pause(0.01)
-        # plt.show()
 
       if d_rel < .4 and (self.only_radar or prob > 0.5):
         print("Crashed!!!!")
         valid = False
 
     print("maneuver end", valid)
-    # time.sleep(3)
-    input()
     return valid, np.array(logs)

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -119,9 +119,9 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
       v_rel = 0.
 
     # print at 5hz
-    # if (self.rk.frame % (self.rate // 5)) == 0:
-    #   print("%2.2f sec   %6.2f m  %6.2f m/s  %6.2f m/s2   lead_rel: %6.2f m  %6.2f m/s"
-    #         % (self.current_time(), self.distance, self.speed, self.acceleration, d_rel, v_rel))
+    if (self.rk.frame % (self.rate // 5)) == 0:
+      print("%2.2f sec   %6.2f m  %6.2f m/s  %6.2f m/s2   lead_rel: %6.2f m  %6.2f m/s"
+            % (self.current_time(), self.distance, self.speed, self.acceleration, d_rel, v_rel))
 
 
     # ******** update prevs ********

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -8,24 +8,24 @@
 
 # TODO: make new FCW tests
 maneuvers = [
-  # Maneuver(
-  #   'approach stopped car at 20m/s',
-  #   duration=20.,
-  #   initial_speed=25.,
-  #   lead_relevancy=True,
-  #   initial_distance_lead=120.,
-  #   speed_lead_values=[30., 0.],
-  #   breakpoints=[0., 1.],
-  # ),
-  # Maneuver(
-  #   'approach stopped car at 20m/s',
-  #   duration=20.,
-  #   initial_speed=20.,
-  #   lead_relevancy=True,
-  #   initial_distance_lead=90.,
-  #   speed_lead_values=[20., 0.],
-  #   breakpoints=[0., 1.],
-  # ),
+  Maneuver(
+    'approach stopped car at 20m/s',
+    duration=20.,
+    initial_speed=25.,
+    lead_relevancy=True,
+    initial_distance_lead=120.,
+    speed_lead_values=[30., 0.],
+    breakpoints=[0., 1.],
+  ),
+  Maneuver(
+    'approach stopped car at 20m/s',
+    duration=20.,
+    initial_speed=20.,
+    lead_relevancy=True,
+    initial_distance_lead=90.,
+    speed_lead_values=[20., 0.],
+    breakpoints=[0., 1.],
+  ),
   Maneuver(
     'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
     duration=50.,
@@ -64,48 +64,48 @@
     cruise_values=[20., 20., 20.],
     breakpoints=[2., 2.01, 8.51],
   ),
-  # Maneuver(
-  #   "approach stopped car at 20m/s",
-  #   duration=30.,
-  #   initial_speed=20.,
-  #   lead_relevancy=True,
-  #   initial_distance_lead=120.,
-  #   speed_lead_values=[0.0, 0., 0.],
-  #   prob_lead_values=[0.0, 0., 1.],
-  #   cruise_values=[20., 20., 20.],
-  #   breakpoints=[0.0, 2., 2.01],
-  # ),
-  # Maneuver(
-  #   "approach slower cut-in car at 20m/s",
-  #   duration=20.,
-  #   initial_speed=20.,
-  #   lead_relevancy=True,
-  #   initial_distance_lead=50.,
-  #   speed_lead_values=[15., 15.],
-  #   breakpoints=[1., 11.],
-  #   only_lead2=True,
-  # ),
-  # Maneuver(
-  #   "stay stopped behind radar override lead",
-  #   duration=20.,
-  #   initial_speed=0.,
-  #   lead_relevancy=True,
-  #   initial_distance_lead=10.,
-  #   speed_lead_values=[0., 0.],
-  #   prob_lead_values=[0., 0.],
-  #   breakpoints=[1., 11.],
-  #   only_radar=True,
-  # ),
-  # Maneuver(
-  #   "NaN recovery",
-  #   duration=30.,
-  #   initial_speed=15.,
-  #   lead_relevancy=True,
-  #   initial_distance_lead=60.,
-  #   speed_lead_values=[0., 0., 0.0],
-  #   breakpoints=[1., 1.01, 11.],
-  #   cruise_values=[float("nan"), 15., 15.],
-  # ),
+  Maneuver(
+    "approach stopped car at 20m/s",
+    duration=30.,
+    initial_speed=20.,
+    lead_relevancy=True,
+    initial_distance_lead=120.,
+    speed_lead_values=[0.0, 0., 0.],
+    prob_lead_values=[0.0, 0., 1.],
+    cruise_values=[20., 20., 20.],
+    breakpoints=[0.0, 2., 2.01],
+  ),
+  Maneuver(
+    "approach slower cut-in car at 20m/s",
+    duration=20.,
+    initial_speed=20.,
+    lead_relevancy=True,
+    initial_distance_lead=50.,
+    speed_lead_values=[15., 15.],
+    breakpoints=[1., 11.],
+    only_lead2=True,
+  ),
+  Maneuver(
+    "stay stopped behind radar override lead",
+    duration=20.,
+    initial_speed=0.,
+    lead_relevancy=True,
+    initial_distance_lead=10.,
+    speed_lead_values=[0., 0.],
+    prob_lead_values=[0., 0.],
+    breakpoints=[1., 11.],
+    only_radar=True,
+  ),
+  Maneuver(
+    "NaN recovery",
+    duration=30.,
+    initial_speed=15.,
+    lead_relevancy=True,
+    initial_distance_lead=60.,
+    speed_lead_values=[0., 0., 0.0],
+    breakpoints=[1., 1.01, 11.],
+    cruise_values=[float("nan"), 15., 15.],
+  ),
 ]