reachy-mini 1.2.5rc1__py3-none-any.whl

reachy_mini/media/audio_utils.py

@@ -0,0 +1,114 @@
+"""Utility functions for audio handling, specifically for detecting the ReSpeaker sound card."""
+
+import logging
+import subprocess
+from pathlib import Path
+
+
+def _process_card_number_output(output: str) -> int:
+    """Process the output of 'arecord -l' to find the ReSpeaker or Reachy Mini Audio card number."""
+    lines = output.split("\n")
+    for line in lines:
+        if "reachy mini audio" in line.lower():
+            card_number = line.split(" ")[1].split(":")[0]
+            logging.debug(f"Found Reachy Mini Audio sound card: {card_number}")
+            return int(card_number)
+        elif "respeaker" in line.lower():
+            card_number = line.split(" ")[1].split(":")[0]
+            logging.warning(
+                f"Found ReSpeaker sound card: {card_number}. Please update firmware!"
+            )
+            return int(card_number)
+
+    logging.warning("Reachy Mini Audio sound card not found. Returning default card")
+    return 0  # default sound card
+
+
+def get_respeaker_card_number() -> int:
+    """Return the card number of the ReSpeaker sound card, or 0 if not found."""
+    try:
+        result = subprocess.run(
+            ["arecord", "-l"], capture_output=True, text=True, check=True
+        )
+        output = result.stdout
+
+        return _process_card_number_output(output)
+
+    except subprocess.CalledProcessError as e:
+        logging.error(f"Cannot find sound card: {e}")
+        return -1
+
+
+def has_reachymini_asoundrc() -> bool:
+    """Check if ~/.asoundrc exists and contains both reachymini_audio_sink and reachymini_audio_src."""
+    asoundrc_path = Path.home().joinpath(".asoundrc")
+    if not asoundrc_path.exists():
+        return False
+    content = asoundrc_path.read_text(errors="ignore")
+    return "reachymini_audio_sink" in content and "reachymini_audio_src" in content
+
+
+def check_reachymini_asoundrc() -> bool:
+    """Check if ~/.asoundrc exists and is correctly configured for Reachy Mini Audio."""
+    asoundrc_path = Path.home().joinpath(".asoundrc")
+    if not asoundrc_path.exists():
+        return False
+    content = asoundrc_path.read_text(errors="ignore")
+    card_id = get_respeaker_card_number()
+    # Check for both sink and src
+    if not ("reachymini_audio_sink" in content and "reachymini_audio_src" in content):
+        return False
+    # Check that the card number in .asoundrc matches the detected card_id
+    import re
+
+    card_numbers = set(re.findall(r"card\s+(\d+)", content))
+    if str(card_id) not in card_numbers:
+        return False
+    return True
+
+
+def write_asoundrc_to_home() -> None:
+    """Write the .asoundrc file with Reachy Mini audio configuration to the user's home directory."""
+    card_id = get_respeaker_card_number()
+    asoundrc_content = f"""
+pcm.!default {{
+    type hw
+    card {card_id}
+}}
+
+ctl.!default {{
+    type hw
+    card {card_id}
+}}
+
+pcm.reachymini_audio_sink {{
+    type dmix
+    ipc_key 4241
+    slave {{
+        pcm "hw:{card_id},0"
+        channels 2
+        period_size 1024
+        buffer_size 4096
+        rate 16000
+    }}
+    bindings {{
+        0 0
+        1 1
+    }}
+}}
+
+pcm.reachymini_audio_src {{
+    type dsnoop
+    ipc_key 4242
+    slave {{
+        pcm "hw:{card_id},0"
+        channels 2
+        rate 16000
+        period_size 1024
+        buffer_size 4096
+    }}
+}}
+"""
+    asoundrc_path = Path.home().joinpath(".asoundrc")
+    with open(asoundrc_path, "w") as f:
+        f.write(asoundrc_content)

reachy_mini/media/camera_base.py

