| 69 | |
| 70 | |
| 71 | class RealSenseCamera(RgbdCamera): |
| 72 | backend = "realsense" |
| 73 | |
| 74 | def __init__(self, *, serial: str, info: DeviceInfo, width: int, height: int, fps: int): |
| 75 | self._serial = serial |
| 76 | self._info = dict(info) |
| 77 | self._width = int(width) |
| 78 | self._height = int(height) |
| 79 | self._fps = int(fps) |
| 80 | self._pipeline: Any | None = None |
| 81 | self._align: Any | None = None |
| 82 | self._depth_scale = 0.0 |
| 83 | self._intrinsics: CameraIntrinsics | None = None |
| 84 | |
| 85 | @property |
| 86 | def info(self) -> DeviceInfo: |
| 87 | return dict(self._info) |
| 88 | |
| 89 | @property |
| 90 | def intrinsics(self) -> CameraIntrinsics: |
| 91 | if self._intrinsics is None: |
| 92 | raise RuntimeError("RealSense camera has not been started") |
| 93 | return self._intrinsics |
| 94 | |
| 95 | def start(self) -> None: |
| 96 | rs = _rs() |
| 97 | pipeline = rs.pipeline() |
| 98 | config = rs.config() |
| 99 | config.enable_device(self._serial) |
| 100 | config.enable_stream(rs.stream.depth, self._width, self._height, rs.format.z16, self._fps) |
| 101 | config.enable_stream(rs.stream.color, self._width, self._height, rs.format.bgr8, self._fps) |
| 102 | |
| 103 | profile = pipeline.start(config) |
| 104 | self._pipeline = pipeline |
| 105 | self._align = rs.align(rs.stream.color) |
| 106 | self._depth_scale = float(profile.get_device().first_depth_sensor().get_depth_scale()) |
| 107 | self._intrinsics = CameraIntrinsics.from_realsense( |
| 108 | profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics() |
| 109 | ) |
| 110 | self._info.update( |
| 111 | { |
| 112 | "width": str(self._width), |
| 113 | "height": str(self._height), |
| 114 | "fps": str(self._fps), |
| 115 | "depth_scale_m": f"{self._depth_scale:.12g}", |
| 116 | } |
| 117 | ) |
| 118 | |
| 119 | def wait_frame(self, timeout_ms: int) -> RgbdFrame | None: |
| 120 | if self._pipeline is None or self._align is None: |
| 121 | raise RuntimeError("RealSense camera has not been started") |
| 122 | |
| 123 | frameset = self._pipeline.wait_for_frames(int(timeout_ms)) |
| 124 | aligned = self._align.process(frameset) |
| 125 | depth_frame = aligned.get_depth_frame() |
| 126 | color_frame = aligned.get_color_frame() |
| 127 | if not depth_frame or not color_frame: |
| 128 | return None |