RealSense-D435i-G1/tests/fake_realsense.py

"""A fake pyrealsense2 pipeline for tests (there is no RealSense on the development PC).

Two ways to deliver frames:
- per call (default): wait_for_frames blocks `latency_s` and returns a new frameset;
- clocked (`fps=...`): the camera produces a frameset every 1/fps s whether anyone
  reads it or not, and wait_for_frames returns the next one, like the real device.
Frame N carries its number in the pixels (a 16x16 color block = N % 256, depth[0, 0] = N)
so a test can tell which frame an image came from. FakeAlign stands in for rs.align.
The driver only uses a few methods of the SDK objects, and these fakes provide exactly those.
"""

import threading
import time

import numpy as np

FAKE_LATENCY_S = 0.1   # one frame every 100 ms, as the R01 test asks
WIDTH, HEIGHT = 640, 480
MARK = 16              # side of the color block that carries the frame number


class FakeVideoFrame:
    def __init__(self, data: np.ndarray, index: int, fps: float):
        self._data = data
        self._index = index
        self._fps = fps

    def get_data(self):
        # A buffer over the frame's memory, as pyrealsense2's BufData: np.asanyarray() of it is a view.
        return memoryview(self._data)

    @property
    def array(self) -> np.ndarray:
        return self._data

    def get_height(self) -> int:
        return self._data.shape[0]

    def get_width(self) -> int:
        return self._data.shape[1]

    def get_timestamp(self) -> float:
        """Camera time of the frame, milliseconds."""
        return self._index * 1000.0 / self._fps

    def get_frame_timestamp_domain(self) -> str:
        return "timestamp_domain.hardware_clock"

    def __bool__(self) -> bool:
        return True


class FakeFrameset:
    def __init__(self, index: int, fps: float = 1.0 / FAKE_LATENCY_S):
        self.index = index
        rng = np.random.default_rng(index)
        # Textured images, so the JPEG/PNG encoders do realistic work.
        color = rng.integers(0, 255, (HEIGHT, WIDTH, 3), dtype=np.uint8)
        color[:MARK, :MARK] = index % 256
        depth = rng.integers(300, 3000, (HEIGHT, WIDTH), dtype=np.uint16)
        depth[0, 0] = index
        self._color = FakeVideoFrame(color, index, fps)
        self._depth = FakeVideoFrame(depth, index, fps)

    def get_color_frame(self):
        return self._color

    def get_depth_frame(self):
        return self._depth


def frame_number_of_color(bgr: np.ndarray) -> int:
    """The frame number a fake color image carries (JPEG-tolerant: median of the block)."""
    return int(np.median(bgr[2:MARK - 2, 2:MARK - 2]))


class FakePipeline:
    """wait_for_frames returns framesets numbered 1, 2, 3, ...; counts the calls."""

    def __init__(self, latency_s: float = FAKE_LATENCY_S, fps: float | None = None):
        self.latency_s = latency_s
        self.fps = fps
        self.calls = 0
        self.max_concurrent = 0
        self._active = 0
        self._lock = threading.Lock()
        self._start = time.monotonic()
        self.stopped = False
        self.last_frameset = None

    def _next_clocked_index(self) -> int:
        """Sleep until the camera's next frame and return its number."""
        period = 1.0 / self.fps
        elapsed = time.monotonic() - self._start
        index = int(elapsed / period) + 1
        time.sleep(max(0.0, self._start + index * period - time.monotonic()))
        return index

    def wait_for_frames(self, timeout_ms: int = 5000):
        with self._lock:
            self._active += 1
            self.max_concurrent = max(self.max_concurrent, self._active)
            self.calls += 1
            call = self.calls
        try:
            if self.fps:
                self.last_frameset = FakeFrameset(self._next_clocked_index(), self.fps)
            else:
                time.sleep(self.latency_s)
                self.last_frameset = FakeFrameset(call)
            return self.last_frameset
        finally:
            with self._lock:
                self._active -= 1

    def stop(self) -> None:
        self.stopped = True


class FakeAlign:
    """rs.align stand-in: takes `delay_s` and returns the frameset (the fake depth is already on the color grid)."""

    def __init__(self, delay_s: float):
        self.delay_s = delay_s
        self.calls = 0

    def process(self, frameset):
        self.calls += 1
        time.sleep(self.delay_s)
        return frameset


def open_fake_camera(camera, pipeline: FakePipeline, align: FakeAlign | None = None) -> None:
    """Put a RealSenseD435 in the 'open' state on a fake pipeline (USB 3 path; align only if given).

    Uses the driver's own start hook when it has one, so the same helper serves
    every version of the driver.
    """
    import pyrealsense2 as rs
    camera._pipeline = pipeline
    camera._align = align
    camera._color_format = rs.format.bgr8
    camera._depth_scale = 0.001
    camera._initialized = True
    camera._open = True
    start = getattr(camera, "_start_acquisition", None)
    if start is not None:
        start()