diff --git a/src/lerobot/cameras/reachy2_camera/__init__.py b/src/lerobot/cameras/reachy2_camera/__init__.py new file mode 100644 index 000000000..87a6a488d --- /dev/null +++ b/src/lerobot/cameras/reachy2_camera/__init__.py @@ -0,0 +1,16 @@ +# Copyright 2024 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .reachy2_camera import Reachy2Camera +from .configuration_reachy2_camera import Reachy2CameraConfig diff --git a/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py b/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py new file mode 100644 index 000000000..3d72c8bdf --- /dev/null +++ b/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py @@ -0,0 +1,65 @@ +# Copyright 2024 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass +from pathlib import Path + +from ..configs import CameraConfig, ColorMode, Cv2Rotation + + +@CameraConfig.register_subclass("reachy2_camera") +@dataclass +class Reachy2CameraConfig(CameraConfig): + """Configuration class for OpenCV-based camera devices or video files. + + This class provides configuration options for cameras accessed through OpenCV, + supporting both physical camera devices and video files. It includes settings + for resolution, frame rate, color mode, and image rotation. + + Example configurations: + ```python + # Basic configurations + OpenCVCameraConfig(0, 30, 1280, 720) # 1280x720 @ 30FPS + OpenCVCameraConfig(/dev/video4, 60, 640, 480) # 640x480 @ 60FPS + + # Advanced configurations + OpenCVCameraConfig(128422271347, 30, 640, 480, rotation=Cv2Rotation.ROTATE_90) # With 90° rotation + ``` + + Attributes: + index_or_path: Either an integer representing the camera device index, + or a Path object pointing to a video file. + fps: Requested frames per second for the color stream. + width: Requested frame width in pixels for the color stream. + height: Requested frame height in pixels for the color stream. + color_mode: Color mode for image output (RGB or BGR). Defaults to RGB. + rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation. + warmup_s: Time reading frames before returning from connect (in seconds) + + Note: + - Only 3-channel color output (RGB/BGR) is currently supported. + """ + + name: str + image_type: str + color_mode: ColorMode = ColorMode.RGB + rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION + ip_address: str | None = "localhost" + port: int = 50065 + + def __post_init__(self): + if self.color_mode not in ["rgb", "bgr"]: + raise ValueError( + f"`color_mode` is expected to be 'rgb' or 'bgr', but {self.color_mode} is provided." + ) diff --git a/src/lerobot/cameras/reachy2_camera/reachy2_camera.py b/src/lerobot/cameras/reachy2_camera/reachy2_camera.py new file mode 100644 index 000000000..e3d0f4c7b --- /dev/null +++ b/src/lerobot/cameras/reachy2_camera/reachy2_camera.py @@ -0,0 +1,268 @@ +# Copyright 2024 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Provides the OpenCVCamera class for capturing frames from cameras using OpenCV. +""" + +import logging +import math +import os +import platform +import time +from pathlib import Path +from threading import Event, Lock, Thread +from typing import Any + +# Fix MSMF hardware transform compatibility for Windows before importing cv2 +if platform.system() == "Windows" and "OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS" not in os.environ: + os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" +import cv2 +import numpy as np + +from lerobot.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError + +from reachy2_sdk import ReachySDK +from reachy2_sdk.media.camera import CameraView +from reachy2_sdk.media.camera_manager import CameraManager + +from ..camera import Camera +from .configuration_reachy2_camera import ColorMode, Reachy2CameraConfig + + +logger = logging.getLogger(__name__) + + +class Reachy2Camera(Camera): + """ + Manages Reachy 2 camera using Reachy 2 CameraManager. + + This class provides a high-level interface to connect to, configure, and read + frames from Reachy 2 cameras. It supports both synchronous and asynchronous + frame reading. + + An Reachy2Camera instance requires a camera name (e.g., "teleop") and an image + type (e.g., "left") to be specified in the configuration. + + The camera's default settings (FPS, resolution, color mode) are used unless + overridden in the configuration. + """ + + def __init__(self, config: Reachy2CameraConfig): + """ + Initializes the OpenCVCamera instance. + + Args: + config: The configuration settings for the camera. + """ + super().__init__(config) + + self.config = config + + self.fps = config.fps + self.color_mode = config.color_mode + + self.reachy2_sdk: ReachySDK | None = None + + self.thread: Thread | None = None + self.stop_event: Event | None = None + self.frame_lock: Lock = Lock() + self.latest_frame: np.ndarray | None = None + self.new_frame_event: Event = Event() + + def __str__(self) -> str: + return f"{self.__class__.__name__}({self.config.name}, {self.config.image_type})" + + @property + def is_connected(self) -> bool: + """Checks if the camera is currently connected and opened.""" + return (self.reachy.is_connected() and self.reachy.cameras.teleop is not None and self.reachy.cameras.depth is not None) if self.reachy is not None else False + + def connect(self, warmup: bool = True): + """ + Connects to the Reachy2 CameraManager as specified in the configuration. + """ + self.reachy = ReachySDK(self.config.ip_address) + + self.cam_manager = CameraManager(host=self.config.ip_address, port=self.config.port) + self.cam_manager.initialize_cameras() + + logger.info(f"{self} connected.") + print(f"{self} connected.") + + @staticmethod + def find_cameras() -> list[dict[str, Any]]: + """ + Detects available Reachy 2 cameras. + + Returns: + List[Dict[str, Any]]: A list of dictionaries, + where each dictionary contains 'type', 'id' (port index or path), + and the default profile properties (width, height, fps, format). + """ + pass + + def read(self, color_mode: ColorMode | None = None) -> np.ndarray: + """ + Reads a single frame synchronously from the camera. + + This is a blocking call. It waits for the next available frame from the + camera hardware via OpenCV. + + Args: + color_mode (Optional[ColorMode]): If specified, overrides the default + color mode (`self.color_mode`) for this read operation (e.g., + request RGB even if default is BGR). + + Returns: + np.ndarray: The captured frame as a NumPy array in the format + (height, width, channels), using the specified or default + color mode and applying any configured rotation. + """ + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + start_time = time.perf_counter() + + frame = None + + if self.config.name == "teleop" and hasattr(self.cam_manager, "teleop"): + if self.config.image_type == "left": + frame = self.cam_manager.teleop.get_frame(CameraView.LEFT)[0] + elif self.config.image_type == "right": + frame = self.cam_manager.teleop.get_frame(CameraView.RIGHT)[0] + elif self.config.name == "depth" and hasattr(self.cam_manager, "depth"): + if self.config.image_type == "depth": + frame = self.cam_manager.depth.get_depth_frame()[0] + elif self.config.image_type == "rgb": + frame = self.cam_manager.depth.get_frame()[0] + + if frame is None: + return None + + if frame is not None and self.config.color_mode == "rgb": + frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + + read_duration_ms = (time.perf_counter() - start_time) * 1e3 + logger.debug(f"{self} read took: {read_duration_ms:.1f}ms") + + return frame + + def _read_loop(self): + """ + Internal loop run by the background thread for asynchronous reading. + + On each iteration: + 1. Reads a color frame + 2. Stores result in latest_frame (thread-safe) + 3. Sets new_frame_event to notify listeners + + Stops on DeviceNotConnectedError, logs other errors and continues. + """ + while not self.stop_event.is_set(): + try: + color_image = self.read() + + with self.frame_lock: + self.latest_frame = color_image + self.new_frame_event.set() + + except DeviceNotConnectedError: + break + except Exception as e: + logger.warning(f"Error reading frame in background thread for {self}: {e}") + + def _start_read_thread(self) -> None: + """Starts or restarts the background read thread if it's not running.""" + if self.thread is not None and self.thread.is_alive(): + self.thread.join(timeout=0.1) + if self.stop_event is not None: + self.stop_event.set() + + self.stop_event = Event() + self.thread = Thread(target=self._read_loop, args=(), name=f"{self}_read_loop") + self.thread.daemon = True + self.thread.start() + + def _stop_read_thread(self) -> None: + """Signals the background read thread to stop and waits for it to join.""" + if self.stop_event is not None: + self.stop_event.set() + + if self.thread is not None and self.thread.is_alive(): + self.thread.join(timeout=2.0) + + self.thread = None + self.stop_event = None + + def async_read(self, timeout_ms: float = 200) -> np.ndarray: + """ + Reads the latest available frame asynchronously. + + This method retrieves the most recent frame captured by the background + read thread. It does not block waiting for the camera hardware directly, + but may wait up to timeout_ms for the background thread to provide a frame. + + Args: + timeout_ms (float): Maximum time in milliseconds to wait for a frame + to become available. Defaults to 200ms (0.2 seconds). + + Returns: + np.ndarray: The latest captured frame as a NumPy array in the format + (height, width, channels), processed according to configuration. + + Raises: + DeviceNotConnectedError: If the camera is not connected. + TimeoutError: If no frame becomes available within the specified timeout. + RuntimeError: If an unexpected error occurs. + """ + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + if self.thread is None or not self.thread.is_alive(): + self._start_read_thread() + + if not self.new_frame_event.wait(timeout=timeout_ms / 1000.0): + thread_alive = self.thread is not None and self.thread.is_alive() + raise TimeoutError( + f"Timed out waiting for frame from camera {self} after {timeout_ms} ms. " + f"Read thread alive: {thread_alive}." + ) + + with self.frame_lock: + frame = self.latest_frame + self.new_frame_event.clear() + + if frame is None: + raise RuntimeError(f"Internal error: Event set but no frame available for {self}.") + + return frame + + def disconnect(self): + """ + Disconnects from the camera and cleans up resources. + + Stops the background read thread (if running) and releases the OpenCV + VideoCapture object. + + Raises: + DeviceNotConnectedError: If the camera is already disconnected. + """ + if not self.is_connected and self.thread is None: + raise DeviceNotConnectedError(f"{self} not connected.") + + if self.thread is not None: + self._stop_read_thread() + + logger.info(f"{self} disconnected.") diff --git a/src/lerobot/cameras/utils.py b/src/lerobot/cameras/utils.py index 1eb69840b..dfac33e17 100644 --- a/src/lerobot/cameras/utils.py +++ b/src/lerobot/cameras/utils.py @@ -37,8 +37,14 @@ def make_cameras_from_configs(camera_configs: dict[str, CameraConfig]) -> dict[s from .realsense.camera_realsense import RealSenseCamera cameras[key] = RealSenseCamera(cfg) + + elif cfg.type == "reachy2_camera": + from .reachy2_camera.reachy2_camera import Reachy2Camera + + cameras[key] = Reachy2Camera(cfg) + else: - raise ValueError(f"The motor type '{cfg.type}' is not valid.") + raise ValueError(f"The camera type '{cfg.type}' is not valid.") return cameras