fix right image bug and make logging optional

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()
 

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

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,38 +331,28 @@ 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)
 
         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"):
             break
         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()

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()

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()