#!/usr/bin/env python
import os
import copy
import json
import numpy as np
import selfdrive . messaging as messaging
from selfdrive . locationd . calibration_helpers import Calibration
from selfdrive . swaglog import cloudlog
from common . params import Params
from common . transformations . model import model_height
from common . transformations . camera import view_frame_from_device_frame , get_view_frame_from_road_frame , \
eon_intrinsics , get_calib_from_vp , H , W
MPH_TO_MS = 0.44704
MIN_SPEED_FILTER = 15 * MPH_TO_MS
MAX_YAW_RATE_FILTER = np . radians ( 2 ) # per second
INPUTS_NEEDED = 300 # allow to update VP every so many frames
INPUTS_WANTED = 600 # We want a little bit more than we need for stability
WRITE_CYCLES = 400 # write every 400 cycles
VP_INIT = np . array ( [ W / 2. , H / 2. ] )
# These validity corners were chosen by looking at 1000
# and taking most extreme cases with some margin.
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
7 years ago
VP_VALIDITY_CORNERS = np . array ( [ [ W / / 2 - 150 , 280 ] , [ W / / 2 + 150 , 540 ] ] )
DEBUG = os . getenv ( " DEBUG " ) is not None
def is_calibration_valid ( vp ) :
return vp [ 0 ] > VP_VALIDITY_CORNERS [ 0 , 0 ] and vp [ 0 ] < VP_VALIDITY_CORNERS [ 1 , 0 ] and \
vp [ 1 ] > VP_VALIDITY_CORNERS [ 0 , 1 ] and vp [ 1 ] < VP_VALIDITY_CORNERS [ 1 , 1 ]
class Calibrator ( object ) :
def __init__ ( self , param_put = False ) :
self . param_put = param_put
self . vp = copy . copy ( VP_INIT )
self . vps = [ ]
self . cal_status = Calibration . UNCALIBRATED
self . write_counter = 0
self . just_calibrated = False
self . params = Params ( )
calibration_params = self . params . get ( " CalibrationParams " )
if calibration_params :
try :
calibration_params = json . loads ( calibration_params )
self . vp = np . array ( calibration_params [ " vanishing_point " ] )
self . vps = np . tile ( self . vp , ( calibration_params [ ' valid_points ' ] , 1 ) ) . tolist ( )
self . update_status ( )
except Exception :
cloudlog . exception ( " CalibrationParams file found but error encountered " )
def update_status ( self ) :
start_status = self . cal_status
if len ( self . vps ) < INPUTS_NEEDED :
self . cal_status = Calibration . UNCALIBRATED
else :
self . cal_status = Calibration . CALIBRATED if is_calibration_valid ( self . vp ) else Calibration . INVALID
end_status = self . cal_status
self . just_calibrated = False
if start_status == Calibration . UNCALIBRATED and end_status == Calibration . CALIBRATED :
self . just_calibrated = True
def handle_cam_odom ( self , log ) :
trans , rot = log . trans , log . rot
if np . linalg . norm ( trans ) > MIN_SPEED_FILTER and abs ( rot [ 2 ] ) < MAX_YAW_RATE_FILTER :
new_vp = eon_intrinsics . dot ( view_frame_from_device_frame . dot ( trans ) )
new_vp = new_vp [ : 2 ] / new_vp [ 2 ]
self . vps . append ( new_vp )
self . vps = self . vps [ - INPUTS_WANTED : ]
self . vp = np . mean ( self . vps , axis = 0 )
self . update_status ( )
self . write_counter + = 1
if self . param_put and ( self . write_counter % WRITE_CYCLES == 0 or self . just_calibrated ) :
cal_params = { " vanishing_point " : list ( self . vp ) ,
" valid_points " : len ( self . vps ) }
self . params . put ( " CalibrationParams " , json . dumps ( cal_params ) )
return new_vp
else :
return None
def send_data ( self , pm ) :
calib = get_calib_from_vp ( self . vp )
extrinsic_matrix = get_view_frame_from_road_frame ( 0 , calib [ 1 ] , calib [ 2 ] , model_height )
cal_send = messaging . new_message ( )
cal_send . init ( ' liveCalibration ' )
cal_send . liveCalibration . calStatus = self . cal_status
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
7 years ago
cal_send . liveCalibration . calPerc = min ( len ( self . vps ) * 100 / / INPUTS_NEEDED , 100 )
cal_send . liveCalibration . extrinsicMatrix = [ float ( x ) for x in extrinsic_matrix . flatten ( ) ]
cal_send . liveCalibration . rpyCalib = [ float ( x ) for x in calib ]
pm . send ( ' liveCalibration ' , cal_send )
def calibrationd_thread ( sm = None , pm = None ) :
if sm is None :
sm = messaging . SubMaster ( [ ' cameraOdometry ' ] )
if pm is None :
pm = messaging . PubMaster ( [ ' liveCalibration ' ] )
calibrator = Calibrator ( param_put = True )
# buffer with all the messages that still need to be input into the kalman
while 1 :
sm . update ( )
new_vp = calibrator . handle_cam_odom ( sm [ ' cameraOdometry ' ] )
if DEBUG and new_vp is not None :
print ' got new vp ' , new_vp
calibrator . send_data ( pm )
def main ( sm = None , pm = None ) :
calibrationd_thread ( sm , pm )
if __name__ == " __main__ " :
main ( )
