import zmq
import math
import numpy as np
from common . realtime import sec_since_boot
from selfdrive . services import service_list
from selfdrive . swaglog import cloudlog
from selfdrive . controls . lib . lateral_mpc import libmpc_py
from selfdrive . controls . lib . drive_helpers import MPC_COST_LAT
from selfdrive . controls . lib . model_parser import ModelParser
import selfdrive . messaging as messaging
def calc_states_after_delay ( states , v_ego , steer_angle , curvature_factor , steer_ratio , delay ) :
states [ 0 ] . x = v_ego * delay
states [ 0 ] . psi = v_ego * curvature_factor * math . radians ( steer_angle ) / steer_ratio * delay
return states
class PathPlanner ( object ) :
def __init__ ( self , CP ) :
self . MP = ModelParser ( )
self . l_poly = [ 0. , 0. , 0. , 0. ]
self . r_poly = [ 0. , 0. , 0. , 0. ]
self . last_cloudlog_t = 0
context = zmq . Context ( )
self . plan = messaging . pub_sock ( context , service_list [ ' pathPlan ' ] . port )
self . livempc = messaging . pub_sock ( context , service_list [ ' liveMpc ' ] . port )
self . setup_mpc ( CP . steerRateCost )
self . invalid_counter = 0
def setup_mpc ( self , steer_rate_cost ) :
self . libmpc = libmpc_py . libmpc
self . libmpc . init ( MPC_COST_LAT . PATH , MPC_COST_LAT . LANE , MPC_COST_LAT . HEADING , steer_rate_cost )
self . mpc_solution = libmpc_py . ffi . new ( " log_t * " )
self . cur_state = libmpc_py . ffi . new ( " state_t * " )
self . cur_state [ 0 ] . x = 0.0
self . cur_state [ 0 ] . y = 0.0
self . cur_state [ 0 ] . psi = 0.0
self . cur_state [ 0 ] . delta = 0.0
self . angle_steers_des = 0.0
self . angle_steers_des_mpc = 0.0
self . angle_steers_des_prev = 0.0
self . angle_steers_des_time = 0.0
def update ( self , CP , VM , CS , md , live100 , live_parameters ) :
v_ego = CS . carState . vEgo
angle_steers = CS . carState . steeringAngle
active = live100 . live100 . active
angle_offset_average = live_parameters . liveParameters . angleOffsetAverage
angle_offset_bias = live100 . live100 . angleModelBias + angle_offset_average
self . MP . update ( v_ego , md )
# Run MPC
self . angle_steers_des_prev = self . angle_steers_des_mpc
VM . update_params ( live_parameters . liveParameters . stiffnessFactor , live_parameters . liveParameters . steerRatio )
curvature_factor = VM . curvature_factor ( v_ego )
l_poly = libmpc_py . ffi . new ( " double[4] " , list ( self . MP . l_poly ) )
r_poly = libmpc_py . ffi . new ( " double[4] " , list ( self . MP . r_poly ) )
p_poly = libmpc_py . ffi . new ( " double[4] " , list ( self . MP . p_poly ) )
# account for actuation delay
self . cur_state = calc_states_after_delay ( self . cur_state , v_ego , angle_steers - angle_offset_average , curvature_factor , VM . sR , CP . steerActuatorDelay )
v_ego_mpc = max ( v_ego , 5.0 ) # avoid mpc roughness due to low speed
self . libmpc . run_mpc ( self . cur_state , self . mpc_solution ,
l_poly , r_poly , p_poly ,
self . MP . l_prob , self . MP . r_prob , self . MP . p_prob , curvature_factor , v_ego_mpc , self . MP . lane_width )
# reset to current steer angle if not active or overriding
if active :
delta_desired = self . mpc_solution [ 0 ] . delta [ 1 ]
rate_desired = math . degrees ( self . mpc_solution [ 0 ] . rate [ 0 ] * VM . sR )
else :
delta_desired = math . radians ( angle_steers - angle_offset_bias ) / VM . sR
rate_desired = 0.0
self . cur_state [ 0 ] . delta = delta_desired
self . angle_steers_des_mpc = float ( math . degrees ( delta_desired * VM . sR ) + angle_offset_bias )
# Check for infeasable MPC solution
mpc_nans = np . any ( np . isnan ( list ( self . mpc_solution [ 0 ] . delta ) ) )
t = sec_since_boot ( )
if mpc_nans :
self . libmpc . init ( MPC_COST_LAT . PATH , MPC_COST_LAT . LANE , MPC_COST_LAT . HEADING , CP . steerRateCost )
self . cur_state [ 0 ] . delta = math . radians ( angle_steers ) / VM . sR
if t > self . last_cloudlog_t + 5.0 :
self . last_cloudlog_t = t
cloudlog . warning ( " Lateral mpc - nan: True " )
if self . mpc_solution [ 0 ] . cost > 20000. or mpc_nans : # TODO: find a better way to detect when MPC did not converge
self . invalid_counter + = 1
else :
self . invalid_counter = 0
plan_valid = self . invalid_counter < 2
plan_send = messaging . new_message ( )
plan_send . init ( ' pathPlan ' )
plan_send . pathPlan . laneWidth = float ( self . MP . lane_width )
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
plan_send . pathPlan . dPoly = [ float ( x ) for x in self . MP . d_poly ]
plan_send . pathPlan . cPoly = [ float ( x ) for x in self . MP . c_poly ]
plan_send . pathPlan . cProb = float ( self . MP . c_prob )
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
plan_send . pathPlan . lPoly = [ float ( x ) for x in l_poly ]
plan_send . pathPlan . lProb = float ( self . MP . l_prob )
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
plan_send . pathPlan . rPoly = [ float ( x ) for x in r_poly ]
plan_send . pathPlan . rProb = float ( self . MP . r_prob )
plan_send . pathPlan . angleSteers = float ( self . angle_steers_des_mpc )
plan_send . pathPlan . rateSteers = float ( rate_desired )
plan_send . pathPlan . angleOffset = float ( angle_offset_average )
plan_send . pathPlan . valid = bool ( plan_valid )
plan_send . pathPlan . paramsValid = bool ( live_parameters . liveParameters . valid )
self . plan . send ( plan_send . to_bytes ( ) )
dat = messaging . new_message ( )
dat . init ( ' liveMpc ' )
dat . liveMpc . x = list ( self . mpc_solution [ 0 ] . x )
dat . liveMpc . y = list ( self . mpc_solution [ 0 ] . y )
dat . liveMpc . psi = list ( self . mpc_solution [ 0 ] . psi )
dat . liveMpc . delta = list ( self . mpc_solution [ 0 ] . delta )
dat . liveMpc . cost = self . mpc_solution [ 0 ] . cost
self . livempc . send ( dat . to_bytes ( ) )
