#!/usr/bin/env python3
from cereal import car
import numpy as np
from selfdrive . modeld . constants import T_IDXS
from selfdrive . controls . lib . longitudinal_mpc_lib . long_mpc import T_IDXS as T_IDXS_LONG
from common . params import Params
from common . realtime import Priority , config_realtime_process
from system . swaglog import cloudlog
from selfdrive . controls . lib . longitudinal_planner import LongitudinalPlanner
from selfdrive . controls . lib . lateral_planner import LateralPlanner
import cereal . messaging as messaging
def plannerd_thread ( sm = None , pm = None ) :
config_realtime_process ( 5 , Priority . CTRL_LOW )
cloudlog . info ( " plannerd is waiting for CarParams " )
params = Params ( )
CP = car . CarParams . from_bytes ( params . get ( " CarParams " , block = True ) )
cloudlog . info ( " plannerd got CarParams: %s " , CP . carName )
longitudinal_planner = LongitudinalPlanner ( CP )
lateral_planner = LateralPlanner ( CP )
if sm is None :
sm = messaging . SubMaster ( [ ' carControl ' , ' carState ' , ' controlsState ' , ' radarState ' , ' modelV2 ' ] ,
poll = [ ' radarState ' , ' modelV2 ' ] , ignore_avg_freq = [ ' radarState ' ] )
if pm is None :
pm = messaging . PubMaster ( [ ' longitudinalPlan ' , ' lateralPlan ' ] )
while True :
sm . update ( )
if sm . updated [ ' modelV2 ' ] :
longitudinal_planner . update ( sm )
longitudinal_planner . publish ( sm , pm )
lateral_planner . update ( sm , np . interp ( T_IDXS , T_IDXS_LONG , longitudinal_planner . mpc . v_solution ) )
lateral_planner . publish ( sm , pm )
def main ( sm = None , pm = None ) :
plannerd_thread ( sm , pm )
if __name__ == " __main__ " :
main ( )