mostly tested, and awesome

cereal/log.capnp

@@ -620,6 +620,7 @@ struct PathPlan {
   mpcAngles @14 :List(Float32);
   mpcRates @15 :List(Float32);
   mpcTimes @16 :List(Float32);
+  laneProb @17 :Float32;
   valid @9 :Bool;
   paramsValid @10 :Bool;
   modelValid @12 :Bool;

dashboard.py

@@ -11,6 +11,12 @@
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 from common.realtime import set_realtime_priority, Ratekeeper
 from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv, calc_poly_curvature
+import importlib
+from collections import defaultdict, deque
+from fastcluster import linkage_vector
+from selfdrive.controls.lib.vehicle_model import VehicleModel
+from cereal import car
+from common.params import Params
 
 try:
   from selfdrive.kegman_conf import kegman_conf
@@ -36,10 +42,11 @@ def dashboard_thread(rate=200):
   vEgo = 0.0
   pathPlan = messaging.sub_sock(context, service_list['pathPlan'].port, addr=ipaddress, conflate=True, poller=poller)
   #pathPlan = None
-  live100 = messaging.sub_sock(context, service_list['live100'].port, addr=ipaddress, conflate=False, poller=poller)
-  liveParameters = messaging.sub_sock(context, service_list['liveParameters'].port, addr=ipaddress, conflate=True, poller=poller)
+  live100 = messaging.sub_sock(context, service_list['live100'].port, addr=ipaddress, conflate=True, poller=poller)
+  #liveParameters = messaging.sub_sock(context, service_list['liveParameters'].port, addr=ipaddress, conflate=True, poller=poller)
+  liveParameters = None
   #live20 = messaging.sub_sock(context, service_list['live20'].port, addr=ipaddress, conflate=True, poller=poller)
-  #model = messaging.sub_sock(context, service_list['model'].port, addr=ipaddress, conflate=True, poller=poller)
+  model = messaging.sub_sock(context, service_list['model'].port, addr=ipaddress, conflate=True, poller=poller)
   #frame = messaging.sub_sock(context, service_list['frame'].port, addr=ipaddress, conflate=True, poller=poller)
   #sensorEvents = messaging.sub_sock(context, service_list['sensorEvents'].port, addr=ipaddress, conflate=True, poller=poller)
   #carControl = messaging.sub_sock(context, service_list['carControl'].port, addr=ipaddress, conflate=True, poller=poller)
@@ -53,7 +60,7 @@ def dashboard_thread(rate=200):
   carControl = "disabled"
   sensorEvents = "disabled"
   frame = "disabled"
-  model = "disabled"
+  #model = "disabled"
   live20 = "disabled"
 
   _model = None
@@ -82,6 +89,9 @@ def dashboard_thread(rate=200):
   prev_l_curv = None
   prev_r_curv = None
   prev_p_curv = None
+  prev_l_sum = 0
+  prev_r_sum = 0
+  prev_p_sum = 0
 
   current_rate = rate
   rk = Ratekeeper(current_rate, print_delay_threshold=np.inf)
@@ -90,6 +100,10 @@ def dashboard_thread(rate=200):
   monoTimeOffset = 0
   receiveTime = 0
 
+  CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
+  VM = VehicleModel(CP)
+  _path_pinv = compute_path_pinv()
+
   while 1:
     #try:
       #receiveTime = int(time.time() * 1000000000)
@@ -123,6 +137,7 @@ def dashboard_thread(rate=200):
               actual_angle_change_noise = ((99. * actual_angle_change_noise) + (math.pow(abs_angle_change, 2.))) / 100.
               last_desired = l100.live100.angleSteersDes
               last_actual = l100.live100.angleSteers
+              v_curv = l100.live100.curvature
 
               influxLineString += ("opData,sources=capnp ang_err_noise=%1.1f,des_noise=%1.1f,ang_noise=%1.1f,angle_steers_des=%1.2f,angle_steers=%1.2f,dampened_angle_steers_des=%1.2f,dampened_angle_rate_des=%1.2f,dampened_angle_steers=%1.2f,v_ego=%1.2f,steer_override=%1.2f,v_ego=%1.4f,p=%1.2f,i=%1.4f,f=%1.4f,cumLagMs=%1.2f,vCruise=%1.2f %s\n" %
                           (angle_error_noise, desired_angle_change_noise, actual_angle_change_noise, l100.live100.angleSteersDes, l100.live100.angleSteers, l100.live100.dampAngleSteersDes,  l100.live100.dampAngleRateDes, l100.live100.dampAngleSteers, l100.live100.vEgo, l100.live100.steerOverride, l100.live100.vPid,
@@ -177,8 +192,65 @@ def dashboard_thread(rate=200):
               driverMonitoringOn = false,
               alertType = "",
               alertSound = "" ) )'''
-        #elif socket is model:
-        #  _model = messaging.recv_one(socket)
+        elif socket is model:
+          _model = messaging.recv_one(socket)
+          md = _model.model
+          if vEgo > 0: # and l100.live100.active:
+            influxLineString += ("opLines,sources=capnp ")
+            influxLineString += ("l0=%1.3f,p0=%1.3f,r0=%1.3f" % (md.leftLane.points[0], md.path.points[0], md.rightLane.points[0]))
+            lp = md.leftLane.points
+            rp = md.rightLane.points
+            pp = md.path.points
+
+            p_sum = np.sum(md.path.points)
+            l_sum = np.sum(md.leftLane.points)
+            r_sum = np.sum(md.rightLane.points)
+
+            l_change = l_sum - prev_l_sum
+            r_change = r_sum - prev_r_sum
+            p_change = p_sum - prev_p_sum
+            prev_l_sum = l_sum
+            prev_r_sum = r_sum
+            prev_p_sum = p_sum
+
+
+            p_poly = model_polyfit(md.path.points, _path_pinv)
+            l_poly = model_polyfit(md.leftLane.points, _path_pinv)
+            r_poly = model_polyfit(md.rightLane.points, _path_pinv)
+
+            far_pinv = [_path_pinv[0][25:50],_path_pinv[1][25:50],_path_pinv[2][25:50],_path_pinv[3][25:50]]
+            near_pinv = [_path_pinv[0][0:30],_path_pinv[1][0:30],_path_pinv[2][0:30],_path_pinv[3][0:30]]
+
+            p_poly_far = model_polyfit(map(float, md.path.points)[25:50], far_pinv)  # predicted path
+            l_poly_far = model_polyfit(map(float, md.leftLane.points)[25:50], far_pinv)  # left line
+            r_poly_far = model_polyfit(map(float, md.rightLane.points)[25:50], far_pinv)  # right line
+
+            p_poly_near = model_polyfit(map(float, md.path.points)[0:30], near_pinv)  # predicted path
+            l_poly_near = model_polyfit(map(float, md.leftLane.points)[0:30], near_pinv)  # left line
+            r_poly_near = model_polyfit(map(float, md.rightLane.points)[0:30], near_pinv)  # right line
+
+            p_curv = calc_poly_curvature(p_poly)
+            l_curv = calc_poly_curvature(l_poly)
+            r_curv = calc_poly_curvature(r_poly)
+            p_curv1 = calc_poly_curvature(p_poly_far)
+            l_curv1 = calc_poly_curvature(l_poly_far)
+            r_curv1 = calc_poly_curvature(r_poly_far)
+            p_curv2 = calc_poly_curvature(p_poly_near)
+            l_curv2 = calc_poly_curvature(l_poly_near)
+            r_curv2 = calc_poly_curvature(r_poly_near)
+            #left_curv = calc_poly_curvature([lp[0], lp[9], lp[19], lp[29], lp[39],lp[49]])
+            #path_curv = calc_poly_curvature([pp[0], pp[9], pp[19], pp[29], pp[39],pp[49]])
+            #path_curv = calc_poly_curvature(md.path.points)
+            #right_curv = calc_poly_curvature([rp[0], rp[9], rp[19], rp[29], rp[39],rp[49]])  #calc_poly_curvature(md.rightLane.points)
+            #print(left_curv, path_curv, right_curv)
+            for i in range(1,50):
+              influxLineString += (",l%d=%1.3f,p%d=%1.3f,r%d=%1.3f" % (i, md.leftLane.points[i], i, md.path.points[i], i, md.rightLane.points[i]))
+            #influxLineString += (",left_curv=%1.1f %s\n" % (left_curv, receiveTime))
+            influxLineString += (",vEgo=%1.1f,vCurv=%1.5f,lstd=%1.2f,rstd=%1.2f,pstd=%1.2f,lsum=%d,rsum=%d,psum=%d,lchange=%d,rchange=%d,pchange=%d,lProb=%1.2f,pProb=%1.2f,rProb=%1.2f,p_curv1=%1.5f,l_curv1=%1.5f,r_curv1=%1.5f,p_curv2=%1.5f,l_curv2=%1.5f,r_curv2=%1.5f,v_curv=%1.5f,p_curv=%1.5f,l_curv=%1.5f,r_curv=%1.5f %s\n" % \
+                 (vEgo, v_curv, md.leftLane.std,md.rightLane.std,md.path.std,l_sum, r_sum, p_sum, l_change,r_change,p_change,md.leftLane.prob, md.path.prob, md.rightLane.prob,p_curv1,l_curv1,r_curv1,p_curv2,l_curv2,r_curv2, v_curv, p_curv,l_curv, r_curv, receiveTime))
+
+            frame_count += 1
+
           '''model = (
                   frameId = 19786,
                   path = (
@@ -446,7 +518,7 @@ def dashboard_thread(rate=200):
     #  print(identifier)
     #  pass
 
-def main(rate=200):
+def main(rate=100):
   dashboard_thread(rate)
 
 if __name__ == "__main__":

selfdrive/controls/controlsd.py

@@ -343,7 +343,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
     "vEgoRaw": CS.vEgoRaw,
     "angleSteers": CS.steeringAngle,
     "dampAngleSteers": float(LaC.dampened_angle_steers),
-    "curvature": VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo),
+    "curvature": VM.calc_curvature((LaC.dampened_angle_steers - path_plan.pathPlan.angleOffset) * CV.DEG_TO_RAD, CS.vEgo),
     "steerOverride": CS.steeringPressed,
     "state": state,
     "engageable": not bool(get_events(events, [ET.NO_ENTRY])),

selfdrive/controls/lib/latcontrol.py

@@ -57,6 +57,7 @@ def __init__(self, CP):
     self.rough_steers_rate = 0.0
     self.prev_angle_steers = 0.0
     self.calculate_rate = True
+    self.lane_prob_reset = False
 
     KpV = [interp(25.0, CP.steerKpBP, CP.steerKpV)]
     KiV = [interp(25.0, CP.steerKiBP, CP.steerKiV)]
@@ -128,6 +129,12 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM
         if not steer_override:
           self.dampened_angle_steers = (((self.actual_smoothing - 1.) * self.dampened_angle_steers) + projected_angle_steers) / self.actual_smoothing
 
+      if path_plan.laneProb == 0.0 and self.lane_prob_reset == False:
+        self.dampened_desired_angle = path_plan.angleSteers
+        self.lane_prob_reset = True
+      elif path_plan.laneProb > 0.0:
+        self.lane_prob_reset = False
+
       if CP.steerControlType == car.CarParams.SteerControlType.torque:
 
         steers_max = get_steer_max(CP, v_ego)

selfdrive/controls/lib/model_parser.py

@@ -13,8 +13,20 @@ def __init__(self):
     self.r_poly = [0., 0., 0., 0.]
     self.l_avg_poly = [0., 0., 0., 0.]
     self.r_avg_poly = [0., 0., 0., 0.]
-    self.p_curv = 0.0
+    self.v_avg_curv = 0.0
+    self.p_avg_curv_far = 0.0
+    self.l_avg_curv_far = 0.0
+    self.r_avg_curv_far = 0.0
+    self.p_avg_curv_near = 0.0
+    self.l_avg_curv_near = 0.0
+    self.r_avg_curv_near = 0.0
+    self.l_sum_avg = 0.0
+    self.r_sum_avg = 0.0
+
     self.c_prob = 0.
+    self.l_sum = 0
+    self.p_sum = 0
+    self.r_sum = 0
     self.last_model = 0.
     self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
     self._path_pinv = compute_path_pinv()
@@ -24,13 +36,27 @@ def __init__(self):
     self.lane_width = 3.7
     self.l_prob = 0.
     self.r_prob = 0.
+    self.lane_prob= 0.
 
-  def update(self, v_ego, md):
+  def update(self, v_ego, md, v_curv=0.0):
     if md is not None:
       p_poly = model_polyfit(md.model.path.points, self._path_pinv)  # predicted path
       l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv)  # left line
       r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv)  # right line
 