@@ -0,0 +1,100 @@
+"""Base classes for camera implementations.
+
+The camera implementations support various backends and provide a unified
+interface for capturing images.
+"""
+
+import logging
+from abc import ABC, abstractmethod
+from typing import Optional
+
+import numpy as np
+import numpy.typing as npt
+
+from reachy_mini.media.camera_constants import (
+    CameraResolution,
+    CameraSpecs,
+    MujocoCameraSpecs,
+)
+
+
+class CameraBase(ABC):
+    """Abstract class for opening and managing a camera."""
+
+    def __init__(
+        self,
+        log_level: str = "INFO",
+    ) -> None:
+        """Initialize the camera."""
+        self.logger = logging.getLogger(__name__)
+        self.logger.setLevel(log_level)
+        self._resolution: Optional[CameraResolution] = None
+        self.camera_specs: Optional[CameraSpecs] = None
+        self.resized_K: Optional[npt.NDArray[np.float64]] = None
+
+    @property
+    def resolution(self) -> tuple[int, int]:
+        """Get the current camera resolution as a tuple (width, height)."""
+        if self._resolution is None:
+            raise RuntimeError("Camera resolution is not set.")
+        return (self._resolution.value[0], self._resolution.value[1])
+
+    @property
+    def framerate(self) -> int:
+        """Get the current camera frames per second."""
+        if self._resolution is None:
+            raise RuntimeError("Camera resolution is not set.")
+        return int(self._resolution.value[2])
+
+    @property
+    def K(self) -> Optional[npt.NDArray[np.float64]]:
+        """Get the camera intrinsic matrix for the current resolution."""
+        return self.resized_K
+
+    @property
+    def D(self) -> Optional[npt.NDArray[np.float64]]:
+        """Get the camera distortion coefficients."""
+        if self.camera_specs is not None:
+            return self.camera_specs.D
+        return None
+
+    def set_resolution(self, resolution: CameraResolution) -> None:
+        """Set the camera resolution."""
+        if self.camera_specs is None:
+            raise RuntimeError(
+                "Camera specs not set. Open the camera before setting the resolution."
+            )
+
+        if isinstance(self.camera_specs, MujocoCameraSpecs):
+            raise RuntimeError(
+                "Cannot change resolution of Mujoco simulated camera for now."
+            )
+
+        if resolution not in self.camera_specs.available_resolutions:
+            raise ValueError(
+                f"Resolution not supported by the camera. Available resolutions are : {self.camera_specs.available_resolutions}"
+            )
+
+        w_ratio = resolution.value[0] / self.camera_specs.default_resolution.value[0]
+        h_ratio = resolution.value[1] / self.camera_specs.default_resolution.value[1]
+        self.resized_K = self.camera_specs.K.copy()
+
+        self.resized_K[0, 0] *= w_ratio
+        self.resized_K[1, 1] *= h_ratio
+        self.resized_K[0, 2] *= w_ratio
+        self.resized_K[1, 2] *= h_ratio
+
+    @abstractmethod
+    def open(self) -> None:
+        """Open the camera."""
+        pass
+
+    @abstractmethod
+    def read(self) -> Optional[npt.NDArray[np.uint8]]:
+        """Read an image from the camera. Returns the image or None if error."""
+        pass
+
+    @abstractmethod
+    def close(self) -> None:
+        """Close the camera and release resources."""
+        pass

reachy_mini/media/camera_constants.py

