Performance improvements to the gripper and head cameras

stretch4_body/robot/robot_params_SE4.py

@@ -1359,7 +1359,7 @@
                 "image_size": (1200, 1920),
                 "fps": 30,
                 "rotate_number_of_times": 1,
-                "buffer_size": 2,
+                "buffer_size": 1,
                 "is_compressed": False,
                 "is_lossless": False, # Only used if is_compressed is true
                 "jpeg_quality": 90, # Only used if is_compressed is true and is_lossless is False
@@ -1377,7 +1377,7 @@
                 "image_size": (1200, 1920),
                 "fps": 30,
                 "rotate_number_of_times": -1,
-                "buffer_size": 2,
+                "buffer_size": 1,
                 "is_compressed": False,
                 "is_lossless": False, # Only used if is_compressed is true
                 "jpeg_quality": 90, # Only used if is_compressed is true and is_lossless is False
@@ -1396,7 +1396,7 @@
                 "image_size": (3040, 4032),  # Almost full 12MP resolution, 24 pixels subtracted to be divisible by 16 for compression
                 "fps": 10,
                 "rotate_number_of_times": -1,
-                "buffer_size": 2,
+                "buffer_size": 1,
                 "is_compressed": False,
                 "is_lossless": False, # Only used if is_compressed is true
                 "jpeg_quality": 90, # Only used if is_compressed is true and is_lossless is False
@@ -1416,15 +1416,17 @@
                 # "image_size": (800, 1280),
                 "fps": 30,
                 "rotate_number_of_times": 0,
-                "buffer_size": 2,
-                "is_compressed": False,
+                "buffer_size": 1,
+                "is_compressed": True,
                 "is_lossless": False, # Only used if is_compressed is true
-                "jpeg_quality": 90, # Only used if is_compressed is true and is_lossless is False
+                "jpeg_quality": 80, # Only used if is_compressed is true and is_lossless is False
                 "distortion_model": None,
                 "use_auto_exposure": True,
                 "limit_max": None, # Only used if use_auto_exposure is True
                 "exposure_time": None, # Only used if use_auto_exposure is False
-                "iso": None # Only used if use_auto_exposure is False
+                "iso": None, # Only used if use_auto_exposure is False
+                "sync_threshold_ms": 15.0,
+                "stereo_max_range_mm": 10000.0
             }
         },
         'gripper_right': {
@@ -1435,15 +1437,17 @@
                 # "image_size": (800, 1280),
                 "fps": 30,
                 "rotate_number_of_times": 0,
-                "buffer_size": 2,
-                "is_compressed": False,
+                "buffer_size": 1,
+                "is_compressed": True,
                 "is_lossless": False, # Only used if is_compressed is true
-                "jpeg_quality": 90, # Only used if is_compressed is true and is_lossless is False
+                "jpeg_quality": 80, # Only used if is_compressed is true and is_lossless is False
                 "distortion_model": None,
                 "use_auto_exposure": True,
                 "limit_max": None, # Only used if use_auto_exposure is True
                 "exposure_time": None, # Only used if use_auto_exposure is False
-                "iso": None # Only used if use_auto_exposure is False
+                "iso": None, # Only used if use_auto_exposure is False
+                "sync_threshold_ms": 15.0,
+                "stereo_max_range_mm": 10000.0
             }
         },
     },

stretch4_body/subsystem/cameras/__init__.py

@@ -19,5 +19,6 @@
     stream_right_rgbd,
     stream_center_rgbd,
     stream_left_right_rgbd,
-    stream_left_right_center_rgbd
+    stream_left_right_center_rgbd,
+    stream_gripper_rgbd
 )

stretch4_body/subsystem/cameras/adapters/luxonis_camera_adapter.py

@@ -108,7 +108,7 @@ def get_depthai_camera_socket(camera_type: RGBCameras):
         raise Exception(f"{camera_type} is not supported as a Luxonis device.")
 
     @staticmethod
-    def create_camera_node(pipeline: dai.Pipeline, camera_config: RGBCameraConfig) -> tuple[dai.node.Camera, dai.Node.Output]:
+    def create_camera_node(pipeline: dai.Pipeline, camera_config: RGBCameraConfig) -> tuple[dai.node.Camera, dai.Node.Output, dai.Node.Output|None]:
         """
         Takes a dai.Pipeline reference and adds a camera node to it.
         """
