def plannerd_thread(sm=None, pm=None): config_realtime_process(5 if TICI else 2, 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) use_lanelines = not params.get_bool('EndToEndToggle') wide_camera = params.get_bool('EnableWideCamera') if TICI else False longitudinal_planner = Planner(CP) lateral_planner = LateralPlanner(CP, use_lanelines=use_lanelines, wide_camera=wide_camera) if sm is None: sm = messaging.SubMaster( ['carState', 'controlsState', 'radarState', 'modelV2'], poll=['radarState', 'modelV2']) if pm is None: pm = messaging.PubMaster([ 'longitudinalPlan', 'liveLongitudinalMpc', 'lateralPlan', 'liveMpc' ]) while True: sm.update() if sm.updated['modelV2']: lateral_planner.update(sm, CP) lateral_planner.publish(sm, pm) if sm.updated['radarState']: longitudinal_planner.update(sm, CP) longitudinal_planner.publish(sm, pm)
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) use_lanelines = False wide_camera = params.get_bool('WideCameraOnly') cloudlog.event("e2e mode", on=use_lanelines) longitudinal_planner = Planner(CP) lateral_planner = LateralPlanner(use_lanelines=use_lanelines, wide_camera=wide_camera) if sm is None: sm = messaging.SubMaster( ['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']: lateral_planner.update(sm) lateral_planner.publish(sm, pm) longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm)
def plannerd_thread(sm=None, pm=None): config_realtime_process(2, Priority.CTRL_LOW) cloudlog.info("plannerd is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) PL = Planner(CP) PP = LateralPlanner(CP) VM = VehicleModel(CP) if sm is None: sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2', 'liveParameters'], poll=['radarState', 'modelV2']) if pm is None: pm = messaging.PubMaster(['longitudinalPlan', 'liveLongitudinalMpc', 'lateralPlan', 'liveMpc']) sm['liveParameters'].valid = True sm['liveParameters'].sensorValid = True sm['liveParameters'].steerRatio = CP.steerRatio sm['liveParameters'].stiffnessFactor = 1.0 while True: sm.update() if sm.updated['modelV2']: PP.update(sm, CP, VM) PP.publish(sm, pm) if sm.updated['radarState']: PL.update(sm, CP, VM, PP) PL.publish(sm, pm)