openpilot v0.3.9 tweaks

old-commit-hash: 1ad9cc8c67

selfdrive/test/plant/maneuver.py

@@ -45,7 +45,7 @@ class Maneuver(object):
       grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values)
       speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
 
-      distance, speed, acceleration, distance_lead, brake, gas, steer_torque, live100 = plant.step(speed_lead, current_button, grade)
+      distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, live100= plant.step(speed_lead, current_button, grade)
       if live100:
         last_live100 = live100[-1]
 
@@ -64,7 +64,8 @@ class Maneuver(object):
           v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid,
           cruise_speed=last_live100.vCruise,
           jerk_factor=last_live100.jerkFactor,
-          a_target=last_live100.aTarget)
+          a_target=last_live100.aTarget,
+          fcw=fcw)
     
     print "maneuver end"
 

selfdrive/test/plant/maneuverplots.py

@@ -33,11 +33,13 @@ class ManeuverPlot(object):
 
     self.v_target_array = []
 
+    self.fcw_array = []
+
     self.title = title
     
   def add_data(self, time, gas, brake, steer_torque, distance, speed, 
     acceleration, up_accel_cmd, ui_accel_cmd, d_rel, v_rel, v_lead, 
-    v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target):
+    v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target, fcw):
     self.time_array.append(time)
     self.gas_array.append(gas)
     self.brake_array.append(brake)
@@ -55,6 +57,7 @@ class ManeuverPlot(object):
     self.cruise_speed_array.append(cruise_speed)
     self.jerk_factor_array.append(jerk_factor)
     self.a_target_array.append(a_target)
+    self.fcw_array.append(fcw)
 
 
   def write_plot(self, path, maneuver_name):
@@ -88,10 +91,11 @@ class ManeuverPlot(object):
     plt.plot(
       np.array(self.time_array), np.array(self.acceleration_array), 'g',
       np.array(self.time_array), np.array(self.a_target_array), 'k--',
+      np.array(self.time_array), np.array(self.fcw_array), 'ro',
     )
     plt.xlabel('Time [s]')
     plt.ylabel('Acceleration [m/s^2]')
-    plt.legend(['ego-plant', 'target'], loc=0)
+    plt.legend(['ego-plant', 'target', 'fcw'], loc=0)
     plt.grid()
     pylab.savefig("/".join([path, maneuver_name, 'acceleration.svg']), dpi=1000)
 

selfdrive/test/plant/plant.py

@@ -102,6 +102,7 @@ class Plant(object):
       Plant.model = messaging.pub_sock(context, service_list['model'].port)
       Plant.cal = messaging.pub_sock(context, service_list['liveCalibration'].port)
       Plant.live100 = messaging.sub_sock(context, service_list['live100'].port)
+      Plant.plan = messaging.sub_sock(context, service_list['plan'].port)
       Plant.messaging_initialized = True
 
     self.angle_steer = 0.
@@ -162,10 +163,14 @@ class Plant(object):
     self.cp.update_can(can_msgs)
 
     # ******** get live100 messages for plotting ***
-    live_msgs = []
+    live100_msgs = []
     for a in messaging.drain_sock(Plant.live100):
-      live_msgs.append(a.live100)
+      live100_msgs.append(a.live100)
 
+    fcw = None
+    for a in messaging.drain_sock(Plant.plan):
+      if a.plan.fcw:
+        fcw = True
 
     if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
       brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE']
@@ -264,6 +269,9 @@ class Plant(object):
         x.points = [0.0]*50
         x.prob = 1.0
         x.std = 1.0
+      md.model.lead.dist = float(d_rel)
+      md.model.lead.prob = 1.
+      md.model.lead.std = 0.1
       cal.liveCalibration.calStatus = 1
       cal.liveCalibration.calPerc = 100
       # fake values?
@@ -280,7 +288,7 @@ class Plant(object):
     self.distance_lead_prev = distance_lead
 
     self.rk.keep_time()
-    return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, live_msgs)
+    return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, live100_msgs)
 
 # simple engage in standalone mode
 def plant_thread(rate=100):

selfdrive/test/tests/plant/test_longitudinal.py

@@ -197,9 +197,51 @@ maneuvers = [
                             (CB.RES_ACCEL, 1.8), (0.0, 1.9),
                             (CB.RES_ACCEL, 2.0), (0.0, 2.1),
                             (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+  ),
+  Maneuver(
+    "fcw: traveling at 30 m/s and approaching lead traveling at 20m/s",
+    duration=15.,
+    initial_speed=30.,
+    lead_relevancy=True,
+    initial_distance_lead=100.,
+    speed_lead_values=[20.],
+    speed_lead_breakpoints=[1.],
+    cruise_button_presses = []
+  ),
+  Maneuver(
+    "fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 1m/s2",
+    duration=18.,
+    initial_speed=20.,
+    lead_relevancy=True,
+    initial_distance_lead=35.,
+    speed_lead_values=[20., 0.],
+    speed_lead_breakpoints=[3., 23.],
+    cruise_button_presses = []
+  ),
+  Maneuver(
+    "fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 3m/s2",
+    duration=13.,
+    initial_speed=20.,
+    lead_relevancy=True,
+    initial_distance_lead=35.,
+    speed_lead_values=[20., 0.],
+    speed_lead_breakpoints=[3., 9.6],
+    cruise_button_presses = []
+  ),
+  Maneuver(
+    "fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 5m/s2",
+    duration=8.,
+    initial_speed=20.,
+    lead_relevancy=True,
+    initial_distance_lead=35.,
+    speed_lead_values=[20., 0.],
+    speed_lead_breakpoints=[3., 7.],
+    cruise_button_presses = []
   )
 ]
 
+#maneuvers = [maneuvers[-1]]
+
 def setup_output():
   output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
   if not os.path.exists(os.path.join(output_dir, "index.html")):
@@ -235,6 +277,7 @@ class LongitudinalControl(unittest.TestCase):
     shutil.rmtree('/data/params', ignore_errors=True)
     params = Params()
     params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
+    params.put("IsFcwEnabled", "1")
 
     manager.gctx = {}
     manager.prepare_managed_process('radard')