reachy-mini 1.0.0__py3-none-any.whl

reachy_mini/media/camera_gstreamer.py

@@ -0,0 +1,162 @@
+"""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.
+"""
+
+from threading import Thread
+from typing import Optional
+
+import numpy as np
+import numpy.typing as npt
+
+from reachy_mini.media.camera_constants import CameraResolution
+
+from .camera_base import CameraBackend, CameraBase
+
+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
+
+
+class GStreamerCamera(CameraBase):
+    """Camera implementation using GStreamer."""
+
+    def __init__(
+        self,
+        log_level: str = "INFO",
+        resolution: CameraResolution = CameraResolution.R1280x720,
+    ) -> None:
+        """Initialize the GStreamer camera."""
+        super().__init__(
+            backend=CameraBackend.GSTREAMER, log_level=log_level, resolution=resolution
+        )
+        Gst.init(None)
+        self._loop = GLib.MainLoop()
+        self._thread_bus_calls: Optional[Thread] = None
+
+        self.pipeline = Gst.Pipeline.new("camera_recorder")
+
+        # note for some applications the jpeg image could be directly used
+        self._appsink_video: GstApp = Gst.ElementFactory.make("appsink")
+        caps_video = Gst.Caps.from_string(
+            f"video/x-raw,format=BGR, width={self.resolution[0]},height={self.resolution[1]},framerate={self.framerate}/1"
+        )
+        self._appsink_video.set_property("caps", caps_video)
+        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)
+
+        cam_path = self.get_arducam_video_device()
+        if cam_path == "":
+            self.logger.warning("Recording pipeline set without camera.")
+            self.pipeline.remove(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 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_arducam_video_device(self) -> str:
+        """Use Gst.DeviceMonitor to find the unix camera path /dev/videoX of the Arducam_12MP webcam.
+
+        Returns the device path (e.g., '/dev/video2'), or '' if not found.
+        """
+        monitor = Gst.DeviceMonitor()
+        monitor.add_filter("Video/Source")
+        monitor.start()
+
+        devices = monitor.get_devices()
+        for device in devices:
+            name = device.get_display_name()
+            device_props = device.get_properties()
+            if name and "Arducam_12MP" in name:
+                if device_props and device_props.has_field("api.v4l2.path"):
+                    device_path = device_props.get_string("api.v4l2.path")
+                    self.logger.debug(f"Found Arducam_12MP at {device_path}")
+                    monitor.stop()
+                    return str(device_path)
+        monitor.stop()
+        self.logger.warning("Arducam_12MP webcam not found.")
+        return ""

reachy_mini/media/camera_opencv.py

@@ -0,0 +1,61 @@
+"""OpenCv camera backend.
+
+This module provides an implementation of the CameraBase class using OpenCV.
+"""
+
+from typing import Optional
+
+import cv2
+import numpy as np
+import numpy.typing as npt
+
+from reachy_mini.media.camera_constants import CameraResolution
+from reachy_mini.media.camera_utils import find_camera
+
+from .camera_base import CameraBackend, CameraBase
+
+
+class OpenCVCamera(CameraBase):
+    """Camera implementation using OpenCV."""
+
+    def __init__(
+        self,
+        log_level: str = "INFO",
+        resolution: CameraResolution = CameraResolution.R1280x720,
+    ) -> None:
+        """Initialize the OpenCV camera."""
+        super().__init__(
+            backend=CameraBackend.OPENCV, log_level=log_level, resolution=resolution
+        )
+        self.cap: Optional[cv2.VideoCapture] = None
+
+    def open(self, udp_camera: Optional[str] = None) -> None:
+        """Open the camera using OpenCV VideoCapture."""
+        if udp_camera:
+            self.cap = cv2.VideoCapture(udp_camera)
+        else:
+            self.cap = find_camera()
+            if self.cap is None:
+                raise RuntimeError("Camera not found")
+            self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.resolution[0])
+            self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.resolution[1])
+
+        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 or None if error."""
+        if self.cap is None:
+            raise RuntimeError("Camera is not opened.")
+
+        ret, frame = self.cap.read()
+        if not ret:
+            return None
+
+        return np.asarray(frame, dtype=np.uint8, copy=False)
+
+    def close(self) -> None:
+        """Release the camera resource."""
+        if self.cap is not None:
+            self.cap.release()
+            self.cap = None

reachy_mini/media/camera_utils.py

@@ -0,0 +1,60 @@
+"""Camera utility for Reachy Mini."""
+
+import platform
+
+import cv2
+from cv2_enumerate_cameras import enumerate_cameras
+
+
+def find_camera(
+    vid: int = 0x0C45,
+    pid: int = 0x636D,
+    apiPreference: int = cv2.CAP_ANY,
+) -> cv2.VideoCapture | None:
+    """Find and return a camera with the specified VID and PID.
+
+    Args:
+        vid (int): Vendor ID of the camera. Default is 0x0C45 (Arducam).
+        pid (int): Product ID of the camera. Default is 0x636D (Arducam).
+        apiPreference (int): Preferred API backend for the camera. Default is cv2.CAP_ANY.
+
+    Returns:
+        cv2.VideoCapture | None: A VideoCapture object if the camera is found and opened successfully, otherwise None.
+
+    """
+    if platform.system() == "Linux":
+        apiPreference = cv2.CAP_V4L2
+
+    selected_cap = None
+    for c in enumerate_cameras(apiPreference):
+        if c.vid == vid and c.pid == pid:
+            # the Arducam camera creates two /dev/videoX devices
+            # that enumerate_cameras cannot differentiate
+            try:
+                cap = cv2.VideoCapture(c.index, c.backend)
+                if cap.isOpened():
+                    selected_cap = cap
+            except Exception as e:
+                print(f"Error opening camera {c.index}: {e}")
+    return selected_cap
+
+
+if __name__ == "__main__":
+    from reachy_mini.media.camera_constants import CameraResolution
+
+    cam = find_camera()
+
+    if cam is None:
+        print("Camera not found")
+    else:
+        cam.set(cv2.CAP_PROP_FRAME_WIDTH, CameraResolution.R1280x720.value[0])
+        cam.set(cv2.CAP_PROP_FRAME_HEIGHT, CameraResolution.R1280x720.value[1])
+
+        while True:
+            ret, frame = cam.read()
+            if not ret:
+                print("Failed to grab frame")
+                break
+            cv2.imshow("Camera Feed", frame)
+            if cv2.waitKey(1) & 0xFF == ord("q"):
+                break

reachy_mini/media/media_manager.py

@@ -0,0 +1,194 @@
+"""Media Manager.
+
+Provides camera and audio access based on the selected backedn
+"""
+
+import logging
+from enum import Enum
+from typing import Optional
+
+import numpy as np
+import numpy.typing as npt
+
+from reachy_mini.media.audio_base import AudioBase
+from reachy_mini.media.camera_base import CameraBase
+from reachy_mini.media.camera_constants import CameraResolution
+
+# actual backends are dynamically imported
+
+
+class MediaBackend(Enum):
+    """Media backends."""
+
+    NO_MEDIA = "no_media"
+    DEFAULT = "default"
+    DEFAULT_NO_VIDEO = "default_no_video"
+    GSTREAMER = "gstreamer"
+
+
+class MediaManager:
+    """Abstract class for opening and managing audio devices."""
+
+    def __init__(
+        self,
+        backend: MediaBackend = MediaBackend.DEFAULT,
+        log_level: str = "INFO",
+        use_sim: bool = False,
+        resolution: CameraResolution = CameraResolution.R1280x720,
+    ) -> None:
+        """Initialize the audio device."""
+        self.logger = logging.getLogger(__name__)
+        self.logger.setLevel(log_level)
+        self.backend = backend
+        self.camera: Optional[CameraBase] = None
+        if (
+            not backend == MediaBackend.DEFAULT_NO_VIDEO
+            and not backend == MediaBackend.NO_MEDIA
+        ):
+            self._init_camera(use_sim, log_level, resolution)
+        self.audio: Optional[AudioBase] = None
+        if not backend == MediaBackend.NO_MEDIA:
+            self._init_audio(log_level)
+
+    def __del__(self) -> None:
+        """Destructor to ensure resources are released."""
+        if self.camera is not None:
+            self.camera.close()
+        if self.audio is not None:
+            self.audio.stop_recording()
+            self.audio.stop_playing()
+
+    def _init_camera(
+        self,
+        use_sim: bool,
+        log_level: str,
+        resolution: CameraResolution,
+    ) -> None:
+        """Initialize the camera."""
+        self.logger.debug("Initializing camera...")
+        if self.backend == MediaBackend.DEFAULT:
+            self.logger.info("Using OpenCV camera backend.")
+            from reachy_mini.media.camera_opencv import OpenCVCamera
+
+            self.camera = OpenCVCamera(log_level=log_level, resolution=resolution)
+            if use_sim:
+                self.camera.open(udp_camera="udp://@127.0.0.1:5005")
+            else:
+                self.camera.open()
+        elif self.backend == MediaBackend.GSTREAMER:
+            self.logger.info("Using GStreamer camera backend.")
+            from reachy_mini.media.camera_gstreamer import GStreamerCamera
+
+            self.camera = GStreamerCamera(log_level=log_level, resolution=resolution)
+            self.camera.open()
+            # Todo: use simulation with gstreamer?
+
+        else:
+            raise NotImplementedError(f"Camera backend {self.backend} not implemented.")
+
+    def get_frame(self) -> Optional[npt.NDArray[np.uint8]]:
+        """Get a frame from the camera.
+
+        Returns:
+            Optional[npt.NDArray[np.uint8]]: The captured BGR frame, or None if the camera is not available.
+
+        """
+        if self.camera is None:
+            self.logger.warning("Camera is not initialized.")
+            return None
+        return self.camera.read()
+
+    def _init_audio(self, log_level: str) -> None:
+        """Initialize the audio system."""
+        self.logger.debug("Initializing audio...")
+        if (
+            self.backend == MediaBackend.DEFAULT
+            or self.backend == MediaBackend.DEFAULT_NO_VIDEO
+        ):
+            self.logger.info("Using SoundDevice audio backend.")
+            from reachy_mini.media.audio_sounddevice import SoundDeviceAudio
+
+            self.audio = SoundDeviceAudio(log_level=log_level)
+        elif self.backend == MediaBackend.GSTREAMER:
+            self.logger.info("Using GStreamer audio backend.")
+            from reachy_mini.media.audio_gstreamer import GStreamerAudio
+
+            self.audio = GStreamerAudio(log_level=log_level)
+        else:
+            raise NotImplementedError(f"Audio backend {self.backend} not implemented.")
+
+    def play_sound(self, sound_file: str) -> None:
+        """Play a sound file.
+
+        Args:
+            sound_file (str): Path to the sound file to play.
+
+        """
+        if self.audio is None:
+            self.logger.warning("Audio system is not initialized.")
+            return
+        self.audio.play_sound(sound_file)
+
+    def start_recording(self) -> None:
+        """Start recording audio."""
+        if self.audio is None:
+            self.logger.warning("Audio system is not initialized.")
+            return
+        self.audio.start_recording()
+
+    def get_audio_sample(self) -> Optional[bytes | npt.NDArray[np.float32]]:
+        """Get an audio sample from the audio device.
+
+        Returns:
+            Optional[np.ndarray]: The recorded audio sample, or None if no data is available.
+
+        """
+        if self.audio is None:
+            self.logger.warning("Audio system is not initialized.")
+            return None
+        return self.audio.get_audio_sample()
+
+    def get_audio_samplerate(self) -> int:
+        """Get the samplerate of the audio device.
+
+        Returns:
+            int: The samplerate of the audio device.
+
+        """
+        if self.audio is None:
+            self.logger.warning("Audio system is not initialized.")
+            return -1
+        return self.audio.SAMPLE_RATE
+
+    def stop_recording(self) -> None:
+        """Stop recording audio."""
+        if self.audio is None:
+            self.logger.warning("Audio system is not initialized.")
+            return
+        self.audio.stop_recording()
+
+    def start_playing(self) -> None:
+        """Start playing audio."""
+        if self.audio is None:
+            self.logger.warning("Audio system is not initialized.")
+            return
+        self.audio.start_playing()
+
+    def push_audio_sample(self, data: npt.NDArray[np.float32]) -> None:
+        """Push audio data to the output device.
+
+        Args:
+            data (npt.NDArray[np.float32]): The audio data to push to the output device (mono format).
+
+        """
+        if self.audio is None:
+            self.logger.warning("Audio system is not initialized.")
+            return
+        self.audio.push_audio_sample(data)
+
+    def stop_playing(self) -> None:
+        """Stop playing audio."""
+        if self.audio is None:
+            self.logger.warning("Audio system is not initialized.")
+            return
+        self.audio.stop_playing()

reachy_mini/motion/__init__.py

@@ -0,0 +1,4 @@
+"""Motion module for Reachy Mini.
+
+This module contains both utilities to create and play moves, as well as utilities to download datasets of recorded moves.
+"""

reachy_mini/motion/goto.py

@@ -0,0 +1,71 @@
+"""A goto move to a target head pose and/or antennas position."""
+
+import numpy as np
+import numpy.typing as npt
+
+from reachy_mini.utils.interpolation import (
+    InterpolationTechnique,
+    linear_pose_interpolation,
+    time_trajectory,
+)
+
+from .move import Move
+
+
+class GotoMove(Move):
+    """A goto move to a target head pose and/or antennas position."""
+
+    def __init__(
+        self,
+        start_head_pose: npt.NDArray[np.float64],
+        target_head_pose: npt.NDArray[np.float64] | None,
+        start_antennas: npt.NDArray[np.float64],
+        target_antennas: npt.NDArray[np.float64] | None,
+        start_body_yaw: float,
+        target_body_yaw: float | None,
+        duration: float,
+        method: InterpolationTechnique,
+    ):
+        """Set up the goto move."""
+        self.start_head_pose = start_head_pose
+        self.target_head_pose = (
+            target_head_pose if target_head_pose is not None else start_head_pose
+        )
+        self.start_antennas = start_antennas
+        self.target_antennas = (
+            target_antennas if target_antennas is not None else start_antennas
+        )
+        self.start_body_yaw = start_body_yaw
+        self.target_body_yaw = (
+            target_body_yaw if target_body_yaw is not None else start_body_yaw
+        )
+
+        self._duration = duration
+        self.method = method
+
+    @property
+    def duration(self) -> float:
+        """Duration of the goto in seconds."""
+        return self._duration
+
+    def evaluate(
+        self, t: float
+    ) -> tuple[
+        npt.NDArray[np.float64] | None, npt.NDArray[np.float64] | None, float | None
+    ]:
+        """Evaluate the goto at time t."""
+        interp_time = time_trajectory(t / self.duration, method=self.method)
+
+        interp_head_pose = linear_pose_interpolation(
+            self.start_head_pose, self.target_head_pose, interp_time
+        )
+        interp_antennas_joint = (
+            self.start_antennas
+            + (self.target_antennas - self.start_antennas) * interp_time
+        )
+        interp_body_yaw_joint = (
+            self.start_body_yaw
+            + (self.target_body_yaw - self.start_body_yaw) * interp_time
+        )
+
+        return interp_head_pose, interp_antennas_joint, interp_body_yaw_joint

reachy_mini/motion/move.py

@@ -0,0 +1,36 @@
+"""Module for defining motion moves on the ReachyMini robot."""
+
+from abc import ABC, abstractmethod
+
+import numpy as np
+import numpy.typing as npt
+
+
+class Move(ABC):
+    """Abstract base class for defining a move on the ReachyMini robot."""
+
+    @property
+    @abstractmethod
+    def duration(self) -> float:
+        """Duration of the move in seconds."""
+        pass
+
+    @abstractmethod
+    def evaluate(
+        self,
+        t: float,
+    ) -> tuple[
+        npt.NDArray[np.float64] | None, npt.NDArray[np.float64] | None, float | None
+    ]:
+        """Evaluate the move at time t, typically called at a high-frequency (eg. 100Hz).
+
+        Arguments:
+            t: The time at which to evaluate the move (in seconds). It will always be between 0 and duration.
+
+        Returns:
+            head: The head position (4x4 homogeneous matrix).
+            antennas: The antennas positions (rad).
+            body_yaw: The body yaw angle (rad).
+
+        """
+        pass