diff --git a/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_rgb_camera.py b/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_rgb_camera.py index 17b11a1..290d207 100644 --- a/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_rgb_camera.py +++ b/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_rgb_camera.py @@ -60,6 +60,7 @@ def __init__( camera_cls: type, camera_kwargs: dict = {}, shared_memory_namespace: str = "camera", + log_debug: bool = False, ): """Instantiates the publisher. Note that the publisher (and its process) will not start until start() is called. @@ -77,6 +78,7 @@ def __init__( self._camera_cls = camera_cls self._camera_kwargs = camera_kwargs self._camera = None + self.log_debug = log_debug self.running_event = multiprocessing.Event() self.shutdown_event = multiprocessing.Event() diff --git a/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_rgbd_camera.py b/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_rgbd_camera.py index 9ad1248..7856475 100644 --- a/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_rgbd_camera.py +++ b/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_rgbd_camera.py @@ -42,8 +42,9 @@ def __init__( camera_cls: type, camera_kwargs: dict = {}, shared_memory_namespace: str = "camera", + log_debug: bool = False, ): - super().__init__(camera_cls, camera_kwargs, shared_memory_namespace) + super().__init__(camera_cls, camera_kwargs, shared_memory_namespace, log_debug) self.depth_shm: Optional[shared_memory.SharedMemory] = None self.depth_shape_shm: Optional[shared_memory.SharedMemory] = None diff --git a/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_stereo_rgbd_camera.py b/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_stereo_rgbd_camera.py index 85cec97..4b9b5df 100644 --- a/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_stereo_rgbd_camera.py +++ b/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_stereo_rgbd_camera.py @@ -37,8 +37,9 @@ def __init__( camera_cls: type, camera_kwargs: dict = {}, shared_memory_namespace: str = "camera", + log_debug: bool = False, ): - super().__init__(camera_cls, camera_kwargs, shared_memory_namespace) + super().__init__(camera_cls, camera_kwargs, shared_memory_namespace, log_debug) self.rgb_right_shm: Optional[shared_memory.SharedMemory] = None self.rgb_right_shape_shm: Optional[shared_memory.SharedMemory] = None @@ -118,7 +119,7 @@ def run(self) -> None: timestamp_publish = time.time() self.timestamp_shm_array[0] = timestamp_publish - if timestamp_prev_publish is not None: + if timestamp_prev_publish is not None and self.log_debug: publish_period = timestamp_publish - timestamp_prev_publish if publish_period > 1.1 * self.camera_period: logger.warning( @@ -131,11 +132,12 @@ def run(self) -> None: self.write_lock_shm_array[0] = False time_shm_write_end = time.time() - logger.debug( - f"Retrieval time: {time_retreive_end - time_retreive_start:.3f} s, (grab time: {time_grab_end - time_retreive_start:.3f} s)," - f"Lock time: {time_lock_end - time_lock_start:.3f} s, " - f"SHM write time: {time_shm_write_end - time_shm_write_start:.3f} s" - ) + if self.log_debug: + logger.debug( + f"Retrieval time: {time_retreive_end - time_retreive_start:.3f} s, (grab time: {time_grab_end - time_retreive_start:.3f} s)," + f"Lock time: {time_lock_end - time_lock_start:.3f} s, " + f"SHM write time: {time_shm_write_end - time_shm_write_start:.3f} s" + ) self.running_event.set() except Exception as e: @@ -287,7 +289,6 @@ def __del__(self) -> None: camera_fps = 15 # import os - # os.environ["CUDA_VISIBLE_DEVICES"] = "3" publisher = MultiprocessStereoRGBDPublisher( @@ -297,6 +298,7 @@ def __del__(self) -> None: "fps": camera_fps, "depth_mode": Zed2i.NEURAL_DEPTH_MODE, }, + log_debug=True, ) publisher.start() @@ -329,9 +331,9 @@ def __del__(self) -> None: image = receiver.get_rgb_image_as_int() image_right = receiver._retrieve_rgb_image_as_int(view=StereoRGBDCamera.RIGHT_RGB) depth_map = receiver._retrieve_depth_map() - # depth_image = receiver._retrieve_depth_image() + depth_image = receiver._retrieve_depth_image() confidence_map = receiver._retrieve_confidence_map() - # point_cloud = receiver._retrieve_colored_point_cloud() + point_cloud = receiver._retrieve_colored_point_cloud() image_bgr = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) image_right_bgr = cv2.cvtColor(image_right, cv2.COLOR_RGB2BGR) @@ -339,11 +341,11 @@ def __del__(self) -> None: cv2.imshow("RGB Image", image_bgr) cv2.imshow("RGB Image Right", image_right_bgr) cv2.imshow("Depth Map", depth_map) - # cv2.imshow("Depth Image", depth_image) + cv2.imshow("Depth Image", depth_image) cv2.imshow("Confidence Map", confidence_map) - # if log_point_cloud: - # rr.log("point_cloud", rr.Points3D(positions=point_cloud.points, colors=point_cloud.colors)) + if log_point_cloud: + rr.log("point_cloud", rr.Points3D(positions=point_cloud.points, colors=point_cloud.colors)) key = cv2.waitKey(10) if key == ord("q"): @@ -351,16 +353,6 @@ def __del__(self) -> None: elif key == ord("l"): log_point_cloud = not log_point_cloud - # if time_previous is not None: - # fps = 1 / (time_current - time_previous) - - # fps_str = f"{fps:.2f}".rjust(6, " ") - # camera_fps_str = f"{camera_fps:.2f}".rjust(6, " ") - # if fps < 0.9 * camera_fps: - # logger.warning(f"FPS: {fps_str} / {camera_fps_str} (too slow)") - # else: - # logger.debug(f"FPS: {fps_str} / {camera_fps_str}") - receiver._close_shared_memory() publisher.stop() publisher.join() diff --git a/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_video_recorder.py b/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_video_recorder.py index 874a71a..b98c638 100644 --- a/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_video_recorder.py +++ b/airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_video_recorder.py @@ -17,7 +17,6 @@ def __init__( shared_memory_namespace: str, video_path: Optional[str] = None, image_transform: Optional[ImageTransform] = None, - log_fps: bool = False, ): super().__init__(daemon=True) self._shared_memory_namespace = shared_memory_namespace @@ -36,7 +35,6 @@ def __init__( video_path = os.path.abspath(video_path) self._video_path = video_path - self.log_fps = log_fps def start(self) -> None: super().start() @@ -57,46 +55,59 @@ def run(self) -> None: logger.info(f"Recording video to {self._video_path}") timestamp_prev_frame = None + image_previous = receiver.get_rgb_image_as_int() + timestamp_prev_frame = receiver.get_current_timestamp() + video_writer.write(cv2.cvtColor(image_previous, cv2.COLOR_RGB2BGR)) + self.recording_started_event.set() + n_consecutive_frames_dropped = 0 while not self.shutdown_event.is_set(): - image_rgb = receiver.get_rgb_image_as_int() - timestamp_current_frame = receiver.get_current_timestamp() + timestamp_receiver = receiver.get_current_timestamp() - if timestamp_prev_frame is not None: - # This method of detecting dropped frames is better than simply checking FPS - timestamp_difference = timestamp_current_frame - timestamp_prev_frame - if timestamp_difference >= 1.2 * camera_period: + # 1. wait until new timestamp is available, but if it takes > 2*camera_period, log a warning + if timestamp_receiver == timestamp_prev_frame: + # if receiver timestamp is not updated, check if it's been too long since last frame + timestamp_current = time.time() + timestamp_difference = timestamp_current - timestamp_prev_frame + + if timestamp_difference >= 2.0 * camera_period: + n_consecutive_frames_dropped += 1 logger.warning( - f"Timestamp difference with previous frame: {timestamp_difference:.3f} s (Frame dropped?)" + f"No frame received within {2.0 * camera_period:.3f} s, repeating previous frame in video (n = {n_consecutive_frames_dropped})." ) + image_rgb = image_previous + timestamp_prev_frame += camera_period # pretend that the frame was received else: - logger.debug(f"Timestamp difference with previous frame: {timestamp_difference:.3f} s") - - timestamp_prev_frame = timestamp_current_frame + continue # wait a bit longer before taking action + else: + # new frame arrived + if n_consecutive_frames_dropped > 0: + logger.info(f"New frame received after missing {n_consecutive_frames_dropped} frames.") + n_consecutive_frames_dropped = 0 + image_rgb = receiver._retrieve_rgb_image_as_int() + timestamp_prev_frame = timestamp_receiver image = cv2.cvtColor(image_rgb, cv2.COLOR_RGB2BGR) - # # Known bug: vertical images still give horizontal videos + # Known bug: vertical images still give horizontal videos if self._image_transform is not None: image = self._image_transform.transform_image(image) video_writer.write(image) - self.recording_started_event.set() video_writer.release() - logger.info(f"Video saved to {self._video_path}") - self.recording_finished_event.set() def stop(self) -> None: self.shutdown_event.set() + logger.info("Set video recording shutdown event.") self.recording_finished_event.wait() if __name__ == "__main__": """Records 10 seconds of video. Assumes there's being published to the "camera" namespace.""" - recorder = MultiprocessVideoRecorder("camera", log_fps=True) + recorder = MultiprocessVideoRecorder("camera") recorder.start() time.sleep(10) recorder.stop() diff --git a/airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py b/airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py index d69120d..b3c538a 100644 --- a/airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py +++ b/airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py @@ -223,7 +223,7 @@ def _retrieve_rgb_image_as_int(self, view: str = StereoRGBDCamera.LEFT_RGB) -> N assert view in StereoRGBDCamera._VIEWS if view == StereoRGBDCamera.RIGHT_RGB: self.camera.retrieve_image(self.image_matrix_right, sl.VIEW.RIGHT) - image_bgra: OpenCVIntImageType = self.image_matrix.get_data() + image_bgra: OpenCVIntImageType = self.image_matrix_right.get_data() else: self.camera.retrieve_image(self.image_matrix, sl.VIEW.LEFT) image_bgra: OpenCVIntImageType = self.image_matrix.get_data()