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

remove old locationd stuff

Browse Source 
old-commit-hash: 64a2d0c3e9
commatwo_master
Willem Melching 5 years ago
parent 2d69a5a335
commit b26d45ce04
2 changed files with 0 additions and 290 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. 206
      selfdrive/locationd/locationd_local.py
  2. 84
      selfdrive/locationd/params_learner.py

206
selfdrive/locationd/locationd_local.py
Unescape View File

@ -1,206 +0,0 @@
#!/usr/bin/env python3
import os
import zmq
import math
import json
import numpy as np
from bisect import bisect_right
from cereal import car
from common.params import Params
from common.numpy_fast import clip
import cereal.messaging as messaging
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.vehicle_model import VehicleModel
from cereal.services import service_list
from selfdrive.locationd.kalman.loc_local_kf import LocLocalKalman
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind
from selfdrive.locationd.params_learner import ParamsLearner
DEBUG = False
kf = LocLocalKalman() # Make sure that model is generated on import time
LEARNING_RATE = 3
class Localizer():
def __init__(self, disabled_logs=None, dog=None):
self.kf = LocLocalKalman()
self.reset_kalman()
self.sensor_data_t = 0.0
self.max_age = .2 # seconds
self.calibration_valid = False
if disabled_logs is None:
self.disabled_logs = list()
else:
self.disabled_logs = disabled_logs
def reset_kalman(self):
self.filter_time = None
self.observation_buffer = []
self.converter = None
self.speed_counter = 0
self.sensor_counter = 0
def liveLocationMsg(self, time):
fix = messaging.log.KalmanOdometry.new_message()
predicted_state = self.kf.x
fix.trans = [float(predicted_state[0]), float(predicted_state[1]), float(predicted_state[2])]
fix.rot = [float(predicted_state[3]), float(predicted_state[4]), float(predicted_state[5])]
return fix
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:
self.kf.predict_and_observe(*self.observation_buffer.pop(0))
def handle_cam_odo(self, log, current_time):
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, np.concatenate([log.cameraOdometry.rot,
log.cameraOdometry.rotStd]))
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans,
log.cameraOdometry.transStd]))
def handle_controls_state(self, log, current_time):
self.speed_counter += 1
if self.speed_counter % 5 == 0:
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.controlsState.vEgo]))
def handle_sensors(self, log, current_time):
for sensor_reading in log.sensorEvents:
# TODO does not yet account for double sensor readings in the log
if sensor_reading.type == 4:
self.sensor_counter += 1
if self.sensor_counter % LEARNING_RATE == 0:
self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-sensor_reading.gyro.v[2], -sensor_reading.gyro.v[1], -sensor_reading.gyro.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.sensor_data_t = current_time
self.handle_sensors(log, current_time)
elif typ == "controlsState":
self.handle_controls_state(log, current_time)
elif typ == "cameraOdometry":
self.handle_cam_odo(log, current_time)
def locationd_thread(gctx, addr, disabled_logs):
poller = zmq.Poller()
controls_state_socket = messaging.sub_sock('controlsState', poller, addr=addr, conflate=True)
sensor_events_socket = messaging.sub_sock('sensorEvents', poller, addr=addr, conflate=True)
camera_odometry_socket = messaging.sub_sock('cameraOdometry', poller, addr=addr, conflate=True)
kalman_odometry_socket = messaging.pub_sock('kalmanOdometry')
live_parameters_socket = messaging.pub_sock('liveParameters')
params_reader = Params()
cloudlog.info("Parameter learner is waiting for CarParams")
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
VM = VehicleModel(CP)
cloudlog.info("Parameter learner got CarParams: %s" % CP.carFingerprint)
params = params_reader.get("LiveParameters")
# Check if car model matches
if params is not None:
params = json.loads(params)
if (params.get('carFingerprint', None) != CP.carFingerprint) or (params.get('carVin', CP.carVin) != CP.carVin):
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'carVin': CP.carVin,
'angleOffsetAverage': 0.0,
'stiffnessFactor': 1.0,
'steerRatio': VM.sR,
}
params_reader.put("LiveParameters", json.dumps(params))
cloudlog.info("Parameter learner resetting to default values")
cloudlog.info("Parameter starting with: %s" % str(params))
localizer = Localizer(disabled_logs=disabled_logs)
learner = ParamsLearner(VM,
angle_offset=params['angleOffsetAverage'],
stiffness_factor=params['stiffnessFactor'],
steer_ratio=params['steerRatio'],
learning_rate=LEARNING_RATE)
i = 1
while True:
for socket, event in poller.poll(timeout=1000):
log = messaging.recv_one(socket)
localizer.handle_log(log)
if socket is controls_state_socket:
if not localizer.kf.t:
continue
if i % LEARNING_RATE == 0:
# controlsState is not updating the Kalman Filter, so update KF manually
localizer.kf.predict(1e-9 * log.logMonoTime)
predicted_state = localizer.kf.x
yaw_rate = -float(predicted_state[5])
steering_angle = math.radians(log.controlsState.angleSteers)
params_valid = learner.update(yaw_rate, log.controlsState.vEgo, steering_angle)
log_t = 1e-9 * log.logMonoTime
sensor_data_age = log_t - localizer.sensor_data_t
params = messaging.new_message()
params.init('liveParameters')
params.liveParameters.valid = bool(params_valid)
params.liveParameters.sensorValid = bool(sensor_data_age < 5.0)
params.liveParameters.angleOffset = float(math.degrees(learner.ao))
params.liveParameters.angleOffsetAverage = float(math.degrees(learner.slow_ao))
params.liveParameters.stiffnessFactor = float(learner.x)
params.liveParameters.steerRatio = float(learner.sR)
live_parameters_socket.send(params.to_bytes())
if i % 6000 == 0: # once a minute
params = learner.get_values()
params['carFingerprint'] = CP.carFingerprint
params['carVin'] = CP.carVin
params_reader.put("LiveParameters", json.dumps(params))
params_reader.put("ControlsParams", json.dumps({'angle_model_bias': log.controlsState.angleModelBias}))
i += 1
elif socket is camera_odometry_socket:
msg = messaging.new_message()
msg.init('kalmanOdometry')
msg.logMonoTime = log.logMonoTime
msg.kalmanOdometry = localizer.liveLocationMsg(log.logMonoTime * 1e-9)
kalman_odometry_socket.send(msg.to_bytes())
elif socket is sensor_events_socket:
pass
def main(gctx=None, addr="127.0.0.1"):
IN_CAR = os.getenv("IN_CAR", False)
disabled_logs = os.getenv("DISABLED_LOGS", "").split(",")
# No speed for now
disabled_logs.append('controlsState')
if IN_CAR:
addr = "192.168.5.11"
locationd_thread(gctx, addr, disabled_logs)
if __name__ == "__main__":
main()

84
selfdrive/locationd/params_learner.py
Unescape View File

@ -1,84 +0,0 @@
import math
from common.numpy_fast import clip
MAX_ANGLE_OFFSET = math.radians(10.)
MAX_ANGLE_OFFSET_TH = math.radians(9.)
MIN_STIFFNESS = 0.5
MAX_STIFFNESS = 2.0
MIN_SR = 0.5
MAX_SR = 2.0
MIN_SR_TH = 0.55
MAX_SR_TH = 1.9
DEBUG = False
class ParamsLearner():
def __init__(self, VM, angle_offset=0., stiffness_factor=1.0, steer_ratio=None, learning_rate=1.0):
self.VM = VM
self.ao = math.radians(angle_offset)
self.slow_ao = math.radians(angle_offset)
self.x = stiffness_factor
self.sR = VM.sR if steer_ratio is None else steer_ratio
self.MIN_SR = MIN_SR * self.VM.sR
self.MAX_SR = MAX_SR * self.VM.sR
self.MIN_SR_TH = MIN_SR_TH * self.VM.sR
self.MAX_SR_TH = MAX_SR_TH * self.VM.sR
self.alpha1 = 0.01 * learning_rate
self.alpha2 = 0.0005 * learning_rate
self.alpha3 = 0.1 * learning_rate
self.alpha4 = 1.0 * learning_rate
def get_values(self):
return {
'angleOffsetAverage': math.degrees(self.slow_ao),
'stiffnessFactor': self.x,
'steerRatio': self.sR,
}
def update(self, psi, u, sa):
cF0 = self.VM.cF
cR0 = self.VM.cR
aR = self.VM.aR
aF = self.VM.aF
l = self.VM.l
m = self.VM.m
x = self.x
ao = self.ao
sR = self.sR
# Gradient descent: learn angle offset, tire stiffness and steer ratio.
if u > 10.0 and abs(math.degrees(sa)) < 15.:
self.ao -= self.alpha1 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2)
ao = self.slow_ao
self.slow_ao -= self.alpha2 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2)
self.x -= self.alpha3 * -2.0*cF0*cR0*l*m*u**3*(ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**3)
self.sR -= self.alpha4 * -2.0*cF0*cR0*l*u*x*(ao - sa)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**3*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2)
if DEBUG:
# s1 = "Measured yaw rate % .6f" % psi
# ao = 0.
# s2 = "Uncompensated yaw % .6f" % (1.0*u*(-ao + sa)/(l*sR*(1 - m*u**2*(aF*cF0*x - aR*cR0*x)/(cF0*cR0*l**2*x**2))))
# instant_ao = aF*m*psi*sR*u/(cR0*l*x) - aR*m*psi*sR*u/(cF0*l*x) - l*psi*sR/u + sa
s4 = "Instant AO: % .6f Avg. AO % .6f" % (math.degrees(self.ao), math.degrees(self.slow_ao))
s5 = "Stiffnes: % .6f x" % self.x
s6 = "sR: %.4f" % self.sR
print("{0} {1} {2}".format(s4, s5, s6))
self.ao = clip(self.ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET)
self.slow_ao = clip(self.slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET)
self.x = clip(self.x, MIN_STIFFNESS, MAX_STIFFNESS)
self.sR = clip(self.sR, self.MIN_SR, self.MAX_SR)
# don't check stiffness for validity, as it can change quickly if sR is off
valid = abs(self.slow_ao) < MAX_ANGLE_OFFSET_TH and \
self.sR > self.MIN_SR_TH and self.sR < self.MAX_SR_TH
return valid
Write Preview
Loading…
Cancel
Save