more fixes

old-commit-hash: 1a1f1182d7

selfdrive/locationd/kalman/live_kf.py

@@ -2,7 +2,7 @@
 import numpy as np
 
 from selfdrive.swaglog import cloudlog
-from selfdrive.locationd.kalman.live_model import gen_model
+from selfdrive.locationd.kalman.live_model import gen_model, States
 from selfdrive.locationd.kalman.kalman_helpers import ObservationKind, KalmanError
 from selfdrive.locationd.kalman.ekf_sym import EKF_sym
 
@@ -29,7 +29,7 @@ initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
 
 
 class LiveKalman():
-  def __init__(self, N=0, max_tracks=3000):
+  def __init__(self):
     # process noise
     Q = np.diag([0.03**2, 0.03**2, 0.03**2,
                  0.0**2, 0.0**2, 0.0**2,
@@ -52,7 +52,7 @@ class LiveKalman():
                       ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
                       ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
 
-    name = f'live_{N}'
+    name = 'live'
     gen_model(name, self.dim_state, self.dim_state_err, [])
 
     # init filter
@@ -105,7 +105,7 @@ class LiveKalman():
       cloudlog.error("Kalman filter quaternions unstable")
       raise KalmanError
 
-    self.filter.x[3:7, 0] = self.filter.x[3:7, 0] / quat_norm
+    self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm
 
     return r
 

selfdrive/locationd/kalman/live_model.py

@@ -1,6 +1,7 @@
 import numpy as np
 import sympy as sp
 import os
+import sysconfig
 
 from laika.constants import EARTH_GM
 from common.sympy_helpers import euler_rotate, quat_rotate, quat_matrix_r
@@ -38,7 +39,7 @@ def gen_model(name, dim_state, dim_state_err, maha_test_kinds):
             dir_path + '/' + name + '_kf.py']
 
     outs = [dir_path + '/' + name + '.o',
-            dir_path + '/' + name + '.so',
+            dir_path + '/' + name + sysconfig.get_config_var('EXT_SUFFIX'),
             dir_path + '/' + name + '.cpp']
     out_times = list(map(os.path.getmtime, outs))
     dep_times = list(map(os.path.getmtime, deps))
@@ -46,7 +47,9 @@ def gen_model(name, dim_state, dim_state_err, maha_test_kinds):
     if min(out_times) > max(dep_times) and not rebuild:
       return
     list(map(os.remove, outs))
-  except OSError:
+  except OSError as e:
+    print('HAHAHA')
+    print(e)
     pass
 
   # make functions and jacobians with sympy
@@ -122,7 +125,7 @@ def gen_model(name, dim_state, dim_state_err, maha_test_kinds):
   err_function_sym = sp.Matrix(np.zeros((dim_state,1)))
   delta_quat = sp.Matrix(np.ones((4)))
   delta_quat[1:,:] = sp.Matrix(0.5*delta_x[3:6,:])
-  err_function_sym[3:7,0] = quat_matrix_r(nom_x[3:6,0])*delta_quat
+  err_function_sym[3:7,0] = quat_matrix_r(nom_x[3:7,0])*delta_quat
   err_function_sym[0:3,:] = sp.Matrix(nom_x[0:3,:] + delta_x[0:3,:])
 
   inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err,1)))

selfdrive/locationd/locationd.py

@@ -49,8 +49,8 @@ class Localizer():
     fix.accel = [float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21])]
 
     local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY])
-    fix.pitchCalibration = math.degrees(math.arctan2(local_vel[2], local_vel[0]))
-    fix.yawCalibration = math.degrees(math.arctan2(local_vel[1], local_vel[0]))
+    fix.pitchCalibration = math.degrees(math.atan2(local_vel[2], local_vel[0]))
+    fix.yawCalibration = math.degrees(math.atan2(local_vel[1], local_vel[0]))
 
     #fix.imuFrame = [(180/np.pi)*float(predicted_state[23]), (180/np.pi)*float(predicted_state[24]), (180/np.pi)*float(predicted_state[25])]
     return fix
@@ -58,7 +58,7 @@ class Localizer():
   def update_kalman(self, time, kind, meas):
     idx = bisect_right([x[0] for x in self.observation_buffer], time)
     self.observation_buffer.insert(idx, (time, kind, meas))
-    while self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
+    while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
       if self.filter_ready:
         try:
           self.kf.predict_and_observe(*self.observation_buffer.pop(0))
