import numpy as np
import sympy as sp
import os
getting ready for Python 3 (#619)
* tabs to spaces
python 2 to 3: https://portingguide.readthedocs.io/en/latest/syntax.html#tabs-and-spaces
* use the new except syntax
python 2 to 3: https://portingguide.readthedocs.io/en/latest/exceptions.html#the-new-except-syntax
* make relative imports absolute
python 2 to 3: https://portingguide.readthedocs.io/en/latest/imports.html#absolute-imports
* Queue renamed to queue in python 3
Use the six compatibility library to support both python 2 and 3: https://portingguide.readthedocs.io/en/latest/stdlib-reorg.html#renamed-modules
* replace dict.has_key() with in
python 2 to 3: https://portingguide.readthedocs.io/en/latest/dicts.html#removed-dict-has-key
* make dict views compatible with python 3
python 2 to 3: https://portingguide.readthedocs.io/en/latest/dicts.html#dict-views-and-iterators
Where needed, wrapping things that will be a view in python 3 with a list(). For example, if it's accessed with []
Python 3 has no iter*() methods, so just using the values() instead of itervalues() as long as it's not too performance intensive. Note that any minor performance hit of using a list instead of a view will go away when switching to python 3. If it is intensive, we could use the six version.
* Explicitly use truncating division
python 2 to 3: https://portingguide.readthedocs.io/en/latest/numbers.html#division
python 3 treats / as float division. When we want the result to be an integer, use //
* replace map() with list comprehension where a list result is needed.
In python 3, map() returns an iterator.
python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-map-and-filter
* replace filter() with list comprehension
In python 3, filter() returns an interatoooooooooooor.
python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-map-and-filter
* wrap zip() in list() where we need the result to be a list
python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-zip
* clean out some lint
Removes these pylint warnings:
************* Module selfdrive.car.chrysler.chryslercan
W: 15, 0: Unnecessary semicolon (unnecessary-semicolon)
W: 16, 0: Unnecessary semicolon (unnecessary-semicolon)
W: 25, 0: Unnecessary semicolon (unnecessary-semicolon)
************* Module common.dbc
W:101, 0: Anomalous backslash in string: '\?'. String constant might be missing an r prefix. (anomalous-backslash-in-string)
************* Module selfdrive.car.gm.interface
R:102, 6: Redefinition of ret.minEnableSpeed type from float to int (redefined-variable-type)
R:103, 6: Redefinition of ret.mass type from int to float (redefined-variable-type)
************* Module selfdrive.updated
R: 20, 6: Redefinition of r type from int to str (redefined-variable-type)
6 years ago
from selfdrive . locationd . kalman . kalman_helpers import ObservationKind
from selfdrive . locationd . kalman . ekf_sym import gen_code
def gen_model ( name , dim_state ) :
# check if rebuild is needed
try :
dir_path = os . path . dirname ( __file__ )
deps = [ dir_path + ' / ' + ' ekf_c.c ' ,
dir_path + ' / ' + ' ekf_sym.py ' ,
dir_path + ' / ' + ' loc_local_model.py ' ,
dir_path + ' / ' + ' loc_local_kf.py ' ]
outs = [ dir_path + ' / ' + name + ' .o ' ,
dir_path + ' / ' + name + ' .so ' ,
dir_path + ' / ' + name + ' .cpp ' ]
out_times = map ( os . path . getmtime , outs )
dep_times = map ( os . path . getmtime , deps )
rebuild = os . getenv ( " REBUILD " , False )
if min ( out_times ) > max ( dep_times ) and not rebuild :
return
map ( os . remove , outs )
except OSError :
pass
# make functions and jacobians with sympy
# state variables
state_sym = sp . MatrixSymbol ( ' state ' , dim_state , 1 )
state = sp . Matrix ( state_sym )
v = state [ 0 : 3 , : ]
omega = state [ 3 : 6 , : ]
vroll , vpitch , vyaw = omega
vx , vy , vz = v
roll_bias , pitch_bias , yaw_bias = state [ 6 : 9 , : ]
odo_scale = state [ 9 , : ]
accel = state [ 10 : 13 , : ]
dt = sp . Symbol ( ' dt ' )
# Time derivative of the state as a function of state
state_dot = sp . Matrix ( np . zeros ( ( dim_state , 1 ) ) )
state_dot [ : 3 , : ] = accel
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = sp . Matrix ( state + dt * state_dot )
#
# Observation functions
#
# extra args
#imu_rot = euler_rotate(*imu_angles)
#h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias,
# vpitch + pitch_bias,
# vyaw + yaw_bias])
h_gyro_sym = sp . Matrix ( [ vroll + roll_bias ,
vpitch + pitch_bias ,
vyaw + yaw_bias ] )
speed = vx * * 2 + vy * * 2 + vz * * 2
h_speed_sym = sp . Matrix ( [ sp . sqrt ( speed ) * odo_scale ] )
h_relative_motion = sp . Matrix ( v )
h_phone_rot_sym = sp . Matrix ( [ vroll ,
vpitch ,
vyaw ] )
obs_eqs = [ [ h_speed_sym , ObservationKind . ODOMETRIC_SPEED , None ] ,
[ h_gyro_sym , ObservationKind . PHONE_GYRO , None ] ,
[ h_phone_rot_sym , ObservationKind . NO_ROT , None ] ,
[ h_relative_motion , ObservationKind . CAMERA_ODO_TRANSLATION , None ] ,
[ h_phone_rot_sym , ObservationKind . CAMERA_ODO_ROTATION , None ] ]
gen_code ( name , f_sym , dt , state_sym , obs_eqs , dim_state , dim_state )