+      '''
+      #print(self._path_pinv[0:4][25:50])
+      far_pinv = [self._path_pinv[0][25:50],self._path_pinv[1][25:50],self._path_pinv[2][25:50],self._path_pinv[3][25:50]]
+      near_pinv = [self._path_pinv[0][0:30],self._path_pinv[1][0:30],self._path_pinv[2][0:30],self._path_pinv[3][0:30]]
+      p_poly_far = model_polyfit(map(float, md.model.path.points)[25:50], far_pinv)  # predicted path
+      l_poly_far = model_polyfit(map(float, md.model.leftLane.points)[25:50], far_pinv)  # left line
+      r_poly_far = model_polyfit(map(float, md.model.rightLane.points)[25:50], far_pinv)  # right line
+
+      p_poly_near = model_polyfit(map(float, md.model.path.points)[0:30], near_pinv)  # predicted path
+      l_poly_near = model_polyfit(map(float, md.model.leftLane.points)[0:30], near_pinv)  # left line
+      r_poly_near = model_polyfit(map(float, md.model.rightLane.points)[0:30], near_pinv)  # right line
+      '''
+
       # only offset left and right lane lines; offsetting p_poly does not make sense
       l_poly[3] += CAMERA_OFFSET
       r_poly[3] += CAMERA_OFFSET
