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

old-commit-hash: 24eeec4e0b
commatwo_master
HaraldSchafer 6 years ago committed by GitHub
parent 0a47a0cb3e
commit b038c1f93b
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
Split 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 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.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.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])]
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'
#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]))
imu_frame = predicted_state[States.IMU_OFFSET]
fix.imuFrame = [math.degrees(imu_frame[0]), math.degrees(imu_frame[1]), math.degrees(imu_frame[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 = 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.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
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):

Write Preview
Loading…
Cancel
Save