def radard_thread(sm=None, pm=None, can_sock=None): set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) mocked = CP.carName == "mock" cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module( 'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface if can_sock is None: can_sock = messaging.sub_sock(service_list['can'].port) if sm is None: sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) # *** publish radarState and liveTracks if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) RI = RadarInterface(CP) rk = Ratekeeper(rate, print_delay_threshold=None) RD = RadarD(mocked) has_radar = not CP.radarOffCan while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) rr = RI.update(can_strings) if rr is None: continue sm.update(0) dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar) dat.radarState.cumLagMs = -rk.remaining * 1000. pm.send('radarState', dat) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(tracks.keys()): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } pm.send('liveTracks', dat) rk.monitor_time()
def plannerd_thread(sm=None, pm=None): gc.disable() # start the loop set_realtime_priority(2) 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 = PathPlanner(CP) VM = VehicleModel(CP) if sm is None: sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'liveMapData']) if pm is None: pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', '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['model']: PP.update(sm, pm, CP, VM) if sm.updated['radarState']: PL.update(sm, pm, CP, VM, PP)
def plannerd_thread(): gc.disable() # start the loop set_realtime_priority(2) params = Params() # Get FCW toggle from settings fcw_enabled = params.get("IsFcwEnabled") == "1" 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, fcw_enabled) PP = PathPlanner(CP) VM = VehicleModel(CP) sm = messaging.SubMaster( ['carState', 'controlsState', 'radarState', 'model', 'liveParameters']) 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['model']: PP.update(sm, CP, VM) if sm.updated['radarState']: PL.update(sm, CP, VM, PP)
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 controlsd_thread(gctx=None): gc.disable() # start the loop set_realtime_priority(3) params = Params() # Pub Sockets sendcan = messaging.pub_sock(service_list['sendcan'].port) controlsstate = messaging.pub_sock(service_list['controlsState'].port) carstate = messaging.pub_sock(service_list['carState'].port) carcontrol = messaging.pub_sock(service_list['carControl'].port) carevents = messaging.pub_sock(service_list['carEvents'].port) carparams = messaging.pub_sock(service_list['carParams'].port) is_metric = params.get("IsMetric") == "1" passive = params.get("Passive") != "0" sm = messaging.SubMaster([ 'thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan' ]) logcan = messaging.sub_sock(service_list['can'].port) CI, CP = get_car(logcan, sendcan) logcan.close() # TODO: Use the logcan socket from above, but that will currenly break the tests can_sock = messaging.sub_sock(service_list['can'].port, timeout=100) CC = car.CarControl.new_message() AM = AlertManager() car_recognized = CP.carName != 'mock' # If stock camera is disconnected, we loaded car controls and it's not chffrplus controller_available = CP.enableCamera and CI.CC is not None and not passive read_only = not car_recognized or not controller_available if read_only: CP.safetyModel = car.CarParams.SafetyModel.elm327 # diagnostic only startup_alert = get_startup_alert(car_recognized, controller_available) AM.add(sm.frame, startup_alert, False) LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) if CP.lateralTuning.which() == 'pid': LaC = LatControlPID(CP) else: LaC = LatControlINDI(CP) driver_status = DriverStatus() # Write CarParams for radard and boardd safety mode params.put("CarParams", CP.to_bytes()) params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 v_cruise_kph_last = 0 overtemp = False free_space = False cal_status = Calibration.INVALID cal_perc = 0 mismatch_counter = 0 low_battery = False events_prev = [] sm['pathPlan'].sensorValid = True # controlsd is driven by can recv, expected at 100Hz rk = Ratekeeper(100, print_delay_threshold=None) prof = Profiler(False) # off by default while True: start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\ data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params) prof.checkpoint("Sample") # Create alerts if not sm.all_alive_and_valid(): events.append( create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not sm['pathPlan'].mpcSolutionValid: events.append( create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sm['pathPlan'].sensorValid: events.append( create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT])) if not sm['pathPlan'].paramsValid: events.append(create_event('vehicleModelInvalid', [ET.WARNING])) if not sm['plan'].radarValid: events.append( create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if sm['plan'].radarCanError: events.append( create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not CS.canValid: events.append( create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and sm[ 'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: events.append( create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not read_only: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc) prof.checkpoint("State Control") # Publish data CC, events_prev = data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, carevents, carparams, controlsstate, sendcan, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev) prof.checkpoint("Sent") rk.monitor_time() prof.display()
def controlsd_thread(sm=None, pm=None, can_sock=None): gc.disable() # start the loop set_realtime_priority(3) params = Params() is_metric = params.get("IsMetric") == "1" passive = params.get("Passive") != "0" # Pub/Sub Sockets if pm is None: pm = messaging.PubMaster([ 'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams' ]) if sm is None: sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ 'gpsLocation'], ignore_alive=['gpsLocation']) if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout) # wait for health and CAN packets hw_type = messaging.recv_one(sm.sock['health']).health.hwType is_panda_black = hw_type == log.HealthData.HwType.blackPanda print("Waiting for CAN messages...") get_one_can(can_sock) CI, CP = get_car(can_sock, pm.sock['sendcan'], is_panda_black) car_recognized = CP.carName != 'mock' # If stock camera is disconnected, we loaded car controls and it's not chffrplus controller_available = CP.enableCamera and CI.CC is not None and not passive read_only = not car_recognized or not controller_available or CP.dashcamOnly if read_only: CP.safetyModel = CP.safetyModelPassive # Write CarParams for radard and boardd safety mode params.put("CarParams", CP.to_bytes()) params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") CC = car.CarControl.new_message() AM = AlertManager() startup_alert = get_startup_alert(car_recognized, controller_available) AM.add(sm.frame, startup_alert, False) LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) if CP.lateralTuning.which() == 'pid': LaC = LatControlPID(CP) elif CP.lateralTuning.which() == 'indi': LaC = LatControlINDI(CP) elif CP.lateralTuning.which() == 'lqr': LaC = LatControlLQR(CP) driver_status = DriverStatus() is_rhd = params.get("IsRHD") if is_rhd is not None: driver_status.is_rhd = bool(int(is_rhd)) state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 v_cruise_kph_last = 0 overtemp = False free_space = False cal_status = Calibration.INVALID cal_perc = 0 mismatch_counter = 0 low_battery = False events_prev = [] sm['pathPlan'].sensorValid = True sm['pathPlan'].posenetValid = True # detect sound card presence sounds_available = not os.path.isfile('/EON') or ( os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE') # controlsd is driven by can recv, expected at 100Hz rk = Ratekeeper(100, print_delay_threshold=None) prof = Profiler(False) # off by default while True: start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\ data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params) prof.checkpoint("Sample") # Create alerts if not sm.all_alive_and_valid(): events.append( create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not sm['pathPlan'].mpcSolutionValid: events.append( create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sm['pathPlan'].sensorValid: events.append( create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT])) if not sm['pathPlan'].paramsValid: events.append(create_event('vehicleModelInvalid', [ET.WARNING])) if not sm['pathPlan'].posenetValid: events.append( create_event('posenetInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not sm['plan'].radarValid: events.append( create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if sm['plan'].radarCanError: events.append( create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not CS.canValid: events.append( create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sounds_available: events.append( create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT])) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and sm[ 'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: events.append( create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not read_only: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc) prof.checkpoint("State Control") # Publish data CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev) prof.checkpoint("Sent") rk.monitor_time() prof.display()
def radard_thread(sm=None, pm=None, can_sock=None): set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) use_tesla_radar = CarSettings().get_value("useTeslaRadar") mocked = (CP.carName == "mock") or ((CP.carName == "tesla") and not use_tesla_radar) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters', 'pathPlan']) # *** publish radarState and liveTracks if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) icLeads = messaging.pub_sock('uiIcLeads') ahbInfo = messaging.pub_sock('ahbInfo') RI = RadarInterface(CP) rk = Ratekeeper(1.0 / DT_RDR, print_delay_threshold=None) RD = RadarD(mocked, RI, use_tesla_radar,RI.delay) has_radar = not CP.radarOffCan or mocked last_md_ts = 0. v_ego = 0. while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) sm.update(0) if sm.updated['controlsState']: v_ego = sm['controlsState'].vEgo rr,rrext,ahbCarDetected = RI.update(can_strings,v_ego) if rr is None: continue dat,datext = RD.update(rk.frame, sm, rr, has_radar, rrext) dat.radarState.cumLagMs = -rk.remaining*1000. pm.send('radarState', dat) icLeads.send(datext.to_bytes()) ahbInfoMsg = tesla.AHBinfo.new_message() ahbInfoMsg.source = 0 ahbInfoMsg.radarCarDetected = ahbCarDetected ahbInfoMsg.cameraCarDetected = False ahbInfo.send(ahbInfoMsg.to_bytes()) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(sorted(tracks.keys())): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } pm.send('liveTracks', dat) rk.monitor_time()
def radard_thread(gctx=None): set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) use_tesla_radar = CarSettings().get_value("useTeslaRadar") mocked = (CP.carName == "mock") or ((CP.carName == "tesla") and not use_tesla_radar) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module( 'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface can_sock = messaging.sub_sock(service_list['can'].port) sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) RI = RadarInterface(CP) # *** publish radarState and liveTracks radarState = messaging.pub_sock(service_list['radarState'].port) liveTracks = messaging.pub_sock(service_list['liveTracks'].port) icLeads = messaging.pub_sock(service_list['uiIcLeads'].port) rk = Ratekeeper(rate, print_delay_threshold=None) RD = RadarD(mocked, RI) has_radar = not CP.radarOffCan or mocked last_md_ts = 0. v_ego = 0. while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) rr, rrext = RI.update(can_strings) if rr is None: continue sm.update(0) if sm.updated['controlsState']: v_ego = sm['controlsState'].vEgo dat, datext = RD.update(rk.frame, RI.delay, sm, rr, has_radar, rrext) dat.radarState.cumLagMs = -rk.remaining * 1000. radarState.send(dat.to_bytes()) icLeads.send(datext.to_bytes()) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(tracks.keys()): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } liveTracks.send(dat.to_bytes()) rk.monitor_time()
from common.basedir import BASEDIR from selfdrive import messaging if __name__ == "__main__": sound_dir = os.path.join(BASEDIR, "selfdrive/assets/sounds") sound_files = [f for f in os.listdir(sound_dir) if f.endswith(".wav")] play_sound = os.path.join(BASEDIR, "selfdrive/ui/test/play_sound") print("disabling charging") os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled') os.environ["LD_LIBRARY_PATH"] = "" sm = messaging.SubMaster(["thermal"]) FNULL = open(os.devnull, "w") start_time = time.time() while True: volume = 15 n = random.randint(5, 10) procs = [] for _ in range(n): sound = random.choice(sound_files) p = subprocess.Popen( [play_sound, os.path.join(sound_dir, sound), str(volume)], stdout=FNULL,
def radard_thread(gctx=None): set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) mocked = CP.carName == "mock" VM = VehicleModel(CP) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module( 'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface can_sock = messaging.sub_sock(service_list['can'].port) sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) RI = RadarInterface(CP) # *** publish radarState and liveTracks radarState = messaging.pub_sock(service_list['radarState'].port) liveTracks = messaging.pub_sock(service_list['liveTracks'].port) rk = Ratekeeper(rate, print_delay_threshold=None) RD = RadarD(VM, mocked) while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) rr = RI.update(can_strings) if rr is None: continue sm.update(0) dat = RD.update(rk.frame, RI.delay, sm, rr) dat.radarState.cumLagMs = -rk.remaining * 1000. radarState.send(dat.to_bytes()) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(tracks.keys()): if DEBUG: print("id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \ (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel, tracks[ids].dPath, tracks[ids].vLat, tracks[ids].vLead, tracks[ids].vLeadK, tracks[ids].aLeadK, tracks[ids].stationary, tracks[ids].measured)) dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), "aRel": float(tracks[ids].aRel), "stationary": bool(tracks[ids].stationary), "oncoming": bool(tracks[ids].oncoming), } liveTracks.send(dat.to_bytes()) rk.monitor_time()
def controlsd_thread(gctx=None): setproctitle('controlsd') params = Params() print(params) # Pub Sockets profiler = Profiler(True, 'controls') sendcan = messaging.pub_sock(service_list['sendcan'].port) controlsstate = messaging.pub_sock(service_list['controlsState'].port) carstate = None #messaging.pub_sock(service_list['carState'].port) carcontrol = messaging.pub_sock(service_list['carControl'].port) carevents = messaging.pub_sock(service_list['carEvents'].port) carparams = messaging.pub_sock(service_list['carParams'].port) sm = messaging.SubMaster(['pathPlan', 'health', 'gpsLocationExternal']) can_sock = messaging.sub_sock(service_list['can'].port) hw_type = messaging.recv_one(sm.sock['health']).health.hwType is_panda_black = hw_type == log.HealthData.HwType.blackPanda print("panda black: ", is_panda_black) wait_for_can(can_sock) CI, CP = get_car(can_sock, sendcan, is_panda_black) #logcan.close() # TODO: Use the logcan socket from above, but that will currenly break the tests #can_timeout = None #if os.environ.get('NO_CAN_TIMEOUT', False) else 100 #can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout) # Write CarParams for radard and boardd safety mode params.put("CarParams", CP.to_bytes()) params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") CC = car.CarControl.new_message() AM = AlertManager() startup_alert = get_startup_alert(True, True) AM.add(sm.frame, startup_alert, False) LaC = LatControlPID(CP) lateral = Lateral(CP) lkasMode = int(float(LaC.kegman.conf['lkasMode'])) #CI.CS.lkasMode = (lkasMode == 0) lac_log = None #car.CarState.lateralControlState.pidState.new_message() state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 events_prev = [] frame = 0 sm['pathPlan'].sensorValid = True sm['pathPlan'].posenetValid = True while True: start_time = 0 # time.time() #sec_since_boot() # Sample data and compute car events CS, events = data_sample(CI, CC, can_sock, carstate, lac_log, lateral, sm, profiler) profiler.checkpoint('data_sample') state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) profiler.checkpoint('state_transition') # Compute actuators (runs PID loops and lateral MPC) sm.update(0) profiler.checkpoint('sm_update') actuators, lac_log = state_control(sm.frame, lkasMode, sm['pathPlan'], CS, CP, state, events, AM, LaC, lac_log, profiler) profiler.checkpoint('state_control') # Publish data CC, events_prev = data_send(sm, CS, CI, CP, state, events, actuators, carstate, carcontrol, carevents, carparams, controlsstate, sendcan, AM, LaC, start_time, lac_log, events_prev, profiler) profiler.checkpoint('data_send') frame += 1 if frame % 10000 == 0 and profiler.enabled: profiler.display() profiler.reset(True)
def ui_thread(addr, frame_address): # TODO: Detect car from replay and use that to select carparams CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017") VM = VehicleModel(CP) CalP = np.asarray([[0, 0], [MODEL_INPUT_SIZE[0], 0], [MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]], [0, MODEL_INPUT_SIZE[1]]]) vanishing_point = np.asarray([[MODEL_CX, MODEL_CY]]) pygame.init() pygame.font.init() assert pygame_modules_have_loaded() if HOR: size = (640 + 384 + 640, 960) write_x = 5 write_y = 680 else: size = (640 + 384, 960 + 300) write_x = 645 write_y = 970 pygame.display.set_caption("openpilot debug UI") screen = pygame.display.set_mode(size, pygame.DOUBLEBUF) alert1_font = pygame.font.SysFont("arial", 30) alert2_font = pygame.font.SysFont("arial", 20) info_font = pygame.font.SysFont("arial", 15) camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert() cameraw_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24).convert() cameraw_test_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24) top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8) frame = messaging.sub_sock('frame', addr=addr, conflate=True) sm = messaging.SubMaster([ 'carState', 'plan', 'carControl', 'radarState', 'liveCalibration', 'controlsState', 'liveTracks', 'model', 'liveMpc', 'liveParameters', 'pathPlan' ]) calibration = None img = np.zeros((480, 640, 3), dtype='uint8') imgff = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype=np.uint8) imgw = np.zeros((160, 320, 3), dtype=np.uint8) # warped image lid_overlay_blank = get_blank_lid_overlay(UP) # plots name_to_arr_idx = { "gas": 0, "computer_gas": 1, "user_brake": 2, "computer_brake": 3, "v_ego": 4, "v_pid": 5, "angle_steers_des": 6, "angle_steers": 7, "angle_steers_k": 8, "steer_torque": 9, "v_override": 10, "v_cruise": 11, "a_ego": 12, "a_target": 13, "accel_override": 14 } plot_arr = np.zeros((100, len(name_to_arr_idx.values()))) plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])] plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0., 75.), (-3.0, 2.0)] plot_names = [[ "gas", "computer_gas", "user_brake", "computer_brake", "accel_override" ], ["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"], ["v_ego", "v_override", "v_pid", "v_cruise"], ["a_ego", "a_target"]] plot_colors = [["b", "b", "g", "r", "y"], ["b", "g", "y", "r"], ["b", "g", "r", "y"], ["b", "r"]] plot_styles = [["-", "-", "-", "-", "-"], ["-", "-", "-", "-"], ["-", "-", "-", "-"], ["-", "-"]] draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles, bigplots=True) counter = 0 while 1: list(pygame.event.get()) screen.fill((64, 64, 64)) lid_overlay = lid_overlay_blank.copy() top_down = top_down_surface, lid_overlay # ***** frame ***** fpkt = messaging.recv_one(frame) rgb_img_raw = fpkt.frame.image if fpkt.frame.transform: img_transform = np.array(fpkt.frame.transform).reshape(3, 3) else: # assume frame is flipped img_transform = np.array([[-1.0, 0.0, FULL_FRAME_SIZE[0] - 1], [0.0, -1.0, FULL_FRAME_SIZE[1] - 1], [0.0, 0.0, 1.0]]) if rgb_img_raw and len( rgb_img_raw) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3: imgff = np.frombuffer(rgb_img_raw, dtype=np.uint8).reshape( (FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3)) imgff = imgff[:, :, ::-1] # Convert BGR to RGB cv2.warpAffine(imgff, np.dot(img_transform, _BB_TO_FULL_FRAME)[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP) intrinsic_matrix = eon_intrinsics else: img.fill(0) intrinsic_matrix = np.eye(3) if calibration is not None: transform = np.dot(img_transform, calibration.model_to_full_frame) imgw = cv2.warpAffine(imgff, transform[:2], (MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]), flags=cv2.WARP_INVERSE_MAP) else: imgw.fill(0) sm.update() w = sm['controlsState'].lateralControlState.which() if w == 'lqrState': angle_steers_k = sm[ 'controlsState'].lateralControlState.lqrState.steerAngle elif w == 'indiState': angle_steers_k = sm[ 'controlsState'].lateralControlState.indiState.steerAngle else: angle_steers_k = np.inf plot_arr[:-1] = plot_arr[1:] plot_arr[ -1, name_to_arr_idx['angle_steers']] = sm['controlsState'].angleSteers plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm[ 'carControl'].actuators.steerAngle plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas plot_arr[ -1, name_to_arr_idx['computer_gas']] = sm['carControl'].actuators.gas plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake plot_arr[-1, name_to_arr_idx['steer_torque']] = sm[ 'carControl'].actuators.steer * ANGLE_SCALE plot_arr[-1, name_to_arr_idx['computer_brake']] = sm[ 'carControl'].actuators.brake plot_arr[-1, name_to_arr_idx['v_ego']] = sm['controlsState'].vEgo plot_arr[-1, name_to_arr_idx['v_pid']] = sm['controlsState'].vPid plot_arr[-1, name_to_arr_idx['v_override']] = sm[ 'carControl'].cruiseControl.speedOverride plot_arr[ -1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo plot_arr[-1, name_to_arr_idx['a_target']] = sm['plan'].aTarget plot_arr[-1, name_to_arr_idx['accel_override']] = sm[ 'carControl'].cruiseControl.accelOverride # ***** model **** if len(sm['model'].path.poly) > 0: model_data = extract_model_data(sm['model']) plot_model(model_data, VM, sm['controlsState'].vEgo, sm['controlsState'].curvature, imgw, calibration, top_down, np.array(sm['pathPlan'].dPoly)) # MPC if sm.updated['liveMpc']: draw_mpc(sm['liveMpc'], top_down) # draw all radar points maybe_update_radar_points(sm['liveTracks'], top_down[1]) if sm.updated['liveCalibration']: extrinsic_matrix = np.asarray( sm['liveCalibration'].extrinsicMatrix).reshape(3, 4) ke = intrinsic_matrix.dot(extrinsic_matrix) warp_matrix = get_camera_frame_from_model_frame(ke) calibration = CalibrationTransformsForWarpMatrix( warp_matrix, intrinsic_matrix, extrinsic_matrix) # draw red pt for lead car in the main img for lead in [sm['radarState'].leadOne, sm['radarState'].leadTwo]: if lead.status: if calibration is not None: draw_lead_on(img, lead.dRel, lead.yRel, calibration, color=(192, 0, 0)) draw_lead_car(lead.dRel, top_down) # *** blits *** pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1)) screen.blit(camera_surface, (0, 0)) # display alerts alert_line1 = alert1_font.render(sm['controlsState'].alertText1, True, (255, 0, 0)) alert_line2 = alert2_font.render(sm['controlsState'].alertText2, True, (255, 0, 0)) screen.blit(alert_line1, (180, 150)) screen.blit(alert_line2, (180, 190)) if calibration is not None and img is not None: cpw = warp_points(CalP, calibration.model_to_bb) vanishing_pointw = warp_points(vanishing_point, calibration.model_to_bb) pygame.draw.polygon(screen, BLUE, tuple(map(tuple, cpw)), 1) pygame.draw.circle(screen, BLUE, list(map(int, map(round, vanishing_pointw[0]))), 2) if HOR: screen.blit(draw_plots(plot_arr), (640 + 384, 0)) else: screen.blit(draw_plots(plot_arr), (0, 600)) pygame.surfarray.blit_array(cameraw_surface, imgw.swapaxes(0, 1)) screen.blit(cameraw_surface, (320, 480)) pygame.surfarray.blit_array(*top_down) screen.blit(top_down[0], (640, 0)) i = 0 SPACING = 25 lines = [ info_font.render("ENABLED", True, GREEN if sm['controlsState'].enabled else BLACK), info_font.render("BRAKE LIGHTS", True, RED if sm['carState'].brakeLights else BLACK), info_font.render( "SPEED: " + str(round(sm['carState'].vEgo, 1)) + " m/s", True, YELLOW), info_font.render( "LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), True, YELLOW), info_font.render( "LONG MPC SOURCE: " + str(sm['plan'].longitudinalPlanSource), True, YELLOW), None, info_font.render( "ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverage, 2)) + " deg", True, YELLOW), info_font.render( "ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffset, 2)) + " deg", True, YELLOW), info_font.render( "STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100., 2)) + " %", True, YELLOW), info_font.render( "STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), True, YELLOW) ] for i, line in enumerate(lines): if line is not None: screen.blit(line, (write_x, write_y + i * SPACING)) # this takes time...vsync or something pygame.display.flip()
def radard_thread(gctx=None): set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) mocked = CP.carName == "mock" VM = VehicleModel(CP) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) # Default parameters live_parameters = messaging.new_message() live_parameters.init('liveParameters') live_parameters.liveParameters.valid = True live_parameters.liveParameters.steerRatio = CP.steerRatio live_parameters.liveParameters.stiffnessFactor = 1.0 MP = ModelParser() RI = RadarInterface(CP) last_md_ts = 0 last_controls_state_ts = 0 # *** publish radarState and liveTracks radarState = messaging.pub_sock(service_list['radarState'].port) liveTracks = messaging.pub_sock(service_list['liveTracks'].port) path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max # Time-alignment rate = 1. / DT_MDL # model and radar are both at 20Hz v_len = 20 # how many speed data points to remember for t alignment with rdr data active = 0 steer_angle = 0. steer_override = False tracks = defaultdict(dict) # Kalman filter stuff: ekfv = EKFV1D() speedSensorV = SimpleSensor(XV, 1, 2) # v_ego v_ego = 0. v_ego_hist_t = deque([0], maxlen=v_len) v_ego_hist_v = deque([0], maxlen=v_len) v_ego_t_aligned = 0. rk = Ratekeeper(rate, print_delay_threshold=None) while 1: rr = RI.update() ar_pts = {} for pt in rr.points: ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured] sm.update(0) if sm.updated['liveParameters']: VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) if sm.updated['controlsState']: active = sm['controlsState'].active v_ego = sm['controlsState'].vEgo steer_angle = sm['controlsState'].angleSteers steer_override = sm['controlsState'].steerOverride v_ego_hist_v.append(v_ego) v_ego_hist_t.append(float(rk.frame)/rate) last_controls_state_ts = sm.logMonoTime['controlsState'] if sm.updated['model']: last_md_ts = sm.logMonoTime['model'] MP.update(v_ego, sm['model']) # run kalman filter only if prob is high enough if MP.lead_prob > 0.7: reading = speedSensorV.read(MP.lead_dist, covar=np.matrix(MP.lead_var)) ekfv.update_scalar(reading) ekfv.predict(DT_MDL) # When changing lanes the distance to the lead car can suddenly change, # which makes the Kalman filter output large relative acceleration if mocked and abs(MP.lead_dist - ekfv.state[XV]) > 2.0: ekfv.state[XV] = MP.lead_dist ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init])) ekfv.state[SPEEDV] = 0. ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(MP.d_poly, float(ekfv.state[XV])), float(ekfv.state[SPEEDV]), False) else: ekfv.state[XV] = MP.lead_dist ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init])) ekfv.state[SPEEDV] = 0. if VISION_POINT in ar_pts: del ar_pts[VISION_POINT] # *** compute the likely path_y *** if (active and not steer_override) or mocked: # use path from model (always when mocking as steering is too noisy) path_y = np.polyval(MP.d_poly, path_x) else: # use path from steer, set angle_offset to 0 it does not only report the physical offset path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=live_parameters.liveParameters.angleOffsetAverage)[0] # *** remove missing points from meta data *** for ids in tracks.keys(): if ids not in ar_pts: tracks.pop(ids, None) # *** compute the tracks *** for ids in ar_pts: # ignore standalone vision point, unless we are mocking the radar if ids == VISION_POINT and not mocked: continue rpt = ar_pts[ids] # align v_ego by a fixed time to align it with the radar measurement cur_time = float(rk.frame)/rate v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_hist_t, v_ego_hist_v) d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2)) # add sign d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y)) # create the track if it doesn't exist or it's a new track if ids not in tracks: tracks[ids] = Track() tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override) # allow the vision model to remove the stationary flag if distance and rel speed roughly match if VISION_POINT in ar_pts: fused_id = None best_score = NO_FUSION_SCORE for ids in tracks: dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - tracks[ids].yRel)) ** 2) rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel) tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff) if best_score > tracks[ids].vision_score: fused_id = ids best_score = tracks[ids].vision_score if fused_id is not None: tracks[fused_id].vision_cnt += 1 tracks[fused_id].update_vision_fusion() if DEBUG: print("NEW CYCLE") if VISION_POINT in ar_pts: print("vision", ar_pts[VISION_POINT]) idens = list(tracks.keys()) track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens]) # If we have multiple points, cluster them if len(track_pts) > 1: cluster_idxs = cluster_points_centroid(track_pts, 2.5) clusters = [None] * (max(cluster_idxs) + 1) for idx in xrange(len(track_pts)): cluster_i = cluster_idxs[idx] if clusters[cluster_i] is None: clusters[cluster_i] = Cluster() clusters[cluster_i].add(tracks[idens[idx]]) elif len(track_pts) == 1: # TODO: why do we need this? clusters = [Cluster()] clusters[0].add(tracks[idens[0]]) else: clusters = [] if DEBUG: for i in clusters: print(i) # *** extract the lead car *** lead_clusters = [c for c in clusters if c.is_potential_lead(v_ego)] lead_clusters.sort(key=lambda x: x.dRel) lead_len = len(lead_clusters) # *** extract the second lead from the whole set of leads *** lead2_clusters = [c for c in lead_clusters if c.is_potential_lead2(lead_clusters)] lead2_clusters.sort(key=lambda x: x.dRel) lead2_len = len(lead2_clusters) # *** publish radarState *** dat = messaging.new_message() dat.init('radarState') dat.valid = sm.all_alive_and_valid(service_list=['controlsState']) dat.radarState.mdMonoTime = last_md_ts dat.radarState.canMonoTimes = list(rr.canMonoTimes) dat.radarState.radarErrors = list(rr.errors) dat.radarState.controlsStateMonoTime = last_controls_state_ts if lead_len > 0: dat.radarState.leadOne = lead_clusters[0].toRadarState() if lead2_len > 0: dat.radarState.leadTwo = lead2_clusters[0].toRadarState() else: dat.radarState.leadTwo.status = False else: dat.radarState.leadOne.status = False dat.radarState.cumLagMs = -rk.remaining*1000. radarState.send(dat.to_bytes()) # *** publish tracks for UI debugging (keep last) *** dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(tracks.keys()): if DEBUG: print("id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \ (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel, tracks[ids].dPath, tracks[ids].vLat, tracks[ids].vLead, tracks[ids].vLeadK, tracks[ids].aLeadK, tracks[ids].stationary, tracks[ids].measured)) dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), "aRel": float(tracks[ids].aRel), "stationary": bool(tracks[ids].stationary), "oncoming": bool(tracks[ids].oncoming), } liveTracks.send(dat.to_bytes()) rk.monitor_time()