@@ -49,8 +75,10 @@ def update(self, v_ego, md):
                         (1 - self.lane_width_certainty) * speed_lane_width
 
       lane_width_diff = abs(self.lane_width - current_lane_width)
-      lane_prob = interp(lane_width_diff, [0.2, 1.0], [1.0, 0.0])
+      lane_prob = interp(lane_width_diff, [0.1, 0.5], [1.0, 0.0])
+
 
+      '''
       l_divergence = (l_poly[2] - self.l_avg_poly[2])
       r_divergence = (self.r_avg_poly[2] - r_poly[2])
       self.p_curv = ((9.0 * self.p_curv) + calc_poly_curvature(p_poly)) / 10.0
@@ -71,9 +99,62 @@ def update(self, v_ego, md):
         #print("   left curv prob  %1.2f" % curv_prob)
         l_prob *= curv_prob
         p_prob *= curv_prob
+      left = md.model.leftLane
+      right = md.model.rightLane
+      l_sum = left.points[0:10]
+      r_sum = right.points[0:10]
+
+      #l_diverging = (l_sum - self.l_sum_avg) > abs(r_sum - self.r_sum_avg) and (v_curv > self.v_avg_curv)
+      #r_diverging = (self.l_sum_avg - r_sum) > abs(l_sum - self.l_sum_avg) and (v_curv < self.v_avg_curv)
+      '''
+
+      steer_compensation = v_curv * v_ego
+      total_left_divergence = md.model.leftLane.points[5] - md.model.leftLane.points[0] + steer_compensation
+      total_right_divergence = -(md.model.rightLane.points[5] - md.model.rightLane.points[0] + steer_compensation)
+
+      if (total_left_divergence > abs(total_right_divergence) and (self.lane_prob > 0 or self.r_prob > 0)) or (self.lane_prob == 0 and self.l_prob == 0):
+        l_prob *= lane_prob
+        p_prob *= lane_prob # (1.0 - (1.0 - lane_prob) / 2.0)
+        if lane_prob == 0.0:
+          r_prob *= 1.5
+      elif total_right_divergence > abs(total_left_divergence) or (self.lane_prob == 0 and self.r_prob == 0):
+        r_prob *= lane_prob
+        p_prob *= lane_prob # (1.0 - (1.0 - lane_prob) / 2.0)
+        if lane_prob == 0.0:
+          l_prob *= 1.5     #(1.0 + (1.0 - lane_prob))
+
+      '''if lane_prob == 0 and self.l_prob == 0.0 and self.r_prob > 0.4 and r_prob > 0.4:
+        l_prob = 0.0
+      elif lane_prob == 0 and self.r_prob == 0.0 and self.l_prob > 0.4 and l_prob > 0.4:
+        r_prob = 0.0
+      '''
+
+      '''if (l_curv_diff_far) > (l_curv_diff_near) or (v_curv > self.v_avg_curv and self.r_poly[3] > r_poly[3]) or self.l_prob == 0.0: # and left.std > right.std:
+        l_prob *= lane_prob
+        if (p_curv_diff_far) > (p_curv_diff_near):
+          p_prob *= (1.0 - ((1.0 - lane_prob) / 2.0))
+        print(l_prob, r_prob, p_prob)
+      elif (r_curv_diff_far) < (r_curv_diff_near) or (v_curv < self.v_avg_curv and self.l_poly[3] < l_poly[3]): # and left.std < right.std:
+        r_prob *= lane_prob
+        if (p_curv_far - self.p_avg_curv_far) < (p_curv_far - self.p_avg_curv_near):
+          p_prob *= (1.0 - ((1.0 - lane_prob) / 2.0))
+        print("  right")
+      elif (l_curv_near - self.l_avg_curv_near) > (r_curv_near - self.r_avg_curv_near): # and left.std > right.std:
+        l_prob *= lane_prob
+        if (p_curv_near - self.p_avg_curv_near) > (r_curv_near - self.r_avg_curv_near):
+          p_prob *= (1.0 - ((1.0 - lane_prob) / 2.0))
+        print(l_prob, r_prob, p_prob)
+      elif (r_curv_near - self.r_avg_curv_near) < (l_curv_near - self.l_avg_curv_near): # and left.std < right.std:
+        r_prob *= lane_prob
+        if (r_curv_near - self.r_avg_curv_near) < (p_curv_near - self.p_avg_curv_near):
+          p_prob *= (1.0 - ((1.0 - lane_prob) / 2.0))
+        print("  right")
+
 
-      self.l_avg_poly[2] = ((9.0 * self.l_avg_poly[2]) + l_poly[2]) / 10.0
-      self.r_avg_poly[2] = ((9.0 * self.r_avg_poly[2]) + r_poly[2]) / 10.0
+      #self.v_avg_curv += 0.25 * (v_curv - self.v_avg_curv)
+      self.l_sum_avg += 0.25 * (l_sum - self.l_sum_avg)
+      self.r_sum_avg += 0.25 * (r_sum - self.r_sum_avg)
+      '''
 
       self.lead_dist = md.model.lead.dist
       self.lead_prob = md.model.lead.prob