@@ -0,0 +1,152 @@
+"""Camera constants for Reachy Mini."""
+
+from dataclasses import dataclass, field
+from enum import Enum
+from typing import List
+
+import numpy as np
+import numpy.typing as npt
+
+
+class CameraResolution(Enum):
+    """Base class for camera resolutions."""
+
+    R1536x864at40fps = (1536, 864, 40)
+
+    R1280x720at60fps = (1280, 720, 60)
+    R1280x720at30fps = (1280, 720, 30)
+
+    R1920x1080at30fps = (1920, 1080, 30)
+    R1920x1080at60fps = (1920, 1080, 60)
+
+    R2304x1296at30fps = (2304, 1296, 30)
+    R1600x1200at30fps = (1600, 1200, 30)
+
+    R3264x2448at30fps = (3264, 2448, 30)
+    R3264x2448at10fps = (3264, 2448, 10)
+
+    R3840x2592at30fps = (3840, 2592, 30)
+    R3840x2592at10fps = (3840, 2592, 10)
+    R3840x2160at30fps = (3840, 2160, 30)
+    R3840x2160at10fps = (3840, 2160, 10)
+
+    R3072x1728at10fps = (3072, 1728, 10)
+
+    R4608x2592at10fps = (4608, 2592, 10)
+
+
+@dataclass
+class CameraSpecs:
+    """Base camera specifications."""
+
+    name: str = ""
+    available_resolutions: List[CameraResolution] = field(default_factory=list)
+    default_resolution: CameraResolution = CameraResolution.R1280x720at30fps
+    vid = 0
+    pid = 0
+    K: npt.NDArray[np.float64] = field(default_factory=lambda: np.eye(3))
+    D: npt.NDArray[np.float64] = field(default_factory=lambda: np.zeros((5,)))
+
+
+@dataclass
+class ArducamSpecs(CameraSpecs):
+    """Arducam camera specifications."""
+
+    name = "arducam"
+    available_resolutions = [
+        CameraResolution.R2304x1296at30fps,
+        CameraResolution.R4608x2592at10fps,
+        CameraResolution.R1920x1080at30fps,
+        CameraResolution.R1600x1200at30fps,
+        CameraResolution.R1280x720at30fps,
+    ]
+    default_resolution = CameraResolution.R1280x720at30fps
+    vid = 0x0C45
+    pid = 0x636D
+    K = np.array([[550.3564, 0.0, 638.0112], [0.0, 549.1653, 364.589], [0.0, 0.0, 1.0]])
+    D = np.array([-0.0694, 0.1565, -0.0004, 0.0003, -0.0983])
+
+
+@dataclass
+class ReachyMiniLiteCamSpecs(CameraSpecs):
+    """Reachy Mini Lite camera specifications."""
+
+    name = "lite"
+    available_resolutions = [
+        CameraResolution.R1920x1080at60fps,
+        CameraResolution.R3840x2592at30fps,
+        CameraResolution.R3840x2160at30fps,
+        CameraResolution.R3264x2448at30fps,
+    ]
+    default_resolution = CameraResolution.R1920x1080at60fps
+    vid = 0x38FB
+    pid = 0x1002
+    K = np.array(
+        [
+            [821.515, 0.0, 962.241],
+            [0.0, 820.830, 542.459],
+            [0.0, 0.0, 1.0],
+        ]
+    )
+
+    D = np.array(
+        [
+            -2.94475669e-02,
+            6.00511974e-02,
+            3.57813971e-06,
+            -2.96459394e-04,
+            -3.79243988e-02,
+        ]
+    )
+
+
+@dataclass
+class ReachyMiniWirelessCamSpecs(ReachyMiniLiteCamSpecs):
+    """Reachy Mini Wireless camera specifications."""
+
+    name = "wireless"
+    available_resolutions = [
+        CameraResolution.R1920x1080at30fps,
+        CameraResolution.R1280x720at60fps,
+        CameraResolution.R3840x2592at10fps,
+        CameraResolution.R3840x2160at10fps,
+        CameraResolution.R3264x2448at10fps,
+        CameraResolution.R3072x1728at10fps,
+    ]
+    default_resolution = CameraResolution.R1920x1080at30fps
+
+
+@dataclass
+class OlderRPiCamSpecs(ReachyMiniLiteCamSpecs):
+    """Older Raspberry Pi camera specifications. Keeping for compatibility."""
+
+    name = "older_rpi"
+    vid = 0x1BCF
+    pid = 0x28C4
+
+
+@dataclass
+class MujocoCameraSpecs(CameraSpecs):
+    """Mujoco simulated camera specifications."""
+
+    available_resolutions = [
+        CameraResolution.R1280x720at60fps,
+    ]
+    default_resolution = CameraResolution.R1280x720at60fps
+    # ideal camera matrix
+    K = np.array(
+        [
+            [
+                CameraResolution.R1280x720at60fps.value[0],
+                0.0,
+                CameraResolution.R1280x720at60fps.value[0] / 2,
+            ],
+            [
+                0.0,
+                CameraResolution.R1280x720at60fps.value[1],
+                CameraResolution.R1280x720at60fps.value[1] / 2,
+            ],
+            [0.0, 0.0, 1.0],
+        ]
+    )
+    D = np.zeros((5,))  # no distortion

