Remove sympy import from paramsd at runtime (#20710)

pull/20713/head
Willem Melching 4 years ago committed by GitHub
parent aae849ea45
commit b27bad8776
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 22
      selfdrive/locationd/models/car_kf.py

@ -4,7 +4,6 @@ import sys
from typing import Any, Dict
import numpy as np
import sympy as sp
from selfdrive.locationd.models.constants import ObservationKind
from selfdrive.swaglog import cloudlog
@ -12,6 +11,7 @@ from selfdrive.swaglog import cloudlog
from rednose.helpers.kalmanfilter import KalmanFilter
if __name__ == '__main__': # Generating sympy
import sympy as sp
from rednose.helpers.ekf_sym import gen_code
else:
from rednose.helpers.ekf_sym_pyx import EKF_sym # pylint: disable=no-name-in-module, import-error
@ -75,12 +75,12 @@ class CarKalman(KalmanFilter):
}
global_vars = [
sp.Symbol('mass'),
sp.Symbol('rotational_inertia'),
sp.Symbol('center_to_front'),
sp.Symbol('center_to_rear'),
sp.Symbol('stiffness_front'),
sp.Symbol('stiffness_rear'),
'mass',
'rotational_inertia',
'center_to_front',
'center_to_rear',
'stiffness_front',
'stiffness_rear',
]
@staticmethod
@ -89,7 +89,8 @@ class CarKalman(KalmanFilter):
name = CarKalman.name
# globals
m, j, aF, aR, cF_orig, cR_orig = CarKalman.global_vars
global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
m, j, aF, aR, cF_orig, cR_orig = global_vars
# make functions and jacobians with sympy
# state variables
@ -143,7 +144,7 @@ class CarKalman(KalmanFilter):
[sp.Matrix([x]), ObservationKind.STIFFNESS, None],
]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=CarKalman.global_vars)
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0): # pylint: disable=super-init-not-called
dim_state = self.initial_x.shape[0]
@ -154,8 +155,7 @@ class CarKalman(KalmanFilter):
x_init[States.ANGLE_OFFSET] = angle_offset
# init filter
global_var_names = [x.name for x in self.global_vars] # pylint: disable=no-member
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=global_var_names, logger=cloudlog)
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
if __name__ == "__main__":

Loading…
Cancel
Save