def wait_for_sockets(socks, timeout=10.0): sm = messaging.SubMaster(socks) t = time.time() recvd = [] while time.time() - t < timeout and len(recvd) < len(socks): sm.update() for s in socks: if s not in recvd and sm.updated[s]: recvd.append(s) return recvd
def model_replay(lr_list, frs): spinner = Spinner() spinner.update("starting model replay") vipc_server = VisionIpcServer("camerad") vipc_server.create_buffers( VisionStreamType.VISION_STREAM_YUV_BACK, 40, False, *(tici_f_frame_size if TICI else eon_f_frame_size)) vipc_server.create_buffers( VisionStreamType.VISION_STREAM_YUV_FRONT, 40, False, *(tici_d_frame_size if TICI else eon_d_frame_size)) vipc_server.start_listener() pm = messaging.PubMaster([ 'roadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan' ]) sm = messaging.SubMaster(['modelV2', 'driverState']) try: managed_processes['modeld'].start() managed_processes['dmonitoringmodeld'].start() time.sleep(5) sm.update(1000) last_desire = None log_msgs = [] frame_idxs = dict.fromkeys(['roadCameraState', 'driverCameraState'], 0) cal = [msg for msg in lr if msg.which() == "liveCalibration"] for msg in cal[:5]: pm.send(msg.which(), replace_calib(msg, None)) for msg in tqdm(lr_list): if msg.which() == "liveCalibration": last_calib = list(msg.liveCalibration.rpyCalib) pm.send(msg.which(), replace_calib(msg, last_calib)) elif msg.which() == "lateralPlan": last_desire = msg.lateralPlan.desire elif msg.which() in ["roadCameraState", "driverCameraState"]: ret = process_frame(msg, pm, sm, log_msgs, vipc_server, spinner, frs, frame_idxs, last_desire) if ret is None: break except KeyboardInterrupt: pass finally: spinner.close() managed_processes['modeld'].stop() managed_processes['dmonitoringmodeld'].stop() return log_msgs
def radard_thread(sm=None, pm=None, can_sock=None): config_realtime_process(5, Priority.CTRL_LOW) # 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)) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module(f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface # *** setup messaging if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState']) # Can't check average frequency, since radar determines timing if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) RI = RadarInterface(CP) rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) RD = RadarD(CP.radarTimeStep, RI.delay) 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(sm, rr) dat.radarState.cumLagMs = -rk.remaining*1000. pm.send('radarState', dat) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message('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 cycle_alerts(duration=200, is_metric=False): alerts = list(EVENTS.keys()) print(alerts) CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") sm = messaging.SubMaster([ 'thermal', 'health', 'frame', 'model', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman' ]) controls_state = messaging.pub_sock('controlsState') thermal = messaging.pub_sock('thermal') idx, last_alert_millis = 0, 0 events = Events() AM = AlertManager() frame = 0 while 1: if frame % duration == 0: idx = (idx + 1) % len(alerts) events.clear() events.add(alerts[idx]) current_alert_types = [ ET.PERMANENT, ET.USER_DISABLE, ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE, ET.PRE_ENABLE, ET.NO_ENTRY, ET.ENABLE, ET.WARNING ] a = events.create_alerts(current_alert_types, [CP, sm, is_metric]) AM.add_many(frame, a) AM.process_alerts(frame) dat = messaging.new_message() dat.init('controlsState') dat.controlsState.alertText1 = AM.alert_text_1 dat.controlsState.alertText2 = AM.alert_text_2 dat.controlsState.alertSize = AM.alert_size dat.controlsState.alertStatus = AM.alert_status dat.controlsState.alertBlinkingRate = AM.alert_rate dat.controlsState.alertType = AM.alert_type dat.controlsState.alertSound = AM.audible_alert controls_state.send(dat.to_bytes()) dat = messaging.new_message() dat.init('thermal') dat.thermal.started = True thermal.send(dat.to_bytes()) frame += 1 time.sleep(0.01)
def main(): sm = None if sm is None: sm = messaging.SubMaster(['plan', 'pathPlan', 'model']) v_ego = 50 * CV.KPH_TO_MS print('curvature test ') while True: sm.update(0) value, model_sum = sc.calc_va(sm, v_ego) print('curvature={:.3f} sum={:.5f}'.format(value, model_sum))
def test_update(self): sock = "carState" pub_sock = messaging.pub_sock(sock) sm = messaging.SubMaster([sock,]) zmq_sleep() for i in range(10): msg = messaging.new_message(sock) pub_sock.send(msg.to_bytes()) sm.update(1000) self.assertEqual(sm.frame, i) self.assertTrue(all(sm.updated.values()))
def manager_thread(): cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log #subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) ignore = [] if os.getenv("NOBOARD") is not None: ignore.append("pandad") if os.getenv("BLOCK") is not None: ignore += os.getenv("BLOCK").split(",") if EON: pm_apply_packages('enable') ensure_running(managed_processes.values(), started=False, not_run=ignore) started_prev = False params = Params() sm = messaging.SubMaster(['deviceState']) pm = messaging.PubMaster(['managerState']) while True: sm.update() not_run = ignore[:] if sm['deviceState'].freeSpacePercent < 5: not_run.append("loggerd") started = sm['deviceState'].started driverview = params.get("IsDriverViewEnabled") == b"1" ensure_running(managed_processes.values(), started, driverview, not_run) # trigger an update after going offroad if started_prev and not started and 'updated' in managed_processes: os.sync() managed_processes['updated'].signal(signal.SIGHUP) started_prev = started running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) for p in managed_processes.values() if p.proc] cloudlog.debug(' '.join(running_list)) # send managerState msg = messaging.new_message('managerState') msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()] pm.send('managerState', msg) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break
def main(sm=None, pm=None): if sm is None: sm = messaging.SubMaster( ['liveLocationKalman', 'gnssMeasurements', 'managerState']) if pm is None: pm = messaging.PubMaster(['navInstruction', 'navRoute']) rk = Ratekeeper(1.0) route_engine = RouteEngine(sm, pm) while True: route_engine.update() rk.keep_time()
def locationd_thread(sm, pm, disabled_logs=None): gc.disable() if disabled_logs is None: disabled_logs = [] if sm is None: socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState'] sm = messaging.SubMaster(socks, ignore_alive=['gpsLocationExternal'], ignore_avg_freq=['sensorEvents']) if pm is None: pm = messaging.PubMaster(['liveLocationKalman']) params = Params() localizer = Localizer(disabled_logs=disabled_logs) while True: sm.update() for sock, updated in sm.updated.items(): if updated and sm.valid[sock]: t = sm.logMonoTime[sock] * 1e-9 if sock == "sensorEvents": localizer.handle_sensors(t, sm[sock]) elif sock == "gpsLocationExternal": localizer.handle_gps(t, sm[sock]) elif sock == "carState": localizer.handle_car_state(t, sm[sock]) elif sock == "cameraOdometry": localizer.handle_cam_odo(t, sm[sock]) elif sock == "liveCalibration": localizer.handle_live_calib(t, sm[sock]) if sm.updated['cameraOdometry']: t = sm.logMonoTime['cameraOdometry'] msg = messaging.new_message('liveLocationKalman') msg.logMonoTime = t msg.liveLocationKalman = localizer.liveLocationMsg() msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid() msg.liveLocationKalman.sensorsOK = sm.alive['sensorEvents'] and sm.valid['sensorEvents'] gps_age = (t / 1e9) - localizer.last_gps_fix msg.liveLocationKalman.gpsOK = gps_age < 1.0 pm.send('liveLocationKalman', msg) if sm.frame % 1200 == 0 and msg.liveLocationKalman.gpsOK: # once a minute location = { 'latitude': msg.liveLocationKalman.positionGeodetic.value[0], 'longitude': msg.liveLocationKalman.positionGeodetic.value[1], 'altitude': msg.liveLocationKalman.positionGeodetic.value[2], } params.put("LastGPSPosition", json.dumps(location))
def main(sm=None, pm=None): if sm is None: sm = messaging.SubMaster(['liveLocationKalman', 'carState']) if pm is None: pm = messaging.PubMaster(['liveParameters']) # TODO: Read from car params at runtime from selfdrive.car.toyota.interface import CarInterface from selfdrive.car.toyota.values import CAR CP = CarInterface.get_params(CAR.COROLLA_TSS2) learner = ParamsLearner(CP) while True: sm.update() for which, updated in sm.updated.items(): if not updated: continue t = sm.logMonoTime[which] * 1e-9 learner.handle_log(t, which, sm[which]) # TODO: set valid to false when locationd stops sending # TODO: make sure controlsd knows when there is no gyro # TODO: move posenetValid somewhere else to show the model uncertainty alert # TODO: Save and resume values from param # TODO: Change KF to allow mass, etc to be inputs in predict step if sm.updated['carState']: msg = messaging.new_message('liveParameters') msg.logMonoTime = sm.logMonoTime['carState'] msg.liveParameters.valid = True # TODO: Check if learned values are sane msg.liveParameters.posenetValid = True msg.liveParameters.sensorValid = True x = learner.kf.x msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) msg.liveParameters.angleOffsetAverage = math.degrees( x[States.ANGLE_OFFSET]) msg.liveParameters.angleOffset = math.degrees( x[States.ANGLE_OFFSET_FAST]) # P = learner.kf.P # print() # print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5) # print("x ", float(x[States.STIFFNESS]), float(P[States.STIFFNESS, States.STIFFNESS])**0.5) # print("ao avg ", math.degrees(x[States.ANGLE_OFFSET]), math.degrees(P[States.ANGLE_OFFSET, States.ANGLE_OFFSET])**0.5) # print("ao ", math.degrees(x[States.ANGLE_OFFSET_FAST]), math.degrees(P[States.ANGLE_OFFSET_FAST, States.ANGLE_OFFSET_FAST])**0.5) pm.send('liveParameters', msg)
def test_boardd_loopback(): # wait for boardd to init spinner = Spinner() time.sleep(2) with Timeout(60, "boardd didn't start"): sm = messaging.SubMaster(['pandaState']) while sm.rcv_frame['pandaState'] < 1: sm.update(1000) # boardd blocks on CarVin and CarParams cp = car.CarParams.new_message() cp.safetyModel = car.CarParams.SafetyModel.allOutput Params().put("CarVin", b"0"*17) Params().put("CarParams", cp.to_bytes()) sendcan = messaging.pub_sock('sendcan') can = messaging.sub_sock('can', conflate=False, timeout=100) time.sleep(1) n = 1000 for i in range(n): spinner.update(f"boardd loopback {i}/{n}") sent_msgs = defaultdict(set) for _ in range(random.randrange(10)): to_send = [] for __ in range(random.randrange(100)): bus = random.randrange(3) addr = random.randrange(1, 1<<29) dat = bytes([random.getrandbits(8) for _ in range(random.randrange(1, 9))]) sent_msgs[bus].add((addr, dat)) to_send.append(make_can_msg(addr, dat, bus)) sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan')) max_recv = 10 while max_recv > 0 and any(len(sent_msgs[bus]) for bus in range(3)): recvd = messaging.drain_sock(can, wait_for_one=True) for msg in recvd: for m in msg.can: if m.src >= 128: k = (m.address, m.dat) assert k in sent_msgs[m.src-128] sent_msgs[m.src-128].discard(k) max_recv -= 1 # if a set isn't empty, messages got dropped for bus in range(3): assert not len(sent_msgs[bus]), f"loop {i}: bus {bus} missing {len(sent_msgs[bus])} messages" spinner.close()
def __init__(self, CP=None): self.long_control_state = 0 # initialized to off self.seq_step_debug = "" self.long_curv_timer = 0 self.path_x = np.arange(192) self.traceSC = trace1.Loger("SPD_CTRL") self.v_model = 0 self.a_model = 0 self.v_cruise = 0 self.a_cruise = 0 self.l_poly = [] self.r_poly = [] self.movAvg = moveavg1.MoveAvg() self.Timer1 = tm.CTime1000("SPD") self.time_no_lean = 0 self.wait_timer2 = 0 self.cruise_set_speed_kph = 0 self.curise_set_first = 0 self.curise_sw_check = 0 self.prev_clu_CruiseSwState = 0 self.prev_VSetDis = 0 self.sc_clu_speed = 0 self.btn_type = Buttons.NONE self.active_time = 0 self.old_model_speed = 0 self.old_model_init = 0 self.curve_speed = 0 self.curvature_gain = 1 self.params = Params() self.cruise_set_mode = int( self.params.get("CruiseStatemodeSelInit", encoding="utf8")) self.map_spd_limit_offset = int( self.params.get("OpkrSpeedLimitOffset", encoding="utf8")) self.map_spd_enable = False self.map_spd_camera = 0 self.map_enabled = False self.second = 0 self.sm = messaging.SubMaster(['liveMapData'])
def test_conflate(self): sock = "carState" pub_sock = messaging.pub_sock(sock) sm = messaging.SubMaster([sock,]) n = 10 for i in range(n+1): msg = messaging.new_message(sock) msg.carState.vEgo = i pub_sock.send(msg.to_bytes()) time.sleep(0.01) sm.update(1000) self.assertEqual(sm[sock].vEgo, n)
def test_init_state(self): socks = random_socks() sm = messaging.SubMaster(socks) self.assertEqual(sm.frame, -1) self.assertFalse(any(sm.updated.values())) self.assertFalse(any(sm.alive.values())) self.assertTrue(all(t == 0. for t in sm.rcv_time.values())) self.assertTrue(all(f == 0 for f in sm.rcv_frame.values())) self.assertTrue(all(t == 0 for t in sm.logMonoTime.values())) for p in [sm.updated, sm.rcv_time, sm.rcv_frame, sm.alive, sm.sock, sm.freq, sm.data, sm.logMonoTime, sm.valid]: self.assertEqual(len(p), len(socks))
def setUp(self): self.vipc_server = VisionIpcServer("camerad") self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *tici_f_frame_size) self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size) self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *tici_f_frame_size) self.vipc_server.start_listener() self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry']) self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration', 'lateralPlan']) managed_processes['modeld'].start() time.sleep(0.2) self.sm.update(1000)
def main(): sm = messaging.SubMaster(['ubloxGnss']) pm = messaging.PubMaster(['gnssMeasurements']) # todo get last_known_position laikad = Laikad(save_ephemeris=True) while True: sm.update() if sm.updated['ubloxGnss']: ublox_msg = sm['ubloxGnss'] msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss']) if msg is not None: pm.send('gnssMeasurements', msg)
def uploader_fn(exit_event): params = Params() dongle_id = params.get("DongleId", encoding='utf8') if dongle_id is None: cloudlog.info("uploader missing dongle_id") raise Exception("uploader can't start without dongle id") if TICI and not Path("/data/media").is_mount(): cloudlog.warning("NVME not mounted") sm = messaging.SubMaster(['deviceState']) pm = messaging.PubMaster(['uploaderState']) uploader = Uploader(dongle_id, ROOT) backoff = 0.1 while not exit_event.is_set(): sm.update(0) offroad = params.get_bool("IsOffroad") network_type = sm[ 'deviceState'].networkType if not force_wifi else NetworkType.wifi if network_type == NetworkType.none: if allow_sleep: time.sleep(60 if offroad else 5) continue good_internet = network_type in [ NetworkType.wifi, NetworkType.ethernet ] allow_raw_upload = params.get_bool("UploadRaw") d = uploader.next_file_to_upload( with_raw=allow_raw_upload and good_internet and offroad) if d is None: # Nothing to upload if allow_sleep: time.sleep(60 if offroad else 5) continue key, fn = d cloudlog.debug("upload %r over %s", d, network_type) success = uploader.upload(key, fn) if success: backoff = 0.1 elif allow_sleep: cloudlog.info("upload backoff %r", backoff) time.sleep(backoff + random.uniform(0, backoff)) backoff = min(backoff * 2, 120) pm.send("uploaderState", uploader.get_msg()) cloudlog.info("upload done, success=%r", success)
def camera_replay(lr, fr): spinner = Spinner() pm = messaging.PubMaster(['frame', 'liveCalibration']) sm = messaging.SubMaster(['model']) # TODO: add dmonitoringmodeld print("preparing procs") manager.prepare_managed_process("camerad") manager.prepare_managed_process("modeld") try: print("starting procs") manager.start_managed_process("camerad") manager.start_managed_process("modeld") time.sleep(5) print("procs started") cal = [msg for msg in lr if msg.which() == "liveCalibration"] for msg in cal[:5]: pm.send(msg.which(), msg.as_builder()) log_msgs = [] frame_idx = 0 for msg in tqdm(lr): if msg.which() == "liveCalibrationd": pm.send(msg.which(), msg.as_builder()) elif msg.which() == "frame": f = msg.as_builder() img = fr.get(frame_idx, pix_fmt="rgb24")[0][:, ::, -1] f.frame.image = img.flatten().tobytes() frame_idx += 1 pm.send(msg.which(), f) with Timeout(seconds=15): log_msgs.append(messaging.recv_one(sm.sock['model'])) spinner.update("modeld replay %d/%d" % (frame_idx, fr.frame_count)) if frame_idx >= fr.frame_count: break except KeyboardInterrupt: pass print("replay done") spinner.close() manager.kill_managed_process('modeld') time.sleep(2) manager.kill_managed_process('camerad') return log_msgs
def test_engage(self): # Startup manager and bridge.py. Check processes are running, then engage and verify. p_manager = subprocess.Popen("./launch_openpilot.sh", cwd='../') self.processes.append(p_manager) sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) q = Queue() carla_bridge = CarlaBridge(bridge.parse_args([])) p_bridge = carla_bridge.run(q, retries=3) self.processes.append(p_bridge) max_time_per_step = 20 # Wait for bridge to startup start_waiting = time.monotonic() while not carla_bridge.started and time.monotonic() < start_waiting + max_time_per_step: time.sleep(0.1) self.assertEqual(p_bridge.exitcode, None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}") start_time = time.monotonic() no_car_events_issues_once = False car_event_issues = [] not_running = [] while time.monotonic() < start_time + max_time_per_step: sm.update() not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning] car_event_issues = [event.name for event in sm['carEvents'] if any([event.noEntry, event.softDisable, event.immediateDisable])] if sm.all_alive() and len(car_event_issues) == 0 and len(not_running) == 0: no_car_events_issues_once = True break self.assertTrue(no_car_events_issues_once, f"Failed because no messages received, or CarEvents '{car_event_issues}' or processes not running '{not_running}'") start_time = time.monotonic() min_counts_control_active = 100 control_active = 0 while time.monotonic() < start_time + max_time_per_step: sm.update() q.put("cruise_up") # Try engaging if sm.all_alive() and sm['controlsState'].active: control_active += 1 if control_active == min_counts_control_active: break self.assertEqual(min_counts_control_active, control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}")
def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"): frame_sizes = [ eon_f_frame_size, eon_d_frame_size, leon_d_frame_size, tici_f_frame_size ] frame_sizes = {w * h: (w, h) for (w, h) in frame_sizes} sm = messaging.SubMaster([frame, front_frame]) while min(sm.logMonoTime.values()) == 0: sm.update() rear = extract_image(sm[frame].image, frame_sizes) front = extract_image(sm[front_frame].image, frame_sizes) return rear, front
def __init__(self, sm=None, pm=None, can_sock=None): # Initialize received messages queue self.msgs_queue = Queue() # Setup sockets self.pm = pm if self.pm is None: self.pm = messaging.PubMaster(['testJoystick']) self.sm = sm if self.sm is None: self.sm = messaging.SubMaster(self.publishers) self.rk = Ratekeeper(RATE, print_delay_threshold=None)
def uploader_fn(exit_event): cloudlog.info("uploader_fn") params = Params() dongle_id = params.get("DongleId").decode('utf8') if dongle_id is None: cloudlog.info("uploader missing dongle_id") raise Exception("uploader can't start without dongle id") uploader = Uploader(dongle_id, ROOT) # dp sm = messaging.SubMaster(['dragonConf']) backoff = 0.1 while True: offroad = params.get("IsOffroad") == b'1' allow_raw_upload = (params.get("IsUploadRawEnabled") != b"0") and offroad on_hotspot = is_on_hotspot() on_wifi = is_on_wifi() sm.update(1000) if sm.updated['dragonConf']: on_wifi = True if sm['dragonConf'].dpUploadOnMobile else on_wifi on_hotspot = False if sm['dragonConf'].dpUploadOnHotspot else on_hotspot should_upload = on_wifi and not on_hotspot if exit_event.is_set(): return d = uploader.next_file_to_upload(with_raw=allow_raw_upload and should_upload) if d is None: # Nothing to upload time.sleep(60 if offroad else 5) continue key, fn = d cloudlog.event("uploader_netcheck", is_on_hotspot=on_hotspot, is_on_wifi=on_wifi) cloudlog.info("to upload %r", d) success = uploader.upload(key, fn) if success: backoff = 0.1 else: cloudlog.info("backoff %r", backoff) time.sleep(backoff + random.uniform(0, backoff)) backoff = min(backoff*2, 120) cloudlog.info("upload done, success=%r", success)
def status_monitor(): # use driverState socker to drive timing. driverState = messaging.sub_sock('driverState', addr=args.addr, conflate=True) sm = messaging.SubMaster(['carState', 'dMonitoringState'], addr=args.addr) steering_status = HandsOnWheelStatus() v_cruise_last = 0 while messaging.recv_one(driverState): try: sm.update() v_cruise = sm['carState'].cruiseState.speed steering_wheel_engaged = len(sm['carState'].buttonEvents) > 0 or \ v_cruise != v_cruise_last or sm['carState'].steeringPressed v_cruise_last = v_cruise # Get status from our own instance of SteeringStatus steering_status.update(Events(), steering_wheel_engaged, sm['carState'].cruiseState.enabled, sm['carState'].vEgo) steering_state = steering_status.hands_on_wheel_state state_name = "Unknown " if steering_state == HandsOnWheelState.none: state_name = "Not Active " elif steering_state == HandsOnWheelState.ok: state_name = "Hands On Wheel " elif steering_state == HandsOnWheelState.minor: state_name = "Hands Off Wheel - Minor " elif steering_state == HandsOnWheelState.warning: state_name = "Hands Off Wheel - Warning " elif steering_state == HandsOnWheelState.critical: state_name = "Hands Off Wheel - Critical" elif steering_state == HandsOnWheelState.terminal: state_name = "Hands Off Wheel - Terminal" # Get events from `dMonitoringState` events = sm['dMonitoringState'].events event_name = events[0].name if len(events) else "None" event_name = "{:<30}".format(event_name[:30]) # Print output sys.stdout.write( f'\rSteering State: {state_name} | event: {event_name}') sys.stdout.flush() except Exception as e: print(e)
def main(): sm = messaging.SubMaster(['ubloxGnss']) pm = messaging.PubMaster(['gnssMeasurements']) laikad = Laikad(use_internet=True) while True: sm.update() if sm.updated['ubloxGnss']: ublox_msg = sm['ubloxGnss'] msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss']) if msg is not None: pm.send('gnssMeasurements', msg)
def manager_thread() -> None: Process(name="road_speed_limiter", target=launcher, args=("selfdrive.road_speed_limiter", "road_speed_limiter")).start() cloudlog.bind(daemon="manager") cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) params = Params() ignore: List[str] = [] if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID): ignore += ["manage_athenad", "uploader"] if os.getenv("NOBOARD") is not None: ignore.append("pandad") ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0] ensure_running(managed_processes.values(), started=False, not_run=ignore) sm = messaging.SubMaster(['deviceState', 'carParams'], poll=['deviceState']) pm = messaging.PubMaster(['managerState']) while True: sm.update() started = sm['deviceState'].started driverview = params.get_bool("IsDriverViewEnabled") ensure_running(managed_processes.values(), started=started, driverview=driverview, notcar=sm['carParams'].notCar, not_run=ignore) running = ' '.join("%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) for p in managed_processes.values() if p.proc) print(running) cloudlog.debug(running) # send managerState msg = messaging.new_message('managerState') msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()] pm.send('managerState', msg) # Exit main loop when uninstall/shutdown/reboot is needed shutdown = False for param in ("DoUninstall", "DoShutdown", "DoReboot"): if params.get_bool(param): shutdown = True params.put("LastManagerExitReason", param) cloudlog.warning(f"Shutting down manager - {param} set") if shutdown: break
def setUpClass(cls): if "DEBUG" in os.environ: segs = filter(lambda x: os.path.exists(os.path.join(x, "rlog.bz2")), Path(ROOT).iterdir()) segs = sorted(segs, key=lambda x: x.stat().st_mtime) cls.lr = list(LogReader(os.path.join(segs[-2], "rlog.bz2"))) return os.environ['SKIP_FW_QUERY'] = "1" os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" set_params_enabled() # Make sure athena isn't running Params().delete("DongleId") Params().delete("AthenadPid") os.system("pkill -9 -f athena") logger_root = Path(ROOT) initial_segments = set() if logger_root.exists(): initial_segments = set(Path(ROOT).iterdir()) # start manager and run openpilot for a minute try: manager_path = os.path.join(BASEDIR, "selfdrive/manager/manager.py") proc = subprocess.Popen(["python", manager_path]) sm = messaging.SubMaster(['carState']) with Timeout(150, "controls didn't start"): while sm.rcv_frame['carState'] < 0: sm.update(1000) # make sure we get at least two full segments cls.segments = [] with Timeout(300, "timed out waiting for logs"): while len(cls.segments) < 3: new_paths = set() if logger_root.exists(): new_paths = set(logger_root.iterdir()) - initial_segments segs = [p for p in new_paths if "--" in str(p)] cls.segments = sorted(segs, key=lambda s: int(str(s).rsplit('--')[-1])) time.sleep(5) finally: proc.terminate() if proc.wait(60) is None: proc.kill() cls.lr = list(LogReader(os.path.join(str(cls.segments[1]), "rlog.bz2")))
def main(): dog = AstroDog() sm = messaging.SubMaster(['ubloxGnss']) pm = messaging.PubMaster(['gnssMeasurements']) while True: sm.update() # Todo if no internet available use latest ephemeris if sm.updated['ubloxGnss']: ublox_msg = sm['ubloxGnss'] msg = process_ublox_msg(ublox_msg, dog, sm.logMonoTime['ubloxGnss']) if msg is None: msg = messaging.new_message('gnssMeasurements') pm.send('gnssMeasurements', msg)
def gpxd_thread(sm=None, pm=None): if sm is None: sm = messaging.SubMaster(['gpsLocationExternal', 'carState']) wait_helper = WaitTimeHelper() gpxd = GpxD() rk = Ratekeeper(LOG_HERTZ, print_delay_threshold=None) while True: sm.update(0) gpxd.log(sm) gpxd.write_log() if wait_helper.shutdown: gpxd.write_log(True) break rk.keep_time()
def locationd_thread(sm, pm, disabled_logs=None): if disabled_logs is None: disabled_logs = [] if sm is None: socks = [ 'gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState' ] sm = messaging.SubMaster(socks, ignore_alive=['gpsLocationExternal']) if pm is None: pm = messaging.PubMaster(['liveLocationKalman']) localizer = Localizer(disabled_logs=disabled_logs) camera_odometry_cnt = 0 while True: sm.update() for sock, updated in sm.updated.items(): if updated: t = sm.logMonoTime[sock] * 1e-9 if sock == "sensorEvents": localizer.handle_sensors(t, sm[sock]) elif sock == "gpsLocationExternal": localizer.handle_gps(t, sm[sock]) elif sock == "carState": localizer.handle_car_state(t, sm[sock]) elif sock == "cameraOdometry": localizer.handle_cam_odo(t, sm[sock]) elif sock == "liveCalibration": localizer.handle_live_calib(t, sm[sock]) if sm.updated['cameraOdometry']: camera_odometry_cnt += 1 if camera_odometry_cnt % OUTPUT_DECIMATION == 0: t = sm.logMonoTime['cameraOdometry'] msg = messaging.new_message('liveLocationKalman') msg.logMonoTime = t msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9) msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid() gps_age = (t / 1e9) - localizer.last_gps_fix msg.liveLocationKalman.gpsOK = gps_age < 1.0 pm.send('liveLocationKalman', msg)
def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"): frame_sizes = [eon_f_frame_size, eon_d_frame_size, leon_d_frame_size, tici_f_frame_size] frame_sizes = {w * h: (w, h) for (w, h) in frame_sizes} sockets = [] if frame is not None: sockets.append(frame) if front_frame is not None: sockets.append(front_frame) sm = messaging.SubMaster(sockets) while min(sm.logMonoTime.values()) == 0: sm.update() rear = extract_image(sm[frame].image, frame_sizes) if frame is not None else None front = extract_image(sm[front_frame].image, frame_sizes) if front_frame is not None else None return rear, front