reachy_mini/media/camera_gstreamer.py

@@ -0,0 +1,231 @@
+"""GStreamer camera backend.
+
+This module provides an implementation of the CameraBase class using GStreamer.
+By default the module directly returns JPEG images as output by the camera.
+"""
+
+import os
+from threading import Thread
+from typing import Optional, Tuple, cast
+
+import numpy as np
+import numpy.typing as npt
+
+from reachy_mini.media.camera_constants import (
+    ArducamSpecs,
+    CameraResolution,
+    CameraSpecs,
+    ReachyMiniLiteCamSpecs,
+    ReachyMiniWirelessCamSpecs,
+)
+
+try:
+    import gi
+
+except ImportError as e:
+    raise ImportError(
+        "The 'gi' module is required for GStreamerCamera but could not be imported. \
+                      Please install the GStreamer backend: pip install .[gstreamer]."
+    ) from e
+
+gi.require_version("Gst", "1.0")
+gi.require_version("GstApp", "1.0")
+
+
+from gi.repository import GLib, Gst, GstApp  # noqa: E402
+
+from .camera_base import CameraBase  # noqa: E402
+
+
+class GStreamerCamera(CameraBase):
+    """Camera implementation using GStreamer."""
+
+    def __init__(
+        self,
+        log_level: str = "INFO",
+    ) -> None:
+        """Initialize the GStreamer camera."""
+        super().__init__(log_level=log_level)
+        Gst.init(None)
+        self._loop = GLib.MainLoop()
+        self._thread_bus_calls: Optional[Thread] = None
+
+        self.pipeline = Gst.Pipeline.new("camera_recorder")
+
+        cam_path, self.camera_specs = self.get_video_device()
+
+        if self.camera_specs is None:
+            raise RuntimeError("Camera specs not set")
+        self._resolution = self.camera_specs.default_resolution
+        self.resized_K = self.camera_specs.K
+
+        if self._resolution is None:
+            raise RuntimeError("Failed to get default camera resolution.")
+
+        # note for some applications the jpeg image could be directly used
+        self._appsink_video: GstApp = Gst.ElementFactory.make("appsink")
+        self.set_resolution(self._resolution)
+        self._appsink_video.set_property("drop", True)  # avoid overflow
+        self._appsink_video.set_property("max-buffers", 1)  # keep last image only
+        self.pipeline.add(self._appsink_video)
+
+        if cam_path == "":
+            self.logger.warning("Recording pipeline set without camera.")
+            self.pipeline.remove(self._appsink_video)
+        elif cam_path == "/tmp/reachymini_camera_socket":
+            camsrc = Gst.ElementFactory.make("unixfdsrc")
+            camsrc.set_property("socket-path", "/tmp/reachymini_camera_socket")
+            self.pipeline.add(camsrc)
+            queue = Gst.ElementFactory.make("queue")
+            self.pipeline.add(queue)
+            videoconvert = Gst.ElementFactory.make("videoconvert")
+            self.pipeline.add(videoconvert)
+            camsrc.link(queue)
+            queue.link(videoconvert)
+            videoconvert.link(self._appsink_video)
+        elif cam_path == "imx708":
+            camsrc = Gst.ElementFactory.make("libcamerasrc")
+            self.pipeline.add(camsrc)
+            queue = Gst.ElementFactory.make("queue")
+            self.pipeline.add(queue)
+            videoconvert = Gst.ElementFactory.make("videoconvert")
+            self.pipeline.add(videoconvert)
+            camsrc.link(queue)
+            queue.link(videoconvert)
+            videoconvert.link(self._appsink_video)
+        else:
+            camsrc = Gst.ElementFactory.make("v4l2src")
+            camsrc.set_property("device", cam_path)
+            self.pipeline.add(camsrc)
+            queue = Gst.ElementFactory.make("queue")
+            self.pipeline.add(queue)
+            # use vaapijpegdec or nvjpegdec for hardware acceleration
+            jpegdec = Gst.ElementFactory.make("jpegdec")
+            self.pipeline.add(jpegdec)
+            videoconvert = Gst.ElementFactory.make("videoconvert")
+            self.pipeline.add(videoconvert)
+            camsrc.link(queue)
+            queue.link(jpegdec)
+            jpegdec.link(videoconvert)
+            videoconvert.link(self._appsink_video)
+
+    def _on_bus_message(self, bus: Gst.Bus, msg: Gst.Message, loop) -> bool:  # type: ignore[no-untyped-def]
+        t = msg.type
+        if t == Gst.MessageType.EOS:
+            self.logger.warning("End-of-stream")
+            return False
+
+        elif t == Gst.MessageType.ERROR:
+            err, debug = msg.parse_error()
+            self.logger.error(f"Error: {err} {debug}")
+            return False
+
+        return True
+
+    def _handle_bus_calls(self) -> None:
+        self.logger.debug("starting bus message loop")
+        bus = self.pipeline.get_bus()
+        bus.add_watch(GLib.PRIORITY_DEFAULT, self._on_bus_message, self._loop)
+        self._loop.run()
+        bus.remove_watch()
+        self.logger.debug("bus message loop stopped")
+
+    def set_resolution(self, resolution: CameraResolution) -> None:
+        """Set the camera resolution."""
+        super().set_resolution(resolution)
+
+        # Check if pipeline is not playing before changing resolution
+        if self.pipeline.get_state(0).state == Gst.State.PLAYING:
+            raise RuntimeError(
+                "Cannot change resolution while the camera is streaming. Please close the camera first."
+            )
+
+        self._resolution = resolution
+        caps_video = Gst.Caps.from_string(
+            f"video/x-raw,format=BGR, width={self._resolution.value[0]},height={self._resolution.value[1]},framerate={self.framerate}/1"
+        )
+        self._appsink_video.set_property("caps", caps_video)
+
+    def open(self) -> None:
+        """Open the camera using GStreamer."""
+        self.pipeline.set_state(Gst.State.PLAYING)
+        self._thread_bus_calls = Thread(target=self._handle_bus_calls, daemon=True)
+        self._thread_bus_calls.start()
+
+    def _get_sample(self, appsink: GstApp.AppSink) -> Optional[bytes]:
+        sample = appsink.try_pull_sample(20_000_000)
+        if sample is None:
+            return None
+        data = None
+        if isinstance(sample, Gst.Sample):
+            buf = sample.get_buffer()
+            if buf is None:
+                self.logger.warning("Buffer is None")
+
+            data = buf.extract_dup(0, buf.get_size())
+        return data
+
+    def read(self) -> Optional[npt.NDArray[np.uint8]]:
+        """Read a frame from the camera. Returns the frame or None if error.
+
+        Returns:
+            Optional[npt.NDArray[np.uint8]]: The captured BGR frame as a NumPy array, or None if error.
+
+        """
+        data = self._get_sample(self._appsink_video)
+        if data is None:
+            return None
+
+        arr = np.frombuffer(data, dtype=np.uint8).reshape(
+            (self.resolution[1], self.resolution[0], 3)
+        )
+        return arr
+
+    def close(self) -> None:
+        """Release the camera resource."""
+        self._loop.quit()
+        self.pipeline.set_state(Gst.State.NULL)
+
+    def get_video_device(self) -> Tuple[str, Optional[CameraSpecs]]:
+        """Use Gst.DeviceMonitor to find the unix camera path /dev/videoX.
+
+        Returns the device path (e.g., '/dev/video2'), or '' if not found.
+        """
+        if os.path.exists("/tmp/reachymini_camera_socket"):
+            camera_specs = cast(CameraSpecs, ReachyMiniWirelessCamSpecs)
+            self.logger.debug(
+                "Found wireless camera socket at /tmp/reachymini_camera_socket"
+            )
+            return "/tmp/reachymini_camera_socket", camera_specs
+
+        monitor = Gst.DeviceMonitor()
+        monitor.add_filter("Video/Source")
+        monitor.start()
+
+        cam_names = ["Reachy", "Arducam_12MP", "imx708"]
+
+        devices = monitor.get_devices()
+        for cam_name in cam_names:
+            for device in devices:
+                name = device.get_display_name()
+                device_props = device.get_properties()
+
+                if cam_name in name:
+                    if device_props and device_props.has_field("api.v4l2.path"):
+                        device_path = device_props.get_string("api.v4l2.path")
+                        camera_specs = (
+                            cast(CameraSpecs, ArducamSpecs)
+                            if cam_name == "Arducam_12MP"
+                            else cast(CameraSpecs, ReachyMiniLiteCamSpecs)
+                        )
+                        self.logger.debug(f"Found {cam_name} camera at {device_path}")
+                        monitor.stop()
+                        return str(device_path), camera_specs
+                    elif cam_name == "imx708":
+                        camera_specs = cast(CameraSpecs, ReachyMiniWirelessCamSpecs)
+                        self.logger.debug(f"Found {cam_name} camera")
+                        monitor.stop()
+                        return cam_name, camera_specs
+        monitor.stop()
+        self.logger.warning("No camera found.")
+        return "", None