@@ -144,6 +144,7 @@ def create_camera_node(pipeline: dai.Pipeline, camera_config: RGBCameraConfig) -
         )
 
         print("camera_config:", camera_config)
+        camera_output_compressed = None
         if camera_config.is_compressed:
             videoEncoder = pipeline.create(dai.node.VideoEncoder)
             videoEncoder.setNumFramesPool(buffer_size)
@@ -154,9 +155,9 @@ def create_camera_node(pipeline: dai.Pipeline, camera_config: RGBCameraConfig) -
                 lossless=camera_config.is_lossless, 
                 quality=camera_config.jpeg_quality
             )
-            camera_output = videoEncoder.bitstream
+            camera_output_compressed = videoEncoder.bitstream
 
-        return node, camera_output
+        return node, camera_output, camera_output_compressed
 
     @staticmethod
     def create_pipeline(luxonis_device_product_name:str) -> tuple[dai.Pipeline, dai.Device]:
@@ -171,25 +172,6 @@ def create_pipeline(luxonis_device_product_name:str) -> tuple[dai.Pipeline, dai.
         pipeline.setXLinkChunkSize(0)
 
         return pipeline, device
-
-    @staticmethod
-    def create_rgbd_node(pipeline: dai.Pipeline, left_rgb_output: dai.Node.Output, right_rgb_out: dai.Node.Output):
-
-        stereo = pipeline.create(dai.node.StereoDepth).build(left_rgb_output, right_rgb_out)
-        rgbd = pipeline.create(dai.node.RGBD)
-
-        stereo.setRectifyEdgeFillColor(0)
-        stereo.enableDistortionCorrection(True)
-        # https://docs.luxonis.com/software-v3/depthai/depthai-components/nodes/stereo_depth
-        stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.ROBOTICS)
-        stereo.initialConfig.postProcessing.thresholdFilter.maxRange = 1000 * 5 # in mm
-        rgbd.setDepthUnits(dai.StereoDepthConfig.AlgorithmControl.DepthUnit.METER)
-
-        stereo.syncedLeft.link(rgbd.inColor)
-        stereo.depth.link(rgbd.inDepth)
-        left_rgb_output.link(stereo.inputAlignTo)
-
-        return stereo, rgbd
 
     def is_open(self):
         return self.pipeline is not None and self.device is not None and self.pipeline.isRunning() and not self.device.isClosed()
@@ -198,11 +180,13 @@ def open(self):
         self.pipeline, self.device = LuxonisCameraAdapter.create_pipeline(self.camera_config.camera_device)
         self.camera = self.pipeline
 
