lane filtering in model_parser is not tested

cereal/log.capnp

@@ -1619,6 +1619,7 @@ struct LiveParametersData {
   angleOffsetAverage @3 :Float32;
   stiffnessFactor @4 :Float32;
   steerRatio @5 :Float32;
+  laneWidth @6 :Float32;
 }
 
 struct LiveMapData {

dashboard.py

@@ -10,19 +10,23 @@
 from selfdrive.controls.lib.pathplanner import PathPlanner
 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
+
 try:
   from selfdrive.kegman_conf import kegman_conf
 except:
   pass
 
-def dashboard_thread(rate=200):
+def dashboard_thread(rate=100):
   set_realtime_priority(5)
 
   USER = ''
   PASSWORD = ''
   DBNAME = 'carDB'
   #influx = InfluxDBClient('192.168.1.61', 8086, USER, PASSWORD, DBNAME)
-  influx = InfluxDBClient('192.168.43.221', 8086, USER, PASSWORD, DBNAME)
+  #influx = InfluxDBClient('192.168.43.221', 8086, USER, PASSWORD, DBNAME)
+  influx = InfluxDBClient('192.168.137.1', 8086, USER, PASSWORD, DBNAME)
+
   context = zmq.Context()
   poller = zmq.Poller()
   ipaddress = "127.0.0.1"
@@ -72,6 +76,12 @@ def dashboard_thread(rate=200):
   canDataString = ""
   influxLineString = ""
   captureResonantParams = True
+  skippedLiveParameters = 0
+  l_poly = [0., 0., 0., 0.]
+  r_poly = [0., 0., 0., 0.]
+  prev_l_curv = None
+  prev_r_curv = None
+  prev_p_curv = None
 
   current_rate = rate
   rk = Ratekeeper(current_rate, print_delay_threshold=np.inf)
@@ -85,36 +95,39 @@ def dashboard_thread(rate=200):
       #receiveTime = int(time.time() * 1000000000)
       for socket, event in poller.poll(0):
         if socket is live100:
-          _live100 = messaging.recv_one(socket)
-          vEgo = _live100.live100.vEgo
-          if vEgo > 0: # and _live100.live100.active:
-            if sample_str != "":
-                sample_str += ","
-            receiveTime = int(monoTimeOffset + _live100.logMonoTime)
-            #print(receiveTime, monoTimeOffset, _live100.logMonoTime)
-            if (abs(receiveTime - int(time.time() * 1000000000)) > 10000000000):
-              angle_error_noise = 0.0
-              last_desired = 0.0
-              last_actual = 0.0
-              actual_angle_change_noise = 0.0
-              desired_angle_change_noise = 0.0
-              angle_error_noise = 0.0
-              monoTimeOffset = (time.time() * 1000000000) - _live100.logMonoTime
-              receiveTime = int(monoTimeOffset + _live100.logMonoTime)
-              print(int(time.time() * 1000000000), receiveTime, monoTimeOffset, _live100.logMonoTime)
-              #receiveTime = 1 / 0
-            abs_error = abs(_live100.live100.angleSteers - _live100.live100.angleSteersDes)
-            angle_error_noise = ((99. * angle_error_noise) + (math.pow(abs_error, 2.))) / 100.
-            abs_desired_change = abs(_live100.live100.angleSteersDes - last_desired)
-            desired_angle_change_noise = ((99. * desired_angle_change_noise) + (math.pow(abs_desired_change, 2.))) / 100.
-            abs_angle_change = abs(_live100.live100.angleSteersDes - last_actual)
-            actual_angle_change_noise = ((99. * actual_angle_change_noise) + (math.pow(abs_angle_change, 2.))) / 100.
-            last_desired = _live100.live100.angleSteersDes
-            last_actual = _live100.live100.angleSteers
+          #_live100 = messaging.recv_one(socket)
+          _live100 = messaging.drain_sock(socket)
+          for l100 in _live100:
+            vEgo = l100.live100.vEgo
+            if vEgo > 0: # and l100.live100.active:
+              #if sample_str != "":
+              #    sample_str += ","
+              receiveTime = int(monoTimeOffset + l100.logMonoTime)
+              #print(receiveTime, monoTimeOffset, l100.logMonoTime)
+              if (abs(receiveTime - int(time.time() * 1000000000)) > 10000000000):
+                angle_error_noise = 0.0
+                last_desired = 0.0
+                last_actual = 0.0
+                actual_angle_change_noise = 0.0
+                desired_angle_change_noise = 0.0
+                angle_error_noise = 0.0
+                monoTimeOffset = (time.time() * 1000000000) - l100.logMonoTime
+                receiveTime = int(monoTimeOffset + l100.logMonoTime)
+                print(int(time.time() * 1000000000), receiveTime, monoTimeOffset, l100.logMonoTime)
+                #receiveTime = 1 / 0
+              abs_error = abs(l100.live100.angleSteers - l100.live100.angleSteersDes)
+              angle_error_noise = ((99. * angle_error_noise) + (math.pow(abs_error, 2.))) / 100.
+              abs_desired_change = abs(l100.live100.angleSteersDes - last_desired)
+              desired_angle_change_noise = ((99. * desired_angle_change_noise) + (math.pow(abs_desired_change, 2.))) / 100.
+              abs_angle_change = abs(l100.live100.angleSteersDes - last_actual)
+              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
 
-            sample_str += ("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" %
-                        (angle_error_noise, desired_angle_change_noise, actual_angle_change_noise, _live100.live100.angleSteersDes, _live100.live100.angleSteers, _live100.live100.dampAngleSteersDes,  _live100.live100.dampAngleRateDes, _live100.live100.dampAngleSteers, _live100.live100.vEgo, _live100.live100.steerOverride, _live100.live100.vPid,
-                        _live100.live100.upSteer, _live100.live100.uiSteer, _live100.live100.ufSteer, _live100.live100.cumLagMs, _live100.live100.vCruise))
+              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,
+                          l100.live100.upSteer, l100.live100.uiSteer, l100.live100.ufSteer, l100.live100.cumLagMs, l100.live100.vCruise, receiveTime))
+              frame_count += 1
 
           '''print(_live100)
             live100 = (
@@ -191,22 +204,42 @@ def dashboard_thread(rate=200):
         elif socket is liveParameters:
           _liveParameters = messaging.recv_one(socket)
           lp = _liveParameters.liveParameters
-          if vEgo >= 0: # and _live100.live100.active:
+          skippedLiveParameters += 1
+          if vEgo >= 0 and skippedLiveParameters >= 100: # and _live100.live100.active:
+            skippedLiveParameters= 0
             if sample_str != "":
                 sample_str += ","
-            sample_str += ("angleOffset=%1.2f,angleOffsetAverage=%1.3f,stiffnessFactor=%1.3f,steerRatio=%1.3f" %
-                        (lp.angleOffset, lp.angleOffsetAverage, lp.stiffnessFactor, lp.steerRatio))
+            sample_str = ("angleOffset=%1.2f,angleOffsetAverage=%1.3f,stiffnessFactor=%1.3f,steerRatio=%1.3f,laneWidtb=%1.1f" %
+                        (lp.angleOffset, lp.angleOffsetAverage, lp.stiffnessFactor, lp.steerRatio, lp.laneWidth))
+            influxLineString += ("opData,sources=capnp " + sample_str + " %s\n" % receiveTime)
+            sample_str = ""
+            frame_count += 1
 
         elif socket is pathPlan:
           _pathPlan = messaging.recv_one(socket)
           if vEgo > 0: # and _live100.live100.active:
             if sample_str != "":
                 sample_str += ","
             a = _pathPlan.pathPlan.mpcAngles
-            sample_str += ("lane_width=%1.2f,lpoly=%1.3f,rpoly=%1.3f,cpoly=%1.3f,dpoly=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f,mpc4=%1.2f,mpc5=%1.2f,mpc6=%1.2f,mpc7=%1.2f,mpc8=%1.2f,mpc9=%1.2f,mpc10=%1.2f,mpc11=%1.2f,mpc12=%1.2f,mpc13=%1.2f,mpc14=%1.2f,mpc15=%1.2f,mpc16=%1.2f,mpc17=%1.2f,mpc18=%1.2f" %
-                        (_pathPlan.pathPlan.laneWidth, _pathPlan.pathPlan.lPoly[3], _pathPlan.pathPlan.rPoly[3], _pathPlan.pathPlan.cPoly[3], _pathPlan.pathPlan.dPoly[3],
+
+            d_curv = int(calc_poly_curvature(map(float, _pathPlan.pathPlan.dPoly)) * 50000)
+            l_curv = int(calc_poly_curvature(map(float, _pathPlan.pathPlan.lPoly)) * 50000)
+            r_curv = int(calc_poly_curvature(map(float, _pathPlan.pathPlan.rPoly)) * 50000)
+
+            #if prev_p_curv is not None:
+            #  print("l_curv: %d  r_curv: %d  p_curv: %d  l_diff: %d  r_diff: %d  p_diff: %d" % (l_curv, r_curv, d_curv, prev_l_curv - l_curv, prev_r_curv - r_curv, prev_p_curv - p_curv))
+
+            #prev_l_curv = l_curv
+            #prev_r_curv = r_curv
+            #prev_p_curv = p_curv
+
+            sample_str = ("lane_width=%1.2f,d_curv=%d,l_curv=%d,r_curv=%d,lpoly2=%1.3f,rpoly2=%1.3f,cpoly2=%1.3f,dpoly2=%1.3f,lpoly3=%1.3f,rpoly3=%1.3f,cpoly3=%1.3f,dpoly3=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f,mpc4=%1.2f,mpc5=%1.2f,mpc6=%1.2f,mpc7=%1.2f,mpc8=%1.2f,mpc9=%1.2f,mpc10=%1.2f,mpc11=%1.2f,mpc12=%1.2f,mpc13=%1.2f,mpc14=%1.2f,mpc15=%1.2f,mpc16=%1.2f,mpc17=%1.2f,mpc18=%1.2f" %
+                        (_pathPlan.pathPlan.laneWidth, d_curv, l_curv, r_curv, _pathPlan.pathPlan.lPoly[2], _pathPlan.pathPlan.rPoly[2], _pathPlan.pathPlan.cPoly[2], _pathPlan.pathPlan.dPoly[2],_pathPlan.pathPlan.lPoly[3], _pathPlan.pathPlan.rPoly[3], _pathPlan.pathPlan.cPoly[3], _pathPlan.pathPlan.dPoly[3],
                               _pathPlan.pathPlan.cProb,  _pathPlan.pathPlan.lProb,  _pathPlan.pathPlan.rProb, a[1], a[2], a[3],
                               a[4], a[5], a[6], a[7], a[8], a[9], a[10], a[11], a[12], a[13], a[14], a[15], a[16], a[17], a[18]))
+            influxLineString += ("opData,sources=capnp " + sample_str + " %s\n" % receiveTime)
+            sample_str = ""
+            frame_count += 1
 
 
         #elif socket is live20:
@@ -375,6 +408,7 @@ def dashboard_thread(rate=200):
         #  _androidLog = messaging.recv_one(socket)
         #  print(_androidLog)
 
+        #print(influxLineString)
         if sample_str != "":
           kegman_counter += 1
           if kegman_counter == 300:

dashfile.py

@@ -0,0 +1,98 @@
+#!/usr/bin/env python
+import csv
+import zmq
+import time
+import numpy as np
+import selfdrive.messaging as messaging
+from selfdrive.services import service_list
+from common.realtime import set_realtime_priority, Ratekeeper
+import os, os.path
+
+# Polling rate should be twice the data rate to prevent aliasing
+def main(rate=200):
+  set_realtime_priority(5)
+  context = zmq.Context()
+  poller = zmq.Poller()
+
+  live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
+  carState = messaging.sub_sock(context, service_list['carState'].port, conflate=True, poller=poller)
+  can = None #messaging.sub_sock(context, service_list['can'].port, conflate=True, poller=poller)
+
+  vEgo = 0.0
+  _live100 = None
+  _can = None
+
+  frame_count = 0
+  skipped_count = 0
+
+  rk = Ratekeeper(rate, print_delay_threshold=np.inf)
+
+  # simple version for working with CWD
+  #print len([name for name in os.listdir('.') if os.path.isfile(name)])
+
+  # path joining version for other paths
+  DIR = '/sdcard/tuning'
+  filenumber = len([name for name in os.listdir(DIR) if os.path.isfile(os.path.join(DIR, name))])
+
+  kegman_counter = 0
+  monoTimeOffset = 0
+  receiveTime = 0
+  angle_rate = 0.0
+
+  print("start")
+  with open(DIR + '/dashboard_file_%d.csv' % filenumber, mode='w') as dash_file:
+    print("opened")
+    dash_writer = csv.writer(dash_file, delimiter=',', quotechar='', quoting=csv.QUOTE_NONE)
+    print("initialized")
+    dash_writer.writerow(['angle_steers_des','angle_steers','angle_rate','dampened_angle_steers_des','dampened_angle_rate_des','dampened_angle_steers','v_ego','steer_override','p','i','f','time'])
+    print("first row")
+
+    while 1:
+      for socket, event in poller.poll(0):
+        if socket is can:
+          _can = messaging.recv_one(socket)
+          print(_can)
+
+        if socket is carState:
+          _carState = messaging.drain_sock(socket)
+          for cs in _carState:
+            angle_rate = cs.carState.steeringRate
+
+        if socket is live100:
+          _live100 = messaging.drain_sock(socket)
+          for l100 in _live100:
+            vEgo = l100.live100.vEgo
+            if vEgo > 0: # and l100.live100.active:
+              receiveTime = int(monoTimeOffset + l100.logMonoTime)
+              if (abs(receiveTime - int(time.time() * 1000000000)) > 10000000000):
+                monoTimeOffset = (time.time() * 1000000000) - l100.logMonoTime
+                receiveTime = int(monoTimeOffset + l100.logMonoTime)
+
+              frame_count += 1
+              dash_writer.writerow([str(round(l100.live100.angleSteersDes, 2)),
+                                    str(round(l100.live100.angleSteers, 2)),
+                                    str(round(angle_rate,1)),
+                                    str(round(l100.live100.dampAngleSteersDes, 2)),
+                                    str(round(l100.live100.dampAngleRateDes, 2)),
+                                    str(round(l100.live100.dampAngleSteers, 2)),
+                                    str(round(l100.live100.vEgo, 1)),
+                                    1 if l100.live100.steerOverride else 0,
+                                    str(round(l100.live100.upSteer, 4)),
+                                    str(round(l100.live100.uiSteer, 4)),
+                                    str(round(l100.live100.ufSteer, 4)),
+                                    str(receiveTime)])
+          else:
+            skipped_count += 1
+        else:
+          skipped_count += 1
+      if frame_count % 200 == 0:
+        print("captured = %d" % frame_count)
+        frame_count += 1
+      if skipped_count % 200 == 0:
+        print("skipped = %d" % skipped_count)
+        skipped_count += 1
+
+      rk.keep_time()
+
+if __name__ == "__main__":
+  main()

selfdrive/boardd/boardd.cc

@@ -222,7 +222,7 @@ void handle_usb_issue(int err, const char func[]) {
   // TODO: check other errors, is simply retrying okay?
 }
 
-bool can_recv(void *s, uint64_t locked_wake_time, bool force_send) {
+bool can_recv(void *s, bool force_send) {
   int err;
   uint32_t data[RECV_SIZE/4];
   int recv, big_index;
@@ -234,11 +234,6 @@ bool can_recv(void *s, uint64_t locked_wake_time, bool force_send) {
   // do recv
   pthread_mutex_lock(&usb_lock);
 
-  cur_time = 1e-3 * nanos_since_boot();
-  if (locked_wake_time > cur_time) {
-    // Short sleep occurs after usb_lock to ensure sync timing
-    usleep(locked_wake_time - cur_time);
-  }
   do {
     err = libusb_bulk_transfer(dev_handle, 0x81, (uint8_t*)data, RECV_SIZE, &recv, TIMEOUT);
     if (err != 0) { handle_usb_issue(err, __func__); }
@@ -494,16 +489,17 @@ void *can_recv_thread(void *crap) {
   zmq_bind(publisher, "tcp://*:8006");
 
   bool frame_sent, skip_once, force_send;
-  uint64_t wake_time, locked_wake_time, last_long_sleep;
+  uint64_t wake_time, cur_time, last_long_sleep;
   int recv_state = 0;
   force_send = true;
   last_long_sleep = 1e-3 * nanos_since_boot();
   wake_time = last_long_sleep;
-  locked_wake_time = wake_time;
 
   while (!do_exit) {
+
+    // TODO: Merge the looping logic after syncID vs non-sincID performance is optimized / debugged
     while (sync_id > 0 && !do_exit) {
-      frame_sent = can_recv(publisher, locked_wake_time, force_send);
+      frame_sent = can_recv(publisher, force_send);
 
       // drain the Panda twice at 4.5ms intervals, then once at 1.0ms interval (twice max if sync_id is set)
       if (frame_sent == true || skip_once == true) {
@@ -534,13 +530,16 @@ void *can_recv_thread(void *crap) {
         }
       }
       else {
-        force_send = (locked_wake_time > last_long_sleep + 1e5);
+        force_send = (wake_time > last_long_sleep);
         wake_time += 1000;
-        locked_wake_time = wake_time;
+        cur_time = 1e-3 * nanos_since_boot();
+        if (wake_time > cur_time) {
+          usleep(wake_time - cur_time);
+        }
       }
     }
     while (sync_id == 0 && !do_exit) {
-      frame_sent = can_recv(publisher, locked_wake_time, force_send);
+      frame_sent = can_recv(publisher, force_send);
 
       // drain the Panda twice at 4.5ms intervals, then once at 1.0ms interval (twice max if sync_id is set)
       if (recv_state++ < 2) {
@@ -573,7 +572,10 @@ void *can_recv_thread(void *crap) {
         force_send = true;
         recv_state = 0;
         wake_time += 1000;
-        locked_wake_time = wake_time;
+        cur_time = 1e-3 * nanos_since_boot();
+        if (wake_time > cur_time) {
+          usleep(wake_time - cur_time);
+        }
       }
     }
   }

selfdrive/controls/lib/latcontrol_helpers.py

@@ -17,13 +17,8 @@ def calc_d_lookahead(v_ego, d_poly):
   # howfar we look ahead is function of speed and how much curvy is the path
   offset_lookahead = 1.
   k_lookahead = 7.
-  # integrate abs value of second derivative of poly to get a measure of path curvature
-  pts_len = 50.  # m
-  if len(d_poly) > 0:
-    pts = np.polyval([6 * d_poly[0], 2 * d_poly[1]], np.arange(0, pts_len))
-  else:
-    pts = 0.
-  curv = np.sum(np.abs(pts)) / pts_len
+
+  curv = abs(calc_poly_curvature(d_poly))
 
   k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
 
@@ -33,6 +28,15 @@ def calc_d_lookahead(v_ego, d_poly):
   d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
   return d_lookahead
 
+def calc_poly_curvature(line_poly):
+  # integrate abs value of second derivative of poly to get a measure of path curvature
+  pts_len = 50.  # m
+  if len(line_poly) > 0:
+    pts = np.polyval([6 * line_poly[0], 2 * line_poly[1]], np.arange(0, pts_len))
+  else:
+    pts = 0.
+  return np.sum((pts)) / pts_len
+
 
 def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
   # this function returns the lateral offset given the steering angle, speed and the lookahead distance