reachy_mini/media/camera_opencv.py

@@ -0,0 +1,89 @@
+"""OpenCv camera backend.
+
+This module provides an implementation of the CameraBase class using OpenCV.
+"""
+
+from typing import Optional, cast
+
+import cv2
+import numpy as np
+import numpy.typing as npt
+
+from reachy_mini.media.camera_constants import (
+    CameraResolution,
+    CameraSpecs,
+    MujocoCameraSpecs,
+)
+from reachy_mini.media.camera_utils import find_camera
+
+from .camera_base import CameraBase
+
+
+class OpenCVCamera(CameraBase):
+    """Camera implementation using OpenCV."""
+
+    def __init__(
+        self,
+        log_level: str = "INFO",
+    ) -> None:
+        """Initialize the OpenCV camera."""
+        super().__init__(log_level=log_level)
+        self.cap: Optional[cv2.VideoCapture] = None
+
+    def set_resolution(self, resolution: CameraResolution) -> None:
+        """Set the camera resolution."""
+        super().set_resolution(resolution)
+
+        self._resolution = resolution
+        if self.cap is not None:
+            self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self._resolution.value[0])
+            self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self._resolution.value[1])
+
+    def open(self, udp_camera: Optional[str] = None) -> None:
+        """Open the camera using OpenCV VideoCapture."""
+        if udp_camera:
+            self.cap = cv2.VideoCapture(udp_camera)
+            self.camera_specs = cast(CameraSpecs, MujocoCameraSpecs)
+            self._resolution = self.camera_specs.default_resolution
+        else:
+            self.cap, self.camera_specs = find_camera()
+            if self.cap is None or self.camera_specs is None:
+                raise RuntimeError("Camera not found")
+
+            self._resolution = self.camera_specs.default_resolution
+            if self._resolution is None:
+                raise RuntimeError("Failed to get default camera resolution.")
+
+            self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self._resolution.value[0])
+            self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self._resolution.value[1])
+
+        self.resized_K = self.camera_specs.K
+
+        if not self.cap.isOpened():
+            raise RuntimeError("Failed to open camera")
+
+    def read(self) -> Optional[npt.NDArray[np.uint8]]:
+        """Read a frame from the camera.
+
+        Returns:
+            The frame as a uint8 numpy array, or None if no frame could be read.
+
+        Raises:
+            RuntimeError: If the camera is not opened.
+
+        """
+        if self.cap is None:
+            raise RuntimeError("Camera is not opened.")
+        ret, frame = self.cap.read()
+        if not ret:
+            return None
+        # Ensure uint8 dtype
+        if frame.dtype != np.uint8:
+            frame = frame.astype(np.uint8, copy=False)
+        return cast(npt.NDArray[np.uint8], frame)
+
+    def close(self) -> None:
+        """Release the camera resource."""
+        if self.cap is not None:
+            self.cap.release()
+            self.cap = None