sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Locationd packet (#1204)

Browse Source 
* initial

* add desire

* initial

* some progress

* fill in, untests

* add timestamping

* fixes

* unix timestamp

* no debug print

* cleanre

* normal syntax

* no laika
pull/1202/head^2
HaraldSchafer 5 years ago committed by GitHub
parent 6db044c86c
commit 24eeec4e0b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 111 additions and 40 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 151
      selfdrive/locationd/locationd.py

151
selfdrive/locationd/locationd.py
Unescape View File

@ -6,56 +6,113 @@ import numpy as np
import cereal.messaging as messaging import cereal.messaging as messaging
import common.transformations.coordinates as coord import common.transformations.coordinates as coord
from common.transformations.orientation import (ecef_euler_from_ned, from common.transformations.orientation import (ecef_euler_from_ned,
euler2quat, euler_from_quat,
ned_euler_from_ecef, ned_euler_from_ecef,
quat2euler, quat_from_euler,
rotations_from_quats) rot_from_quat, rot_from_euler)
from selfdrive.locationd.kalman.helpers import ObservationKind, KalmanError from selfdrive.locationd.kalman.helpers import ObservationKind, KalmanError
from selfdrive.locationd.kalman.models.live_kf import LiveKalman, States from selfdrive.locationd.kalman.models.live_kf import LiveKalman, States
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
#from datetime import datetime
#from laika.gps_time import GPSTime
VISION_DECIMATION = 2 VISION_DECIMATION = 2
SENSOR_DECIMATION = 10 SENSOR_DECIMATION = 10
def to_float(arr):
return [float(arr[0]), float(arr[1]), float(arr[2])]
class Localizer(): class Localizer():
def __init__(self, disabled_logs=[], dog=None): def __init__(self, disabled_logs=[], dog=None):
self.kf = LiveKalman() self.kf = LiveKalman()
self.reset_kalman() self.reset_kalman()
self.max_age = .2 # seconds self.max_age = .2 # seconds
self.disabled_logs = disabled_logs 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): def liveLocationMsg(self, time):
fix = messaging.log.LiveLocationData.new_message()
predicted_state = self.kf.x predicted_state = self.kf.x
predicted_std = np.diagonal(self.kf.P)
fix_ecef = predicted_state[States.ECEF_POS] 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_pos_geo = coord.ecef2geodetic(fix_ecef)
fix.lat = float(fix_pos_geo[0]) fix_pos_geo_std = coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo
fix.lon = float(fix_pos_geo[1]) ned_vel = self.converter.ecef2ned(fix_ecef + vel_ecef) - self.converter.ecef2ned(fix_ecef)
fix.alt = float(fix_pos_geo[2]) 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
fix.speed = float(np.linalg.norm(predicted_state[States.ECEF_VELOCITY])) vel_device = device_from_ecef.dot(vel_ecef)
vel_device_std = device_from_ecef.dot(vel_ecef_std)
orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[States.ECEF_ORIENTATION])) orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION])
fix.roll = math.degrees(orientation_ned_euler[0]) orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR]
fix.pitch = math.degrees(orientation_ned_euler[1]) orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef)
fix.heading = math.degrees(orientation_ned_euler[2]) 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)
fix.gyro = [float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12])] vel_calib_std = self.calib_from_device.dot(vel_device_std)
fix.accel = [float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21])] acc_calib = self.calib_from_device.dot(predicted_state[States.ACCELERATION])
acc_calib_std = self.calib_from_device.dot(predicted_std[States.ACCELERATION_ERR])
ned_vel = self.converter.ecef2ned(predicted_state[States.ECEF_POS] + predicted_state[States.ECEF_VELOCITY]) - self.converter.ecef2ned(predicted_state[States.ECEF_POS]) ang_vel_calib = self.calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY])
fix.vNED = [float(ned_vel[0]), float(ned_vel[1]), float(ned_vel[2])] ang_vel_calib_std = self.calib_from_device.dot(predicted_std[States.ANGULAR_VELOCITY_ERR])
fix.source = 'kalman'
#local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY]) fix = messaging.log.LiveLocationKalman.new_message()
#fix.pitchCalibration = math.degrees(math.atan2(local_vel[2], local_vel[0])) fix.positionGeodetic.value = to_float(fix_pos_geo)
#fix.yawCalibration = math.degrees(math.atan2(local_vel[1], local_vel[0])) fix.positionGeodetic.std = to_float(fix_pos_geo_std)
fix.positionGeodetic.valid = True
imu_frame = predicted_state[States.IMU_OFFSET] fix.positionECEF.value = to_float(fix_ecef)
fix.imuFrame = [math.degrees(imu_frame[0]), math.degrees(imu_frame[1]), math.degrees(imu_frame[2])] 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.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
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
#fix.gpsWeek = self.time.week
#fix.gpsTimeOfWeek = self.time.tow
fix.unixTimestampMillis = self.unix_timestamp_millis
if self.filter_ready and self.calibrated:
fix.status = 'valid'
elif self.filter_ready:
fix.status = 'uncalibrated'
else:
fix.status = 'uninitialized'
return fix return fix
def update_kalman(self, time, kind, meas): 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]) self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
fix_ecef = self.converter.ned2ecef([0, 0, 0]) 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? # TODO initing with bad bearing not allowed, maybe not bad?
if not self.filter_ready and log.speed > 5: if not self.filter_ready and log.speed > 5:
self.filter_ready = True self.filter_ready = True
initial_ecef = fix_ecef initial_ecef = fix_ecef
gps_bearing = math.radians(log.bearing) gps_bearing = math.radians(log.bearing)
initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_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 gps_speed = log.speed
quat_uncertainty = 0.2**2 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_state = LiveKalman.initial_x
initial_covs_diag = LiveKalman.initial_P_diag initial_covs_diag = LiveKalman.initial_P_diag
initial_state[States.ECEF_POS] = initial_ecef initial_state[States.ECEF_POS] = initial_ecef
initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat 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_POS_ERR] = 10**2
initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty
@ -119,12 +179,16 @@ class Localizer():
self.cam_counter += 1 self.cam_counter += 1
if self.cam_counter % VISION_DECIMATION == 0: 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, self.update_kalman(current_time,
ObservationKind.CAMERA_ODO_ROTATION, 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, self.update_kalman(current_time,
ObservationKind.CAMERA_ODO_TRANSLATION, ObservationKind.CAMERA_ODO_TRANSLATION,
np.concatenate([log.trans, log.transStd])) np.concatenate([trans_device, trans_device_std]))
def handle_sensors(self, current_time, log): def handle_sensors(self, current_time, log):
# TODO does not yet account for double sensor readings in the log # TODO does not yet account for double sensor readings in the log
@ -147,6 +211,12 @@ class Localizer():
v = sensor_reading.acceleration.v v = sensor_reading.acceleration.v
self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]]) 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): def reset_kalman(self):
self.filter_time = None self.filter_time = None
self.filter_ready = False self.filter_ready = False
@ -160,9 +230,9 @@ class Localizer():
def locationd_thread(sm, pm, disabled_logs=[]): def locationd_thread(sm, pm, disabled_logs=[]):
if sm is None: if sm is None:
sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry']) sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration'])
if pm is None: if pm is None:
pm = messaging.PubMaster(['liveLocation']) pm = messaging.PubMaster(['liveLocationKalman'])
localizer = Localizer(disabled_logs=disabled_logs) localizer = Localizer(disabled_logs=disabled_logs)
@ -180,16 +250,17 @@ def locationd_thread(sm, pm, disabled_logs=[]):
localizer.handle_car_state(t, sm[sock]) localizer.handle_car_state(t, sm[sock])
elif sock == "cameraOdometry": elif sock == "cameraOdometry":
localizer.handle_cam_odo(t, sm[sock]) 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']: if localizer.filter_ready and sm.updated['gpsLocationExternal']:
t = sm.logMonoTime['gpsLocationExternal'] t = sm.logMonoTime['gpsLocationExternal']
msg = messaging.new_message() msg = messaging.new_message()
msg.logMonoTime = t msg.logMonoTime = t
msg.init('liveLocation') msg.init('liveLocationKalman')
msg.liveLocation = localizer.liveLocationMsg(t * 1e-9) msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9)
pm.send('liveLocationKalman', msg)
pm.send('liveLocation', msg)
def main(sm=None, pm=None): def main(sm=None, pm=None):

Write Preview
Loading…
Cancel
Save