diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 0b57228a7a..beef14b816 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -115,38 +115,84 @@ class ModelState: return outputs + + +class CameraStreams: + def __init__(self, cl_context: CLContext): + while True: + available_streams = VisionIpcClient.available_streams("camerad", block=False) + if available_streams: + self.use_extra_client = VisionStreamType.VISION_STREAM_WIDE_ROAD in available_streams and VisionStreamType.VISION_STREAM_ROAD in available_streams + self.main_wide_camera = VisionStreamType.VISION_STREAM_ROAD not in available_streams + break + time.sleep(.1) + + vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if self.main_wide_camera else VisionStreamType.VISION_STREAM_ROAD + self.vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True, cl_context) + self.vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, cl_context) + cloudlog.warning(f"vision stream set up, main_wide_camera: {self.main_wide_camera}, use_extra_client: {self.use_extra_client}") + + while not self.vipc_client_main.connect(False): + time.sleep(0.1) + while self.use_extra_client and not self.vipc_client_extra.connect(False): + time.sleep(0.1) + + cloudlog.warning(f"connected main cam with buffer size: {self.vipc_client_main.buffer_len} \ + ({self.vipc_client_main.width} x {self.vipc_client_main.height})") + if self.use_extra_client: + cloudlog.warning(f"connected extra cam with buffer size: {self.vipc_client_extra.buffer_len} \ + ({self.vipc_client_extra.width} x {self.vipc_client_extra.height})") + + def receive_frames(self): + buf_main, buf_extra = None, None + meta_main = FrameMeta() + meta_extra = FrameMeta() + # Keep receiving frames until we are at least 1 frame ahead of previous extra frame + while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: + buf_main = self.vipc_client_main.recv() + meta_main = FrameMeta(self.vipc_client_main) + if buf_main is None: + break + + if buf_main is None: + cloudlog.error("vipc_client_main no frame") + return None + + if self.use_extra_client: + # Keep receiving extra frames until frame id matches main camera + while True: + buf_extra = self.vipc_client_extra.recv() + meta_extra = FrameMeta(self.vipc_client_extra) + if buf_extra is None or meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: + break + + if buf_extra is None: + cloudlog.error("vipc_client_extra no frame") + return None + + if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000: + cloudlog.error("frames out of sync! main: {} ({:.5f}), extra: {} ({:.5f})".format( + meta_main.frame_id, meta_main.timestamp_sof / 1e9, + meta_extra.frame_id, meta_extra.timestamp_sof / 1e9)) + + else: + # Use single camera + buf_extra = buf_main + meta_extra = meta_main + return meta_main, meta_extra, buf_main, buf_extra + + def main(demo=False): sentry.set_tag("daemon", PROCESS_NAME) cloudlog.bind(daemon=PROCESS_NAME) setproctitle(PROCESS_NAME) config_realtime_process(7, 54) + cl_context = CLContext() model = ModelState(cl_context) cloudlog.warning("models loaded, modeld starting") - - # visionipc clients - while True: - available_streams = VisionIpcClient.available_streams("camerad", block=False) - if available_streams: - use_extra_client = VisionStreamType.VISION_STREAM_WIDE_ROAD in available_streams and VisionStreamType.VISION_STREAM_ROAD in available_streams - main_wide_camera = VisionStreamType.VISION_STREAM_ROAD not in available_streams - break - time.sleep(.1) - - vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_wide_camera else VisionStreamType.VISION_STREAM_ROAD - vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True, cl_context) - vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, cl_context) - cloudlog.warning(f"vision stream set up, main_wide_camera: {main_wide_camera}, use_extra_client: {use_extra_client}") - - while not vipc_client_main.connect(False): - time.sleep(0.1) - while use_extra_client and not vipc_client_extra.connect(False): - time.sleep(0.1) - - cloudlog.warning(f"connected main cam with buffer size: {vipc_client_main.buffer_len} ({vipc_client_main.width} x {vipc_client_main.height})") - if use_extra_client: - cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") + camera_streams = CameraStreams(cl_context) # messaging pm = PubMaster(["modelV2", "cameraOdometry"]) @@ -166,9 +212,6 @@ def main(demo=False): live_calib_seen = False nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32) nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32) - buf_main, buf_extra = None, None - meta_main = FrameMeta() - meta_extra = FrameMeta() if demo: @@ -182,40 +225,11 @@ def main(demo=False): while True: - # Keep receiving frames until we are at least 1 frame ahead of previous extra frame - while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: - buf_main = vipc_client_main.recv() - meta_main = FrameMeta(vipc_client_main) - if buf_main is None: - break - - if buf_main is None: - cloudlog.error("vipc_client_main no frame") - continue - - if use_extra_client: - # Keep receiving extra frames until frame id matches main camera - while True: - buf_extra = vipc_client_extra.recv() - meta_extra = FrameMeta(vipc_client_extra) - if buf_extra is None or meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: - break - - if buf_extra is None: - cloudlog.error("vipc_client_extra no frame") - continue - - if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000: - cloudlog.error("frames out of sync! main: {} ({:.5f}), extra: {} ({:.5f})".format( - meta_main.frame_id, meta_main.timestamp_sof / 1e9, - meta_extra.frame_id, meta_extra.timestamp_sof / 1e9)) - - else: - # Use single camera - buf_extra = buf_main - meta_extra = meta_main + frames = None + while frames is None: + frames = camera_streams.receive_frames() + meta_main, meta_extra, buf_main, buf_extra = frames - # TODO: path planner timeout? sm.update(0) desire = DH.desire v_ego = sm["carState"].vEgo @@ -223,7 +237,7 @@ def main(demo=False): frame_id = sm["roadCameraState"].frameId if sm.updated["liveCalibration"]: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) - model_transform_main = get_warp_matrix(device_from_calib_euler, main_wide_camera, False).astype(np.float32) + model_transform_main = get_warp_matrix(device_from_calib_euler, camera_streams.main_wide_camera, False).astype(np.float32) model_transform_extra = get_warp_matrix(device_from_calib_euler, True, True).astype(np.float32) live_calib_seen = True