-        self.camera_node, node_output = LuxonisCameraAdapter.create_camera_node(
+        self.camera_node, node_output, node_output_compressed = LuxonisCameraAdapter.create_camera_node(
             pipeline=self.pipeline, camera_config=self.camera_config
         )
 
-        self.output_queue = node_output.createOutputQueue(maxSize=1)
+        output_node = node_output_compressed if node_output_compressed is not None else node_output
+
+        self.output_queue = output_node.createOutputQueue(maxSize=1, blocking=False)
         self.input_queue = self.camera_node.inputControl.createInputQueue()
 
         try:
@@ -256,11 +240,10 @@ def get_frame_from_output_queue(
 
     @staticmethod
     def get_frame_from_output_queue_no_block(
-        output_queue: dai.MessageQueue,
-        timeout: float = 1/120
+        output_queue: dai.MessageQueue
     ):
         while True:
-            message: dai.ImgFrame | None = output_queue.get(timeout=timeout)
+            message: dai.ImgFrame | None = output_queue.tryGet()
             if message:
                 yield LuxonisCameraAdapter.dai_message_to_image_frame(message)
             else:

stretch4_body/subsystem/cameras/adapters/luxonis_gripper_camera_adapter.py

@@ -9,13 +9,17 @@
 from stretch4_body.subsystem.cameras.enums.rgb_camera import RGBCameraConfig, RGBCameras
 from stretch4_body.subsystem.cameras.adapters.luxonis_camera_adapter import LuxonisCameraAdapter, clear_device_cache
 from stretch4_body.subsystem.cameras.adapters.synced_camera import SyncedCamera
-from stretch4_body.subsystem.cameras.models.image_frame import SyncedImageFrame
+from stretch4_body.subsystem.cameras.models.image_frame import SyncedImageFrame, ImageFrame
+import dataclasses
+import numpy as np
+import datetime
 
 
 class GripperCameraLuxonis(SyncedCamera):
     """Start a stream with the gripper left/right stereo cameras and the point cloud pipeline."""
-    def __init__(self, left: RGBCameraConfig, right: RGBCameraConfig):
+    def __init__(self, left: RGBCameraConfig, right: RGBCameraConfig, enable_pointcloud: bool = False):
         self.do_sync_frames = True
+        self.enable_pointcloud = enable_pointcloud
 
         self.left = left
         self.right = right
@@ -26,15 +30,54 @@ def __init__(self, left: RGBCameraConfig, right: RGBCameraConfig):
         self.pipeline, self.device = LuxonisCameraAdapter.create_pipeline(left.camera_device)
         self.camera = self.pipeline
 
-        self.left_camera_node, node_left = LuxonisCameraAdapter.create_camera_node(pipeline=self.pipeline, camera_config=left)
-        self.right_camera_node, node_right = LuxonisCameraAdapter.create_camera_node(pipeline=self.pipeline, camera_config=right)
+        self.left_camera_node, node_left, node_left_compressed = LuxonisCameraAdapter.create_camera_node(pipeline=self.pipeline, camera_config=left)
+        self.right_camera_node, node_right, node_right_compressed = LuxonisCameraAdapter.create_camera_node(pipeline=self.pipeline, camera_config=right)
 
-        stereo, rgbd = LuxonisCameraAdapter.create_rgbd_node(self.pipeline, node_left, node_right)
+        stereo = self.pipeline.create(dai.node.StereoDepth)
+        # stereo.setRectifyEdgeFillColor(0)
+        # stereo.enableDistortionCorrection(True)
+        # https://docs.luxonis.com/software-v3/depthai/depthai-components/nodes/stereo_depth
+        stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.ROBOTICS)
+        stereo.setDepthAlign(LuxonisCameraAdapter.get_depthai_camera_socket(self.right.camera_type)) # Align to right camera
+        stereo.initialConfig.postProcessing.thresholdFilter.maxRange = int(self.right.stereo_max_range_mm)
 
-        self.left_output = stereo.syncedLeft.createOutputQueue(maxSize=1)
-        self.right_output =  stereo.syncedRight.createOutputQueue(maxSize=1)
-        self.depth_output = stereo.depth.createOutputQueue(maxSize=1)
-        self.pointcloud_output = rgbd.pcl.createOutputQueue(maxSize=1)
+        node_left.link(stereo.left)
+        node_right.link(stereo.right)
+
+        sync = self.pipeline.create(dai.node.Sync)
+        sync.setSyncThreshold(datetime.timedelta(milliseconds=int(self.right.sync_threshold_ms)))
+
+        output_node_left = node_left_compressed if node_left_compressed is not None else node_left
+        output_node_right = node_right_compressed if node_right_compressed is not None else node_right
+
+        sync.inputs["left"].setMaxSize(self.left.buffer_size)
+        sync.inputs["left"].setBlocking(False)
+        output_node_left.link(sync.inputs["left"])
+
+        sync.inputs["right"].setMaxSize(self.right.buffer_size)
+        sync.inputs["right"].setBlocking(False)
+        output_node_right.link(sync.inputs["right"])
+
+        sync.inputs["depth"].setMaxSize(self.right.buffer_size)
+        sync.inputs["depth"].setBlocking(False)
+        stereo.depth.link(sync.inputs["depth"])
+
+        if self.enable_pointcloud:
+            rgbd = self.pipeline.create(dai.node.RGBD)
+            rgbd.setDepthUnits(dai.StereoDepthConfig.AlgorithmControl.DepthUnit.METER)
+
+
+            node_left.link(rgbd.inColor)
+            stereo.depth.link(rgbd.inDepth)
+            sync.inputs["pointcloud"].setBlocking(False)
+            rgbd.pcl.link(sync.inputs["pointcloud"])
+            sync.inputs["pointcloud"].setMaxSize(self.right.buffer_size)
+            node_left.link(stereo.inputAlignTo)
+            # self.right_output = node_right.createOutputQueue(maxSize=self.right.buffer_size, blocking=False)
+            # self.depth_output = stereo.depth.createOutputQueue(maxSize=self.right.buffer_size, blocking=False)
+            # self.pointcloud_output = rgbd.pcl.createOutputQueue(maxSize=self.right.buffer_size, blocking=False)
+
+        self.q_sync = sync.out.createOutputQueue(maxSize=self.right.buffer_size, blocking=False)
 
         self.left_input_queue = self.left_camera_node.inputControl.createInputQueue()
         self.right_input_queue = self.right_camera_node.inputControl.createInputQueue()
@@ -44,25 +87,67 @@ def __init__(self, left: RGBCameraConfig, right: RGBCameraConfig):
         except Exception:
             clear_device_cache()
             raise
+
+    def get_gripper_intrinsics(self, camera_type: RGBCameras):
+        """Returns M and D from the hardware factory calibration if available."""
+        try:
+            calib = self.device.readCalibration()
+            M = np.array(calib.getCameraIntrinsics(LuxonisCameraAdapter.get_depthai_camera_socket(camera_type), camera_type.config.image_size[0], camera_type.config.image_size[1]), dtype=np.float64)
+            D = np.array(calib.getDistortionCoefficients(LuxonisCameraAdapter.get_depthai_camera_socket(camera_type)), dtype=np.float64)
+            return M, D
+        except Exception as e:
+            print(f"Warning: could not read calibration from OAK-D: {e}")
+        return None, None
 
     def is_open(self):
         return self.pipeline is not None and self.device is not None and self.pipeline.isRunning() and not self.device.isClosed()
 
     def get_frames(self):
         if not self.is_open():
             raise RuntimeError("Camera is not running.")
+
+        empty_left_or_right_frame = ImageFrame(image=np.zeros((self.left.image_size[1], self.left.image_size[0], 3), dtype=np.uint8), timestamp=0, frame_number=0)
 
         while True:
-
-            left_callback = next(LuxonisCameraAdapter.get_frame_from_output_queue(self.left_output))
-            right_callback = next(LuxonisCameraAdapter.get_frame_from_output_queue(self.right_output))
-            depth_callback = next(LuxonisCameraAdapter.get_frame_from_output_queue(self.depth_output))
-
-            points, points_rgb, points_sequence_number = next(LuxonisCameraAdapter.get_pointcloud_from_output_queue(self.pointcloud_output))
-
-            synced_image = SyncedImageFrame(timestamp=left_callback.timestamp, left=left_callback, right=right_callback, center=None, pointcloud=points, pointcloud_color=points_rgb, depth=depth_callback.image)
-
-            yield synced_image
+            msgGroup = self.q_sync.get()
+            if msgGroup is not None:
+                msgNames = msgGroup.getMessageNames()
+                frame_left_msg = msgGroup["left"] if "left" in msgNames else None
+                frame_right_msg = msgGroup["right"] if "right" in msgNames else None
+                frame_depth_msg = msgGroup["depth"] if "depth" in msgNames else None
+                frame_pointcloud_msg = msgGroup["pointcloud"] if "pointcloud" in msgNames else None
+
+                pointcloud = None
+                pointcloud_color = None
+                if frame_pointcloud_msg:
+                    pointcloud, pointcloud_color = frame_pointcloud_msg.getPointsRGB()
+
+                if frame_depth_msg:
+                    timestamp = frame_depth_msg.getTimestamp().total_seconds()
+                    sequence_num = frame_depth_msg.getSequenceNum()
+                    if frame_left_msg:
+                        left_frame = LuxonisCameraAdapter.dai_message_to_image_frame(frame_left_msg)
+                    else:
+                        left_frame = empty_left_or_right_frame
+                    if frame_right_msg:
+                        right_frame = LuxonisCameraAdapter.dai_message_to_image_frame(frame_right_msg)
+                    else:
+                        right_frame = empty_left_or_right_frame
+
+                    depth_frame = frame_depth_msg.getFrame()
+
+                    synced_image = SyncedImageFrame(
+                        timestamp=timestamp,
+                        left=left_frame,
+                        right=right_frame,
+                        center=None,
+                        depth=depth_frame,
+                        pointcloud=pointcloud,
+                        pointcloud_color=pointcloud_color
+                    )
+                    yield synced_image
+                else:
+                    print("No depth frame received.")
 
     def stop(self):
         self.pipeline.stop()