@@ -68,18 +68,18 @@ class Localizer():
       else:
         self.observation_buffer.pop(0)
 
-  def handle_gps(self, log, current_time):
-    converter = coord.LocalCoord.from_geodetic([log.gpsLocationExternal.latitude, log.gpsLocationExternal.longitude, log.gpsLocationExternal.altitude])
+  def handle_gps(self, current_time, log):
+    converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
     fix_ecef = converter.ned2ecef([0, 0, 0])
 
     # TODO initing with bad bearing not allowed, maybe not bad?
-    if not self.filter_ready and log.gpsLocationExternal.speed > 5:
+    if not self.filter_ready and log.speed > 5:
       self.filter_ready = True
       initial_ecef = fix_ecef
-      gps_bearing = math.radians(log.gpsLocationExternal.bearing)
+      gps_bearing = math.radians(log.bearing)
       initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing])
       initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
-      gps_speed = log.gpsLocationExternal.speed
+      gps_speed = log.speed
       quat_uncertainty = 0.2**2
       initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
 
@@ -105,25 +105,25 @@ class Localizer():
         cloudlog.error("Locationd vs ubloxLocation difference too large, kalman reset")
         self.reset_kalman()
 
-  def handle_car_state(self, log, current_time):
+  def handle_car_state(self, current_time, log):
     self.speed_counter += 1
 
     if self.speed_counter % SENSOR_DECIMATION == 0:
-      self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, log.carState.vEgo)
+      self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, log.vEgo)
       if log.carState.vEgo == 0:
         self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0])
 
-  def handle_cam_odo(self, log, current_time):
+  def handle_cam_odo(self, current_time, log):
     self.update_kalman(current_time,
                        ObservationKind.CAMERA_ODO_ROTATION,
-                       np.concatenate([log.cameraOdometry.rot, log.cameraOdometry.rotStd]))
+                       np.concatenate([log.rot, log.rotStd]))
     self.update_kalman(current_time,
                        ObservationKind.CAMERA_ODO_TRANSLATION,
-                       np.concatenate([log.cameraOdometry.trans, log.cameraOdometry.transStd]))
+                       np.concatenate([log.trans, log.transStd]))
 
-  def handle_sensors(self, log, current_time):
+  def handle_sensors(self, current_time, log):
     # TODO does not yet account for double sensor readings in the log
-    for sensor_reading in log.sensorEvents:
+    for sensor_reading in log:
       # Gyro Uncalibrated
       if sensor_reading.sensor == 5 and sensor_reading.type == 16:
         self.gyro_counter += 1
@@ -142,23 +142,6 @@ class Localizer():
           v = sensor_reading.acceleration.v
           self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]])
 
-  def handle_log(self, log):
-    current_time = 1e-9 * log.logMonoTime
-
-    typ = log.which
-
-    if typ in self.disabled_logs:
-      return
-
-    if typ == "sensorEvents":
-      self.handle_sensors(log, current_time)
-    elif typ == "gpsLocationExternal":
-      self.handle_gps(log, current_time)
-    elif typ == "carState":
-      self.handle_car_state(log, current_time)
-    elif typ == "cameraOdometry":
-      self.handle_cam_odo(log, current_time)
-
   def reset_kalman(self):
     self.filter_time = None
     self.filter_ready = False
@@ -171,7 +154,7 @@ class Localizer():
 
 def locationd_thread(sm, pm, disabled_logs=[]):
   if sm is None:
-    sm = messaging.SubMaster(['carState', 'gpsLocationExternal', 'sensorEvents', 'cameraOdometry'])
+    sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry'])
   if pm is None:
     pm = messaging.PubMaster(['liveLocation'])
 
@@ -182,7 +165,15 @@ def locationd_thread(sm, pm, disabled_logs=[]):
 
     for sock, updated in sm.updated.items():
       if updated:
-        localizer.handle_log(sm[sock])
+        t = sm.logMonoTime[sock] * 1e-9
+        if sock == "sensorEvents":
+          localizer.handle_sensors(t, sm[sock])
+        elif sock == "gpsLocationExternal":
+          localizer.handle_gps(t, sm[sock])
+        elif sock == "carState":
+          localizer.handle_car_state(t, sm[sock])
+        elif sock == "cameraOdometry":
+          localizer.handle_cam_odo(t, sm[sock])
 
     if localizer.filter_ready and sm.updated['gpsLocationExternal']:
       t = sm.logMonoTime['gpsLocationExternal']