FrogPilot 0.9.7

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -12,11 +12,14 @@ class Maneuver:
     self.breakpoints = kwargs.get("breakpoints", [0.0, duration])
     self.speed_lead_values = kwargs.get("speed_lead_values", [0.0 for i in range(len(self.breakpoints))])
     self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.breakpoints))])
+    self.prob_throttle_values = kwargs.get("prob_throttle_values", [1.0 for i in range(len(self.breakpoints))])
     self.cruise_values = kwargs.get("cruise_values", [50.0 for i in range(len(self.breakpoints))])
+    self.pitch_values = kwargs.get("pitch_values", [0.0 for i in range(len(self.breakpoints))])
 
     self.only_lead2 = kwargs.get("only_lead2", False)
     self.only_radar = kwargs.get("only_radar", False)
     self.ensure_start = kwargs.get("ensure_start", False)
+    self.ensure_slowdown = kwargs.get("ensure_slowdown", False)
     self.enabled = kwargs.get("enabled", True)
     self.e2e = kwargs.get("e2e", False)
     self.personality = kwargs.get("personality", 0)
@@ -42,9 +45,11 @@ class Maneuver:
     logs = []
     while plant.current_time < self.duration:
       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)
+      prob_lead = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values)
       cruise = np.interp(plant.current_time, self.breakpoints, self.cruise_values)
-      log = plant.step(speed_lead, prob, cruise)
+      pitch = np.interp(plant.current_time, self.breakpoints, self.pitch_values)
+      prob_throttle = np.interp(plant.current_time, self.breakpoints, self.prob_throttle_values)
+      log = plant.step(speed_lead, prob_lead, cruise, pitch, prob_throttle)
 
       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.
@@ -57,13 +62,18 @@ class Maneuver:
                             speed_lead,
                             log['acceleration']]))
 
-      if d_rel < .4 and (self.only_radar or prob > 0.5):
+      if d_rel < .4 and (self.only_radar or prob_lead > 0.5):
         print("Crashed!!!!")
         valid = False
 
       if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1:
         print('LongitudinalPlanner not starting!')
         valid = False
+
+    if self.ensure_slowdown and log['speed'] > 5.5:
+      print('LongitudinalPlanner not slowing down!')
+      valid = False
+
     if self.force_decel and log['speed'] > 1e-1 and log['acceleration'] > -0.04:
       print('Not stopping with force decel')
       valid = False

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -56,12 +56,13 @@ class Plant:
   def current_time(self):
     return float(self.rk.frame) / self.rate
 
-  def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
+  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle=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')
     control = messaging.new_message('controlsState')
     car_state = messaging.new_message('carState')
+    car_control = messaging.new_message('carControl')
     model = messaging.new_message('modelV2')
     a_lead = (v_lead - self.v_lead_prev)/self.ts
     self.v_lead_prev = v_lead
@@ -71,14 +72,14 @@ class Plant:
       v_rel = v_lead - self.speed
       if self.only_radar:
         status = True
-      elif prob > .5:
+      elif prob_lead > .5:
         status = True
       else:
         status = False
     else:
       d_rel = 200.
       v_rel = 0.
-      prob = 0.0
+      prob_lead = 0.0
       status = False
 
     lead = log.RadarState.LeadData.new_message()
@@ -92,7 +93,7 @@ class Plant:
     # TODO use real radard logic for this
     lead.aLeadTau = float(_LEAD_ACCEL_TAU)
     lead.status = status
-    lead.modelProb = float(prob)
+    lead.modelProb = float(prob_lead)
     if not self.only_lead2:
       radar.radarState.leadOne = lead
     radar.radarState.leadTwo = lead
@@ -109,6 +110,7 @@ class Plant:
     acceleration = log.XYZTData.new_message()
     acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
     model.modelV2.acceleration = acceleration
+    model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
 
     control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
     control.controlsState.vCruise = float(v_cruise * 3.6)
@@ -117,10 +119,13 @@ class Plant:
     control.controlsState.forceDecel = self.force_decel
     car_state.carState.vEgo = float(self.speed)
     car_state.carState.standstill = self.speed < 0.01
+    car_state.carState.vCruise = float(v_cruise * 3.6)
+    car_control.carControl.orientationNED = [0., float(pitch), 0.]
 
     # ******** get controlsState messages for plotting ***
     sm = {'radarState': radar.radarState,
           'carState': car_state.carState,
+          'carControl': car_control.carControl,
           'controlsState': control.controlsState,
           'modelV2': model.modelV2}
     self.planner.update(sm)

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -82,6 +82,44 @@ def create_maneuvers(kwargs):
       breakpoints=[0.0, 2., 2.01],
       **kwargs,
     ),
+    Maneuver(
+      "approach stopped car at 20m/s, with prob_throttle_values and pitch = -0.1",
+      duration=30.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=120.,
+      speed_lead_values=[0.0, 0., 0.],
+      prob_throttle_values=[1., 0., 0.],
+      cruise_values=[20., 20., 20.],
+      pitch_values=[0., -0.1, -0.1],
+      breakpoints=[0.0, 2., 2.01],
+      **kwargs,
+    ),
+    Maneuver(
+      "approach stopped car at 20m/s, with prob_throttle_values and pitch = +0.1",
+      duration=30.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=120.,
+      speed_lead_values=[0.0, 0., 0.],
+      prob_throttle_values=[1., 0., 0.],
+      cruise_values=[20., 20., 20.],
+      pitch_values=[0., 0.1, 0.1],
+      breakpoints=[0.0, 2., 2.01],
+      **kwargs,
+    ),
+    Maneuver(
+      "slow to 5m/s with allow_throttle = False and pitch = +0.1",
+      duration=25.,
+      initial_speed=20.,
+      lead_relevancy=False,
+      prob_throttle_values=[1., 0., 0.],
+      cruise_values=[20., 20., 20.],
+      pitch_values=[0., 0.1, 0.1],
+      breakpoints=[0.0, 2., 2.01],
+      ensure_slowdown=True,
+      **kwargs,
+    ),
     Maneuver(
       "approach slower cut-in car at 20m/s",
       duration=20.,