@@ -83,6 +164,7 @@ def update(self, v_ego, md):
       self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
         l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width)
 
+      self.lane_prob = lane_prob
       self.r_poly = r_poly
       self.r_prob = r_prob
 

selfdrive/controls/lib/pathplanner.py

@@ -53,11 +53,12 @@ def setup_mpc(self, steer_rate_cost):
   def update(self, CP, VM, CS, md, live100, live_parameters):
     v_ego = CS.carState.vEgo
     angle_steers = live100.live100.dampAngleSteers
+    v_curv = live100.live100.curvature
     active = live100.live100.active
 
     angle_offset_bias = live100.live100.angleModelBias + live_parameters.liveParameters.angleOffsetAverage
 
-    self.MP.update(v_ego, md)
+    self.MP.update(v_ego, md, v_curv)
 
     # Run MPC
     self.angle_steers_des_prev = live100.live100.dampAngleSteersDes
@@ -131,6 +132,7 @@ def update(self, CP, VM, CS, md, live100, live_parameters):
     plan_send.pathPlan.mpcAngles = map(float, self.mpc_angles)
     plan_send.pathPlan.mpcRates = map(float, self.mpc_rates)
     plan_send.pathPlan.mpcTimes = map(float, self.mpc_times)
+    plan_send.pathPlan.laneProb =float(self.MP.lane_prob)
     plan_send.pathPlan.valid = bool(plan_valid)
     plan_send.pathPlan.paramsValid = bool(live_parameters.liveParameters.valid)
 

selfdrive/controls/radard.py

@@ -132,6 +132,7 @@ def radard_thread(gctx=None):
     if l100 is not None:
       active = l100.live100.active
       v_ego = l100.live100.vEgo
+      v_curv = l100.live100.curvature
       steer_angle = l100.live100.angleSteers
       steer_override = l100.live100.steerOverride
 
@@ -147,7 +148,7 @@ def radard_thread(gctx=None):
       last_md_ts = md.logMonoTime
 
     # *** get path prediction from the model ***
-    MP.update(v_ego, md)
+    MP.update(v_ego, md, v_curv)
 
     # run kalman filter only if prob is high enough
     if MP.lead_prob > 0.7: