mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-28 18:12:05 +08:00
remove old locationd stuff
old-commit-hash: 64a2d0c3e9a9c97102341b7f3613da0ce0233f4f
This commit is contained in:
Willem Melching
@@ -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()
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user