#!/usr/bin/env python
import os
import numpy as np
from selfdrive . can . parser import CANParser
from cereal import car
from common . realtime import sec_since_boot
import zmq
from selfdrive . services import service_list
import selfdrive . messaging as messaging
RADAR_MSGS = range ( 0x500 , 0x540 )
def _create_radard_can_parser ( ) :
dbc_f = ' ford_fusion_2018_adas.dbc '
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)
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 ( os . path . splitext ( dbc_f ) [ 0 ] , signals , checks , 1 )
class RadarInterface ( object ) :
def __init__ ( self , CP ) :
# radar
self . pts = { }
self . validCnt = { key : 0 for key in RADAR_MSGS }
self . track_id = 0
self . delay = 0.0 # Delay of radar
# Nidec
self . rcp = _create_radard_can_parser ( )
context = zmq . Context ( )
self . logcan = messaging . sub_sock ( context , service_list [ ' can ' ] . port )
def update ( self ) :
canMonoTimes = [ ]
updated_messages = set ( )
while 1 :
tm = int ( sec_since_boot ( ) * 1e9 )
updated_messages . update ( self . rcp . update ( tm , True ) )
# TODO: do not hardcode last msg
if 0x53f in updated_messages :
break
ret = car . RadarState . new_message ( )
errors = [ ]
if not self . rcp . can_valid :
errors . append ( " commIssue " )
ret . errors = errors
ret . canMonoTimes = canMonoTimes
for ii in 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 . RadarState . 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 ' ] * np . pi / 180. # 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 = self . pts . values ( )
return ret
if __name__ == " __main__ " :
RI = RadarInterface ( None )
while 1 :
ret = RI . update ( )
print ( chr ( 27 ) + " [2J " )
print ( ret )
