framesource 0.3.0__py3-none-any.whl

framesource/sources/realsense_capture.py

@@ -0,0 +1,565 @@
+import logging
+import warnings
+from typing import Any, Optional
+
+import cv2
+import numpy as np
+
+from ..errors import MissingDependencyError
+from ..processors.realsense_depth_processor import (
+    RealsenseDepthProcessor,
+    RealsenseProcessingOutput,
+)
+from .video_capture_base import VideoCaptureBase
+
+logger = logging.getLogger(__name__)
+
+
+class RealsenseCapture(VideoCaptureBase):
+    has_discovery = True
+    supports_exposure = True
+    supports_gain = True
+    supports_depth = True
+
+    def _read_implementation(self) -> tuple[bool, Optional[np.ndarray]]:
+        """
+        Read a single frame from the realsense camera.
+        Returns:
+            Tuple[bool, Optional[np.ndarray]]: (success, frame)
+        """
+        if not self.is_connected or self.pipeline is None:
+            return False, None
+
+        try:
+            # Wait for a coherent pair of frames: depth and color
+            frames = self.pipeline.wait_for_frames()
+            aligned = self._align.process(frames)
+
+            depth_frame = frames.get_depth_frame()
+            color_frame = frames.get_color_frame()
+            aligned_depth_frame = aligned.get_depth_frame()
+            aligned_color_frame = aligned.get_color_frame()
+
+            if not depth_frame or not color_frame:
+                return False, None
+        except RuntimeError as e:
+            # Handle case where pipeline has been stopped
+            if "wait_for_frames cannot be called before start()" in str(e):
+                return False, None
+            else:
+                raise  # Re-raise other runtime errors
+
+        # if depth_frame and color_frame:
+        #     print("Depth Intrinsics:", depth_frame.profile.as_video_stream_profile().get_intrinsics())  # noqa: E501
+        #     print("Color Intrinsics:", color_frame.profile.as_video_stream_profile().get_intrinsics())  # noqa: E501
+
+        frame_dict = {
+            "raw_color": color_frame,
+            "raw_depth": depth_frame,
+            "aligned_depth": aligned_depth_frame,
+            "aligned_color": aligned_color_frame,
+        }
+
+        return True, frame_dict
+
+    """Realsense camera capture using Realsense lib."""
+
+    def __init__(
+        self,
+        source: int = 0,
+        *,
+        width: Optional[int] = None,
+        height: Optional[int] = None,
+        fps: Optional[float] = None,
+        processor: Optional[Any] = None,
+        **kwargs,
+    ):
+        """Initialize the RealSense capture.
+
+        Args:
+            source: Camera serial number (str) or device index (int,
+                default: 0).
+            width: Desired color-stream width in pixels. Falls back to the
+                device's maximum supported width when omitted (default:
+                None). Applied on :meth:`connect`.
+            height: Desired color-stream height in pixels. Falls back to the
+                device's maximum supported height when omitted (default:
+                None). Applied on :meth:`connect`.
+            fps: Desired stream frame rate. Falls back to the device's
+                maximum supported frame rate when omitted (default: None).
+                Applied on :meth:`connect`.
+            processor: Frame processor attached at construction time.
+                Defaults to a
+                :class:`~framesource.processors.realsense_depth_processor.RealsenseDepthProcessor`
+                configured for RGB output when not provided.
+            **kwargs: Additional passthrough options stored on ``self.config``.
+        """
+        # Preserve the historical ``self.config`` contents: only forward
+        # explicitly-provided values so the ``self.config.get(...)`` defaults
+        # below keep their old meaning.
+        for _key, _val in (
+            ("width", width),
+            ("height", height),
+            ("fps", fps),
+            ("processor", processor),
+        ):
+            if _val is not None:
+                kwargs[_key] = _val
+        super().__init__(source, **kwargs)
+        self.pipeline = None
+        self.device = None
+        self.profile = None
+        self._align = None
+
+        self.w = self.config.get("width", 0)
+        self.h = self.config.get("height", 0)
+        self.fps = self.config.get("fps", 0)
+
+        self._max_width = 0
+        self._max_height = 0
+        self._max_fps = 0
+
+        self._default_processor = self.config.get("processor", None)
+        if self._default_processor is None:
+            self._default_processor = RealsenseDepthProcessor(
+                output_format=RealsenseProcessingOutput.RGB
+            )
+        self.attach_processor(self._default_processor)
+
+        self.source = source if isinstance(source, int) else 0
+
+        if "is_mono" in kwargs:
+            logger.warning(
+                "'is_mono' argument is only used for certain industrial cameras "
+                "and has no effect for realsense camera."
+            )
+
+    def connect(self) -> bool:
+        """Connect to realsense camera."""
+        try:
+            import pyrealsense2 as rs
+        except ImportError as e:
+            raise MissingDependencyError("pyrealsense2", extra="realsense", details=str(e)) from e
+
+        try:
+            # Configure depth and color streams
+            self.pipeline = rs.pipeline()
+            self._align = rs.align(rs.stream.color)
+            config = rs.config()
+
+            serial_number = self.source if isinstance(self.source, str) else None
+            device_index = self.source if isinstance(self.source, int) else 0
+
+            if serial_number:
+                config.enable_device(serial_number)
+            elif device_index is not None:
+                ctx = rs.context()
+                devices = ctx.query_devices()
+
+                if device_index < 0 or device_index >= len(devices):
+                    raise ValueError(f"Invalid device index: {device_index}")
+                selected_serial = devices[device_index].get_info(rs.camera_info.serial_number)
+                config.enable_device(selected_serial)
+
+            # Get device product line for setting a supporting resolution
+            pipeline_wrapper = rs.pipeline_wrapper(self.pipeline)
+            pipeline_profile = config.resolve(pipeline_wrapper)
+            self.device: rs.device = pipeline_profile.get_device()
+
+            serial_number = self.device.get_info(rs.camera_info.serial_number)
+            device_name = self.device.get_info(rs.camera_info.name)
+            device_product_line = str(self.device.get_info(rs.camera_info.product_line))
+
+            found_rgb = False
+            max_res = (0, 0, 0)
+            for s in self.device.query_sensors():
+                if any(p.stream_type() == rs.stream.color for p in s.get_stream_profiles()):
+                    found_rgb = True
+                for profile in s.get_stream_profiles():
+                    if profile.stream_type() == rs.stream.color:
+                        vp = profile.as_video_stream_profile()
+                        res = (vp.width(), vp.height(), vp.fps())
+                        if res > max_res:
+                            max_res = res
+            if not found_rgb:
+                print("The demo requires Depth camera with Color sensor")
+                exit(0)
+
+            self._max_width = max_res[0]
+            self._max_height = max_res[1]
+            self._max_fps = max_res[2]
+
+            width = self.w if self.w > 0 else self._max_width
+            height = self.h if self.h > 0 else self._max_height
+            fps = self.fps if self.fps > 0 else self._max_fps
+
+            # Enable depth stream without specifying resolution - let it auto-select
+            # (depth sensor often has different max resolution than color sensor)
+            config.enable_stream(rs.stream.depth, rs.format.z16, int(fps))
+            config.enable_stream(rs.stream.color, width, height, rs.format.bgr8, int(fps))
+
+            # Start streaming
+            self.profile = self.pipeline.start(config)
+
+            self.is_connected = True
+            logger.info(
+                f"Connected to realsense camera {self.source}, {device_name} "
+                f"({device_product_line} line) (Serial: {serial_number})"
+            )
+            return True
+        except Exception as e:
+            logger.error(f"Error connecting to realsense camera: {e}")
+            return False
+
+    def disconnect(self) -> bool:
+        """Disconnect from realsense camera."""
+        try:
+            if self.pipeline is not None:
+                self.pipeline.stop()
+            self.is_connected = False
+            logger.info("Disconnected from realsense camera")
+            return True
+        except Exception as e:
+            logger.error(f"Error disconnecting from realsense camera: {e}")
+            return False
+
+    def set_exposure(self, value: float) -> bool:
+        """Set exposure (1.0 to 165000 for most realsense cameras)."""
+        if not self.is_connected or self.pipeline is None:
+            return False
+
+        try:
+            self._exposure = value
+            import pyrealsense2 as rs
+
+            for s in self.device.query_sensors():
+                if s.supports(rs.option.exposure):
+                    s.set_option(rs.option.exposure, int(value))
+                    logger.info(f"Set exposure to {int(value)}")
+                else:
+                    logger.info(f"Not setting exposure on sensor {s}, it is not supported.")
+
+            return True
+        except Exception as e:
+            logger.error(f"Error setting exposure: {e}")
+            return False
+
+    def get_exposure(self) -> Optional[float]:
+        """Get current exposure."""
+        if not self.is_connected or self.pipeline is None:
+            return None
+
+        try:
+            import pyrealsense2 as rs
+
+            for s in self.device.query_sensors():
+                if s.supports(rs.option.exposure):
+                    exposure = s.get_option(rs.option.exposure)
+                    return float(exposure)
+        except Exception:
+            return self._exposure
+
+    def set_gain(self, value: float) -> bool:
+        """Set gain (16-248 for most realsense cameras)."""
+        if not self.is_connected or self.pipeline is None:
+            return False
+
+        try:
+            self._gain = value
+            import pyrealsense2 as rs
+
+            for s in self.device.query_sensors():
+                if s.supports(rs.option.gain):
+                    s.set_option(rs.option.gain, int(value))
+                    logger.info(f"Set gain to {int(value)}")
+                else:
+                    logger.info(f"Not setting gain on sensor {s}, it is not supported.")
+
+            return True
+        except Exception as e:
+            logger.error(f"Error setting gain: {e}")
+            return False
+
+    def get_gain(self) -> Optional[float]:
+        """Get current gain."""
+        if not self.is_connected or self.pipeline is None:
+            return None
+
+        try:
+            import pyrealsense2 as rs
+
+            for s in self.device.query_sensors():
+                if s.supports(rs.option.gain):
+                    gain = s.get_option(rs.option.gain)
+                    return float(gain)
+        except Exception:
+            return self._gain
+
+    def get_exposure_range(self) -> Optional[tuple[float, float]]:
+        """Get current exposure."""
+        if not self.is_connected or self.pipeline is None:
+            return None
+
+        try:
+            import pyrealsense2 as rs
+
+            for s in self.device.query_sensors():
+                if s.supports(rs.option.exposure):
+                    exposure = s.get_option_range(rs.option.exposure)
+                    return float(exposure.min), float(exposure.max)
+        except Exception:
+            return 0.0, 0.0
+
+    def get_gain_range(self) -> Optional[tuple[float, float]]:
+        """Get current gain."""
+        if not self.is_connected or self.pipeline is None:
+            return None
+
+        try:
+            import pyrealsense2 as rs
+
+            for s in self.device.query_sensors():
+                if s.supports(rs.option.gain):
+                    gain = s.get_option_range(rs.option.gain)
+                    return float(gain.min), float(gain.max)
+        except Exception:
+            return 0.0, 0.0
+
+    def _get_active_profile(self):
+        import pyrealsense2 as rs
+
+        color_stream = self.profile.get_stream(rs.stream.color)
+        video_profile = color_stream.as_video_stream_profile()
+        return video_profile
+
+    def get_frame_size(self) -> Optional[tuple[int, int]]:
+        """Get frame size."""
+        if not self.is_connected or self.profile is None:
+            return None
+
+        # Get current resolution
+        video_profile = self._get_active_profile()
+        width = video_profile.width()
+        height = video_profile.height()
+
+        return width, height
+
+    def set_frame_size(self, width: int, height: int) -> bool:
+        """Set frame size."""
+        self.w = width
+        self.h = height
+
+        return True
+
+    def get_fps(self) -> Optional[float]:
+        """Get FPS."""
+        if not self.is_connected or self.profile is None:
+            return None
+
+        video_profile = self._get_active_profile()
+        fps = video_profile.fps()
+        return fps
+
+    def set_fps(self, fps: float) -> bool:
+        self.fps = fps
+        return True
+
+    def enable_auto_exposure(self, enable: bool = True) -> bool:
+        """
+        Enable or disable auto exposure for realsense camera.
+        """
+        if not self.is_connected or self.pipeline is None:
+            return False
+        try:
+            import pyrealsense2 as rs
+
+            for s in self.device.query_sensors():
+                # Check if the sensor supports auto-exposure
+                if s.supports(rs.option.enable_auto_exposure):
+                    s.set_option(rs.option.enable_auto_exposure, int(enable))
+                    logger.info(f"Set auto exposure to {enable}")
+                else:
+                    logger.info(f"Not setting auto exposure on sensor {s}, it is not supported.")
+
+            return True
+        except Exception as e:
+            logger.error(f"Error setting auto exposure: {e}")
+            return False
+
+    @classmethod
+    def discover(cls) -> list:
+        """
+        Discover available RealSense cameras.
+
+        Returns:
+            list: List of dictionaries containing RealSense camera information.
+                Each dict contains: {'index': int, 'serial_number': str, 'name': str,
+                'product_line': str}
+        """
+        devices = []
+
+        try:
+            import pyrealsense2 as rs
+        except ImportError:
+            logger.warning("pyrealsense2 module not available. Cannot discover RealSense cameras.")
+            return []
+
+        try:
+            # Get RealSense context
+            ctx = rs.context()
+
+            # Query all connected devices
+            device_list = ctx.query_devices()
+
+            for index in range(len(device_list)):
+                try:
+                    device = device_list[index]
+                    device_data = {
+                        "index": index,
+                        "serial_number": device.get_info(rs.camera_info.serial_number),
+                        "name": device.get_info(rs.camera_info.name),
+                        "product_line": device.get_info(rs.camera_info.product_line),
+                    }
+                    devices.append(device_data)
+                    logger.info(f"Found RealSense camera: {device_data}")
+
+                except Exception as e:
+                    logger.warning(f"Could not get info for RealSense device {index}: {e}")
+                    continue
+
+        except Exception as e:
+            logger.error(f"Error discovering RealSense cameras: {e}")
+
+        return devices
+
+    @classmethod
+    def get_config_schema(cls) -> dict[str, Any]:
+        """Get configuration schema for RealSense camera capture"""
+        warnings.warn(
+            "get_config_schema() is deprecated and will be removed in a future release; "
+            "UI form schemas belong in the consuming application.",
+            DeprecationWarning,
+            stacklevel=2,
+        )
+        return {
+            "title": "RealSense Camera Configuration",
+            "description": "Configure Intel RealSense depth camera settings",
+            "fields": [
+                {
+                    "name": "source",
+                    "label": "Camera Index",
+                    "type": "number",
+                    "min": 0,
+                    "max": 10,
+                    "placeholder": "0",
+                    "description": "Camera device index (0 for first RealSense camera)",
+                    "required": False,
+                    "default": 0,
+                },
+                {
+                    "name": "width",
+                    "label": "Width",
+                    "type": "number",
+                    "min": 424,
+                    "max": 1920,
+                    "placeholder": "640",
+                    "description": "Frame width in pixels",
+                    "required": False,
+                    "default": 640,
+                },
+                {
+                    "name": "height",
+                    "label": "Height",
+                    "type": "number",
+                    "min": 240,
+                    "max": 1080,
+                    "placeholder": "480",
+                    "description": "Frame height in pixels",
+                    "required": False,
+                    "default": 480,
+                },
+                {
+                    "name": "fps",
+                    "label": "Frame Rate (FPS)",
+                    "type": "number",
+                    "min": 6,
+                    "max": 90,
+                    "placeholder": "30",
+                    "description": "Frames per second",
+                    "required": False,
+                    "default": 30,
+                },
+                {
+                    "name": "depth_range_min",
+                    "label": "Min Depth Range (m)",
+                    "type": "number",
+                    "min": 0.1,
+                    "max": 5.0,
+                    "step": 0.1,
+                    "placeholder": "0.3",
+                    "description": "Minimum depth detection range in meters",
+                    "required": False,
+                    "default": 0.3,
+                },
+                {
+                    "name": "depth_range_max",
+                    "label": "Max Depth Range (m)",
+                    "type": "number",
+                    "min": 1.0,
+                    "max": 10.0,
+                    "step": 0.1,
+                    "placeholder": "3.0",
+                    "description": "Maximum depth detection range in meters",
+                    "required": False,
+                    "default": 3.0,
+                },
+                {
+                    "name": "output_type",
+                    "label": "Output Type",
+                    "type": "select",
+                    "options": [
+                        {"value": "color", "label": "Color Only"},
+                        {"value": "depth", "label": "Depth Only"},
+                        {"value": "both", "label": "Color + Depth"},
+                    ],
+                    "description": "Type of output frames",
+                    "required": False,
+                    "default": "color",
+                },
+            ],
+        }
+
+
+if __name__ == "__main__":
+    # Example usage
+    camera = RealsenseCapture(
+        width=640,
+        height=480,
+        processor=RealsenseDepthProcessor(
+            output_format=RealsenseProcessingOutput.ALIGNED_SIDE_BY_SIDE
+        ),
+    )
+
+    # processor = RealsenseDepthProcessor(
+    #     output_format=RealsenseProcessingOutput.ALIGNED_SIDE_BY_SIDE
+    # )
+    # camera.attach_processor(processor)
+
+    if camera.connect():
+        print("Realsense camera connected successfully.")
+        print(f"Exposure: {camera.get_exposure()}")
+        print(f"Gain: {camera.get_gain()}")
+        print(f"Frame size: {camera.get_frame_size()}")
+
+        # Read a few frames
+        while camera.is_connected:
+            ret, frame = camera.read()
+            if ret:
+                cv2.imshow("Realsense camera", frame)  # type: ignore
+                if cv2.waitKey(1) & 0xFF == ord("q"):
+                    break
+
+        camera.stop()
+        camera.disconnect()
+    else:
+        print("Failed to connect to realsense camera.")