#!/usr/bin/env python3
from cereal import car
from selfdrive . can . parser import CANParser
from selfdrive . car . ford . values import DBC
from selfdrive . config import Conversions as CV
from selfdrive . car . interfaces import RadarInterfaceBase
RADAR_MSGS = list ( range ( 0x500 , 0x540 ) )
def _create_radar_can_parser ( car_fingerprint ) :
dbc_f = DBC [ car_fingerprint ] [ ' radar ' ]
msg_n = len ( RADAR_MSGS )
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
signals = list ( zip ( [ ' X_Rel ' ] * msg_n + [ ' Angle ' ] * msg_n + [ ' V_Rel ' ] * msg_n ,
RADAR_MSGS * 3 ,
[ 0 ] * msg_n + [ 0 ] * msg_n + [ 0 ] * msg_n ) )
checks = list ( zip ( RADAR_MSGS , [ 20 ] * msg_n ) )
return CANParser ( dbc_f , signals , checks , 1 )
class RadarInterface ( RadarInterfaceBase ) :
def __init__ ( self , CP ) :
# radar
self . pts = { }
self . validCnt = { key : 0 for key in RADAR_MSGS }
self . track_id = 0
self . delay = 0 # Delay of radar
self . rcp = _create_radar_can_parser ( CP . carFingerprint )
self . trigger_msg = 0x53f
self . updated_messages = set ( )
def update ( self , can_strings ) :
vls = self . rcp . update_strings ( can_strings )
self . updated_messages . update ( vls )
if self . trigger_msg not in self . updated_messages :
return None
ret = car . RadarData . new_message ( )
errors = [ ]
if not self . rcp . can_valid :
errors . append ( " canError " )
ret . errors = errors
for ii in sorted ( self . updated_messages ) :
cpt = self . rcp . vl [ ii ]
if cpt [ ' X_Rel ' ] > 0.00001 :
self . validCnt [ ii ] = 0 # reset counter
if cpt [ ' X_Rel ' ] > 0.00001 :
self . validCnt [ ii ] + = 1
else :
self . validCnt [ ii ] = max ( self . validCnt [ ii ] - 1 , 0 )
#print ii, self.validCnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle']
# radar point only valid if there have been enough valid measurements
if self . validCnt [ ii ] > 0 :
if ii not in self . pts :
self . pts [ ii ] = car . RadarData . RadarPoint . new_message ( )
self . pts [ ii ] . trackId = self . track_id
self . track_id + = 1
self . pts [ ii ] . dRel = cpt [ ' X_Rel ' ] # from front of car
self . pts [ ii ] . yRel = cpt [ ' X_Rel ' ] * cpt [ ' Angle ' ] * CV . DEG_TO_RAD # in car frame's y axis, left is positive
self . pts [ ii ] . vRel = cpt [ ' V_Rel ' ]
self . pts [ ii ] . aRel = float ( ' nan ' )
self . pts [ ii ] . yvRel = float ( ' nan ' )
self . pts [ ii ] . measured = True
else :
if ii in self . pts :
del self . pts [ ii ]
ret . points = list ( self . pts . values ( ) )
self . updated_messages . clear ( )
return ret
