From 24eeec4e0b236ce87b98f237c775d783f6c82964 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Thu, 5 Mar 2020 15:59:49 -0800 Subject: [PATCH] Locationd packet (#1204) * initial * add desire * initial * some progress * fill in, untests * add timestamping * fixes * unix timestamp * no debug print * cleanre * normal syntax * no laika --- selfdrive/locationd/locationd.py | 139 +++++++++++++++++++++++-------- 1 file changed, 105 insertions(+), 34 deletions(-) diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 3cf9560d6..4c958f139 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -6,56 +6,113 @@ import numpy as np import cereal.messaging as messaging import common.transformations.coordinates as coord from common.transformations.orientation import (ecef_euler_from_ned, - euler2quat, + euler_from_quat, ned_euler_from_ecef, - quat2euler, - rotations_from_quats) + quat_from_euler, + rot_from_quat, rot_from_euler) from selfdrive.locationd.kalman.helpers import ObservationKind, KalmanError from selfdrive.locationd.kalman.models.live_kf import LiveKalman, States from selfdrive.swaglog import cloudlog +#from datetime import datetime +#from laika.gps_time import GPSTime + VISION_DECIMATION = 2 SENSOR_DECIMATION = 10 +def to_float(arr): + return [float(arr[0]), float(arr[1]), float(arr[2])] + + class Localizer(): def __init__(self, disabled_logs=[], dog=None): self.kf = LiveKalman() self.reset_kalman() self.max_age = .2 # seconds self.disabled_logs = disabled_logs + self.calib = np.zeros(3) + self.device_from_calib = np.eye(3) + self.calib_from_device = np.eye(3) + self.calibrated = 0 def liveLocationMsg(self, time): - fix = messaging.log.LiveLocationData.new_message() - predicted_state = self.kf.x + predicted_std = np.diagonal(self.kf.P) fix_ecef = predicted_state[States.ECEF_POS] + fix_ecef_std = predicted_std[States.ECEF_POS_ERR] + vel_ecef = predicted_state[States.ECEF_VELOCITY] + vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR] fix_pos_geo = coord.ecef2geodetic(fix_ecef) - fix.lat = float(fix_pos_geo[0]) - fix.lon = float(fix_pos_geo[1]) - fix.alt = float(fix_pos_geo[2]) + fix_pos_geo_std = coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo + ned_vel = self.converter.ecef2ned(fix_ecef + vel_ecef) - self.converter.ecef2ned(fix_ecef) + ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef) + device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T + vel_device = device_from_ecef.dot(vel_ecef) + vel_device_std = device_from_ecef.dot(vel_ecef_std) + orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]) + orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR] + orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef) + orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned + vel_calib = self.calib_from_device.dot(vel_device) + vel_calib_std = self.calib_from_device.dot(vel_device_std) + acc_calib = self.calib_from_device.dot(predicted_state[States.ACCELERATION]) + acc_calib_std = self.calib_from_device.dot(predicted_std[States.ACCELERATION_ERR]) + ang_vel_calib = self.calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY]) + ang_vel_calib_std = self.calib_from_device.dot(predicted_std[States.ANGULAR_VELOCITY_ERR]) - fix.speed = float(np.linalg.norm(predicted_state[States.ECEF_VELOCITY])) - orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[States.ECEF_ORIENTATION])) - fix.roll = math.degrees(orientation_ned_euler[0]) - fix.pitch = math.degrees(orientation_ned_euler[1]) - fix.heading = math.degrees(orientation_ned_euler[2]) + fix = messaging.log.LiveLocationKalman.new_message() + fix.positionGeodetic.value = to_float(fix_pos_geo) + fix.positionGeodetic.std = to_float(fix_pos_geo_std) + fix.positionGeodetic.valid = True + fix.positionECEF.value = to_float(fix_ecef) + fix.positionECEF.std = to_float(fix_ecef_std) + fix.positionECEF.valid = True + fix.velocityECEF.value = to_float(vel_ecef) + fix.velocityECEF.std = to_float(vel_ecef_std) + fix.velocityECEF.valid = True + fix.velocityNED.value = to_float(ned_vel) + fix.velocityNED.std = to_float(ned_vel_std) + fix.velocityNED.valid = True + fix.velocityDevice.value = to_float(vel_device) + fix.velocityDevice.std = to_float(vel_device_std) + fix.velocityDevice.valid = True + fix.accelerationDevice.value = to_float(predicted_state[States.ACCELERATION]) + fix.accelerationDevice.std = to_float(predicted_std[States.ACCELERATION_ERR]) + fix.accelerationDevice.valid = True - fix.gyro = [float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12])] - fix.accel = [float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21])] + fix.orientationECEF.value = to_float(orientation_ecef) + fix.orientationECEF.std = to_float(orientation_ecef_std) + fix.orientationECEF.valid = True + fix.orientationNED.value = to_float(orientation_ned) + fix.orientationNED.std = to_float(orientation_ned_std) + fix.orientationNED.valid = True + fix.angularVelocityDevice.value = to_float(predicted_state[States.ANGULAR_VELOCITY]) + fix.angularVelocityDevice.std = to_float(predicted_std[States.ANGULAR_VELOCITY_ERR]) + fix.angularVelocityDevice.valid = True - ned_vel = self.converter.ecef2ned(predicted_state[States.ECEF_POS] + predicted_state[States.ECEF_VELOCITY]) - self.converter.ecef2ned(predicted_state[States.ECEF_POS]) - fix.vNED = [float(ned_vel[0]), float(ned_vel[1]), float(ned_vel[2])] - fix.source = 'kalman' + fix.velocityCalibrated.value = to_float(vel_calib) + fix.velocityCalibrated.std = to_float(vel_calib_std) + fix.velocityCalibrated.valid = True + fix.angularVelocityCalibrated.value = to_float(ang_vel_calib) + fix.angularVelocityCalibrated.std = to_float(ang_vel_calib_std) + fix.angularVelocityCalibrated.valid = True + fix.accelerationCalibrated.value = to_float(acc_calib) + fix.accelerationCalibrated.std = to_float(acc_calib_std) + fix.accelerationCalibrated.valid = True - #local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY]) - #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.gpsWeek = self.time.week + #fix.gpsTimeOfWeek = self.time.tow + fix.unixTimestampMillis = self.unix_timestamp_millis - imu_frame = predicted_state[States.IMU_OFFSET] - fix.imuFrame = [math.degrees(imu_frame[0]), math.degrees(imu_frame[1]), math.degrees(imu_frame[2])] + if self.filter_ready and self.calibrated: + fix.status = 'valid' + elif self.filter_ready: + fix.status = 'uncalibrated' + else: + fix.status = 'uninitialized' return fix def update_kalman(self, time, kind, meas): @@ -75,23 +132,26 @@ class Localizer(): self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude]) fix_ecef = self.converter.ned2ecef([0, 0, 0]) + #self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3)) + self.unix_timestamp_millis = log.timestamp + # TODO initing with bad bearing not allowed, maybe not bad? if not self.filter_ready and log.speed > 5: self.filter_ready = True initial_ecef = fix_ecef 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) + initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef) gps_speed = log.speed quat_uncertainty = 0.2**2 - initial_pose_ecef_quat = euler2quat(initial_pose_ecef) + initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef) initial_state = LiveKalman.initial_x initial_covs_diag = LiveKalman.initial_P_diag initial_state[States.ECEF_POS] = initial_ecef initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat - initial_state[States.ECEF_VELOCITY] = rotations_from_quats(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0])) + initial_state[States.ECEF_VELOCITY] = rot_from_quat(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0])) initial_covs_diag[States.ECEF_POS_ERR] = 10**2 initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty @@ -119,12 +179,16 @@ class Localizer(): self.cam_counter += 1 if self.cam_counter % VISION_DECIMATION == 0: + rot_device = self.device_from_calib.dot(log.rot) + rot_device_std = self.device_from_calib.dot(log.rotStd) self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, - np.concatenate([log.rot, log.rotStd])) + np.concatenate([rot_device, rot_device_std])) + trans_device = self.device_from_calib.dot(log.trans) + trans_device_std = self.device_from_calib.dot(log.transStd) self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, - np.concatenate([log.trans, log.transStd])) + np.concatenate([trans_device, trans_device_std])) def handle_sensors(self, current_time, log): # TODO does not yet account for double sensor readings in the log @@ -147,6 +211,12 @@ class Localizer(): v = sensor_reading.acceleration.v self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]]) + def handle_live_calib(self, current_time, log): + self.calib = log.rpyCalib + self.device_from_calib = rot_from_euler(self.calib) + self.calib_from_device = self.device_from_calib.T + self.calibrated = log.calStatus == 1 + def reset_kalman(self): self.filter_time = None self.filter_ready = False @@ -160,9 +230,9 @@ class Localizer(): def locationd_thread(sm, pm, disabled_logs=[]): if sm is None: - sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry']) + sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration']) if pm is None: - pm = messaging.PubMaster(['liveLocation']) + pm = messaging.PubMaster(['liveLocationKalman']) localizer = Localizer(disabled_logs=disabled_logs) @@ -180,16 +250,17 @@ def locationd_thread(sm, pm, disabled_logs=[]): localizer.handle_car_state(t, sm[sock]) elif sock == "cameraOdometry": localizer.handle_cam_odo(t, sm[sock]) + elif sock == "liveCalibration": + localizer.handle_live_calib(t, sm[sock]) if localizer.filter_ready and sm.updated['gpsLocationExternal']: t = sm.logMonoTime['gpsLocationExternal'] msg = messaging.new_message() msg.logMonoTime = t - msg.init('liveLocation') - msg.liveLocation = localizer.liveLocationMsg(t * 1e-9) - - pm.send('liveLocation', msg) + msg.init('liveLocationKalman') + msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9) + pm.send('liveLocationKalman', msg) def main(sm=None, pm=None):