import numpy as np
import math
from common . numpy_fast import interp
_K_CURV_V = [ 1. , 0.6 ]
_K_CURV_BP = [ 0. , 0.002 ]
# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
_LANE_WIDTH_V = [ 3. , 3.8 ]
# break points of speed
_LANE_WIDTH_BP = [ 0. , 31. ]
def calc_d_lookahead ( v_ego , d_poly ) :
# this function computes how far too look for lateral control
# howfar we look ahead is function of speed and how much curvy is the path
offset_lookahead = 1.
k_lookahead = 7.
# integrate abs value of second derivative of poly to get a measure of path curvature
pts_len = 50. # m
if len ( d_poly ) > 0 :
pts = np . polyval ( [ 6 * d_poly [ 0 ] , 2 * d_poly [ 1 ] ] , np . arange ( 0 , pts_len ) )
else :
pts = 0.
curv = np . sum ( np . abs ( pts ) ) / pts_len
k_curv = interp ( curv , _K_CURV_BP , _K_CURV_V )
# sqrt on speed is needed to keep, for a given curvature, the y_des
# proportional to speed. Indeed, y_des is prop to d_lookahead^2
# 36m at 25m/s
d_lookahead = offset_lookahead + math . sqrt ( max ( v_ego , 0 ) ) * k_lookahead * k_curv
return d_lookahead
def calc_lookahead_offset ( v_ego , angle_steers , d_lookahead , VM , angle_offset ) :
# this function returns the lateral offset given the steering angle, speed and the lookahead distance
sa = math . radians ( angle_steers - angle_offset )
curvature = VM . calc_curvature ( sa , v_ego )
# clip is to avoid arcsin NaNs due to too sharp turns
y_actual = d_lookahead * np . tan ( np . arcsin ( np . clip ( d_lookahead * curvature , - 0.999 , 0.999 ) ) / 2. )
return y_actual , curvature
def calc_desired_steer_angle ( v_ego , y_des , d_lookahead , VM , angle_offset ) :
# inverse of the above function
curvature = np . sin ( np . arctan ( y_des / d_lookahead ) * 2. ) / d_lookahead
steer_des = math . degrees ( VM . get_steer_from_curvature ( curvature , v_ego ) ) + angle_offset
return steer_des , curvature
def compute_path_pinv ( l = 50 ) :
deg = 3
x = np . arange ( l * 1.0 )
X = np . vstack ( tuple ( x * * n for n in range ( deg , - 1 , - 1 ) ) ) . T
pinv = np . linalg . pinv ( X )
return pinv
def model_polyfit ( points , path_pinv ) :
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)
old-commit-hash: 9dae0bfac4e54ec2b2e488d2b4ead1495c8f56d8
6 years ago
return np . dot ( path_pinv , [ float ( x ) for x in points ] )
