diff --git a/.cache/calibration/aloha_default/left_follower.json b/.cache/calibration/aloha_default/left_follower.json deleted file mode 100644 index 336c238a..00000000 --- a/.cache/calibration/aloha_default/left_follower.json +++ /dev/null @@ -1,68 +0,0 @@ -{ - "homing_offset": [ - 2048, - 3072, - 3072, - -1024, - -1024, - 2048, - -2048, - 2048, - -2048 - ], - "drive_mode": [ - 1, - 1, - 1, - 0, - 0, - 1, - 0, - 1, - 0 - ], - "start_pos": [ - 2015, - 3058, - 3061, - 1071, - 1071, - 2035, - 2152, - 2029, - 2499 - ], - "end_pos": [ - -1008, - -1963, - -1966, - 2141, - 2143, - -971, - 3043, - -1077, - 3144 - ], - "calib_mode": [ - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "LINEAR" - ], - "motor_names": [ - "waist", - "shoulder", - "shoulder_shadow", - "elbow", - "elbow_shadow", - "forearm_roll", - "wrist_angle", - "wrist_rotate", - "gripper" - ] -} diff --git a/.cache/calibration/aloha_default/left_leader.json b/.cache/calibration/aloha_default/left_leader.json deleted file mode 100644 index d933f2ba..00000000 --- a/.cache/calibration/aloha_default/left_leader.json +++ /dev/null @@ -1,68 +0,0 @@ -{ - "homing_offset": [ - 2048, - 3072, - 3072, - -1024, - -1024, - 2048, - -2048, - 2048, - -1024 - ], - "drive_mode": [ - 1, - 1, - 1, - 0, - 0, - 1, - 0, - 1, - 0 - ], - "start_pos": [ - 2035, - 3024, - 3019, - 979, - 981, - 1982, - 2166, - 2124, - 1968 - ], - "end_pos": [ - -990, - -2017, - -2015, - 2078, - 2076, - -1030, - 3117, - -1016, - 2556 - ], - "calib_mode": [ - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "LINEAR" - ], - "motor_names": [ - "waist", - "shoulder", - "shoulder_shadow", - "elbow", - "elbow_shadow", - "forearm_roll", - "wrist_angle", - "wrist_rotate", - "gripper" - ] -} diff --git a/.cache/calibration/aloha_default/right_follower.json b/.cache/calibration/aloha_default/right_follower.json deleted file mode 100644 index bc69dfaf..00000000 --- a/.cache/calibration/aloha_default/right_follower.json +++ /dev/null @@ -1,68 +0,0 @@ -{ - "homing_offset": [ - 2048, - 3072, - 3072, - -1024, - -1024, - 2048, - -2048, - 2048, - -2048 - ], - "drive_mode": [ - 1, - 1, - 1, - 0, - 0, - 1, - 0, - 1, - 0 - ], - "start_pos": [ - 2056, - 2895, - 2896, - 1191, - 1190, - 2018, - 2051, - 2056, - 2509 - ], - "end_pos": [ - -1040, - -2004, - -2006, - 2126, - 2127, - -1010, - 3050, - -1117, - 3143 - ], - "calib_mode": [ - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "LINEAR" - ], - "motor_names": [ - "waist", - "shoulder", - "shoulder_shadow", - "elbow", - "elbow_shadow", - "forearm_roll", - "wrist_angle", - "wrist_rotate", - "gripper" - ] -} diff --git a/.cache/calibration/aloha_default/right_leader.json b/.cache/calibration/aloha_default/right_leader.json deleted file mode 100644 index d96d1de9..00000000 --- a/.cache/calibration/aloha_default/right_leader.json +++ /dev/null @@ -1,68 +0,0 @@ -{ - "homing_offset": [ - 2048, - 3072, - 3072, - -1024, - -1024, - 2048, - -2048, - 2048, - -2048 - ], - "drive_mode": [ - 1, - 1, - 1, - 0, - 0, - 1, - 0, - 1, - 0 - ], - "start_pos": [ - 2068, - 3034, - 3030, - 1038, - 1041, - 1991, - 1948, - 2090, - 1985 - ], - "end_pos": [ - -1025, - -2014, - -2015, - 2058, - 2060, - -955, - 3091, - -940, - 2576 - ], - "calib_mode": [ - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "DEGREE", - "LINEAR" - ], - "motor_names": [ - "waist", - "shoulder", - "shoulder_shadow", - "elbow", - "elbow_shadow", - "forearm_roll", - "wrist_angle", - "wrist_rotate", - "gripper" - ] -} diff --git a/.gitattributes b/.gitattributes index 096d1613..7d89f37b 100644 --- a/.gitattributes +++ b/.gitattributes @@ -18,4 +18,4 @@ *.arrow filter=lfs diff=lfs merge=lfs -text *.json !text !filter !merge !diff tests/artifacts/cameras/*.png filter=lfs diff=lfs merge=lfs -text -tests/artifacts/cameras/*.bag filter=lfs diff=lfs merge=lfs -text +*.bag filter=lfs diff=lfs merge=lfs -text diff --git a/.gitignore b/.gitignore index 42f2e755..97b6af2f 100644 --- a/.gitignore +++ b/.gitignore @@ -11,7 +11,10 @@ # 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. + +# Dev scripts .dev + # Logging logs tmp @@ -91,10 +94,8 @@ coverage.xml .hypothesis/ .pytest_cache/ -# Ignore .cache except calibration +# Ignore .cache .cache/* -!.cache/calibration/ -!.cache/calibration/** # Translations *.mo diff --git a/lerobot/calibrate.py b/lerobot/calibrate.py index 38608207..088aee29 100644 --- a/lerobot/calibrate.py +++ b/lerobot/calibrate.py @@ -31,6 +31,8 @@ from pprint import pformat import draccus +from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig # noqa: F401 +from lerobot.common.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401 from lerobot.common.robots import ( # noqa: F401 Robot, RobotConfig, diff --git a/lerobot/common/cameras/__init__.py b/lerobot/common/cameras/__init__.py index 272c4e9e..1488cd89 100644 --- a/lerobot/common/cameras/__init__.py +++ b/lerobot/common/cameras/__init__.py @@ -13,5 +13,5 @@ # limitations under the License. from .camera import Camera -from .configs import CameraConfig +from .configs import CameraConfig, ColorMode, Cv2Rotation from .utils import make_cameras_from_configs diff --git a/lerobot/common/cameras/camera.py b/lerobot/common/cameras/camera.py index 8d88794c..0ef85340 100644 --- a/lerobot/common/cameras/camera.py +++ b/lerobot/common/cameras/camera.py @@ -15,6 +15,7 @@ # limitations under the License. import abc +from typing import Any, Dict, List import numpy as np @@ -22,28 +23,100 @@ from .configs import CameraConfig, ColorMode class Camera(abc.ABC): + """Base class for camera implementations. + + Defines a standard interface for camera operations across different backends. + Subclasses must implement all abstract methods. + + Manages basic camera properties (FPS, resolution) and core operations: + - Connection/disconnection + - Frame capture (sync/async) + + Attributes: + fps (int | None): Configured frames per second + width (int | None): Frame width in pixels + height (int | None): Frame height in pixels + warmup_time (int | None): Time reading frames before returning from connect (in seconds) + + Example: + class MyCamera(Camera): + def __init__(self, config): ... + @property + def is_connected(self) -> bool: ... + def connect(self, warmup=True): ... + # Plus other required methods + """ + def __init__(self, config: CameraConfig): + """Initialize the camera with the given configuration. + + Args: + config: Camera configuration containing FPS and resolution. + """ self.fps: int | None = config.fps self.width: int | None = config.width self.height: int | None = config.height + self.warmup_time: int | None = config.warmup_time @property @abc.abstractmethod def is_connected(self) -> bool: + """Check if the camera is currently connected. + + Returns: + bool: True if the camera is connected and ready to capture frames, + False otherwise. + """ + pass + + @staticmethod + @abc.abstractmethod + def find_cameras() -> List[Dict[str, Any]]: + """Detects available cameras connected to the system. + Returns: + List[Dict[str, Any]]: A list of dictionaries, + where each dictionary contains information about a detected camera. + """ pass @abc.abstractmethod - def connect(self, do_warmup_read: bool = True) -> None: + def connect(self, warmup: bool = True) -> None: + """Establish connection to the camera. + + Args: + warmup: If True (default), captures a warmup frame before returning. Useful + for cameras that require time to adjust capture settings. + If False, skips the warmup frame. + """ pass @abc.abstractmethod def read(self, color_mode: ColorMode | None = None) -> np.ndarray: + """Capture and return a single frame from the camera. + + Args: + color_mode: Desired color mode for the output frame. If None, + uses the camera's default color mode. + + Returns: + np.ndarray: Captured frame as a numpy array. + """ pass @abc.abstractmethod - def async_read(self, timeout_ms: float = 2000) -> np.ndarray: + def async_read(self, timeout_ms: float = ...) -> np.ndarray: + """Asynchronously capture and return a single frame from the camera. + + Args: + timeout_ms: Maximum time to wait for a frame in milliseconds. + Defaults to implementation-specific timeout. + + Returns: + np.ndarray: Captured frame as a numpy array. + """ pass @abc.abstractmethod def disconnect(self) -> None: + """Disconnect from the camera and release resources.""" pass diff --git a/lerobot/common/cameras/configs.py b/lerobot/common/cameras/configs.py index 1f8dda86..6189db4f 100644 --- a/lerobot/common/cameras/configs.py +++ b/lerobot/common/cameras/configs.py @@ -21,12 +21,12 @@ from enum import Enum import draccus -class ColorMode(Enum): +class ColorMode(str, Enum): RGB = "rgb" BGR = "bgr" -class Cv2Rotation(Enum): +class Cv2Rotation(int, Enum): NO_ROTATION = 0 ROTATE_90 = 90 ROTATE_180 = 180 @@ -38,6 +38,7 @@ class CameraConfig(draccus.ChoiceRegistry, abc.ABC): fps: int | None = None width: int | None = None height: int | None = None + warmup_time: int | None = None @property def type(self) -> str: diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index 117b8500..fd252bdc 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -30,10 +30,9 @@ import cv2 import numpy as np from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError -from lerobot.common.utils.utils import capture_timestamp_utc from ..camera import Camera -from ..utils import IndexOrPath, get_cv2_backend, get_cv2_rotation +from ..utils import get_cv2_backend, get_cv2_rotation from .configuration_opencv import ColorMode, OpenCVCameraConfig # NOTE(Steven): The maximum opencv device index depends on your operating system. For instance, @@ -59,20 +58,16 @@ class OpenCVCamera(Camera): or port changes, especially on Linux. Use the provided utility script to find available camera indices or paths: ```bash - python -m lerobot.find_cameras + python -m lerobot.find_cameras opencv ``` The camera's default settings (FPS, resolution, color mode) are used unless overridden in the configuration. - Args: - config (OpenCVCameraConfig): Configuration object containing settings like - camera index/path, desired FPS, width, height, color mode, and rotation. - Example: ```python from lerobot.common.cameras.opencv import OpenCVCamera - from lerobot.common.cameras.configuration_opencv import OpenCVCameraConfig, ColorMode + from lerobot.common.cameras.configuration_opencv import OpenCVCameraConfig, ColorMode, Cv2Rotation # Basic usage with camera index 0 config = OpenCVCameraConfig(index_or_path=0) @@ -97,7 +92,7 @@ class OpenCVCamera(Camera): width=1280, height=720, color_mode=ColorMode.RGB, - rotation=90 + rotation=Cv2Rotation.ROTATE_90 ) custom_camera = OpenCVCamera(custom_config) # ... connect, read, disconnect ... @@ -114,37 +109,79 @@ class OpenCVCamera(Camera): super().__init__(config) self.config = config - self.index_or_path: IndexOrPath = config.index_or_path + self.index_or_path = config.index_or_path - self.fps: int | None = config.fps - self.channels: int = config.channels - self.color_mode: ColorMode = config.color_mode + self.fps = config.fps + self.color_mode = config.color_mode + self.warmup_time = config.warmup_time - self.videocapture_camera: cv2.VideoCapture | None = None + self.videocapture: cv2.VideoCapture | None = None self.thread: Thread | None = None self.stop_event: Event | None = None self.frame_queue: queue.Queue = queue.Queue(maxsize=1) - self.logs: dict = {} # NOTE(Steven): Might be removed in the future - self.rotation: int | None = get_cv2_rotation(config.rotation) - self.backend: int = get_cv2_backend() # NOTE(Steven): If we specify backend the opencv open fails + self.backend: int = get_cv2_backend() if self.height and self.width: if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]: - self.prerotated_width, self.prerotated_height = self.height, self.width + self.capture_width, self.capture_height = self.height, self.width else: - self.prerotated_width, self.prerotated_height = self.width, self.height + self.capture_width, self.capture_height = self.width, self.height def __str__(self) -> str: - """Returns a string representation of the camera instance.""" return f"{self.__class__.__name__}({self.index_or_path})" @property def is_connected(self) -> bool: """Checks if the camera is currently connected and opened.""" - return isinstance(self.videocapture_camera, cv2.VideoCapture) and self.videocapture_camera.isOpened() + return isinstance(self.videocapture, cv2.VideoCapture) and self.videocapture.isOpened() + + def connect(self, warmup: bool = True): + """ + Connects to the OpenCV camera specified in the configuration. + + Initializes the OpenCV VideoCapture object, sets desired camera properties + (FPS, width, height), and performs initial checks. + + Raises: + DeviceAlreadyConnectedError: If the camera is already connected. + ValueError: If the specified camera index/path is not found or accessible. + ConnectionError: If the camera is found but fails to open. + RuntimeError: If the camera opens but fails to apply requested FPS/resolution settings. + """ + if self.is_connected: + raise DeviceAlreadyConnectedError(f"{self} is already connected.") + + # Use 1 thread for OpenCV operations to avoid potential conflicts or + # blocking in multi-threaded applications, especially during data collection. + cv2.setNumThreads(1) + + self.videocapture = cv2.VideoCapture(self.index_or_path, self.backend) + + if not self.videocapture.isOpened(): + self.videocapture.release() + self.videocapture = None + raise ConnectionError( + f"Failed to open OpenCV camera {self.index_or_path}." + f"Run 'python -m lerobot.find_cameras opencv' for details about the available cameras in your system." + ) + + self._configure_capture_settings() + + if warmup: + if self.warmup_time is None: + raise ValueError( + f"Warmup time is not set for {self}. Please set a warmup time in the configuration." + ) + logger.debug(f"Reading a warm-up frames for {self} for {self.warmup_time} seconds...") + start_time = time.time() + while time.time() - start_time < self.warmup_time: + self.read() + time.sleep(0.1) + + logger.debug(f"Camera {self.index_or_path} connected and configured successfully.") def _configure_capture_settings(self) -> None: """ @@ -167,120 +204,65 @@ class OpenCVCamera(Camera): if not self.is_connected: raise DeviceNotConnectedError(f"Cannot configure settings for {self} as it is not connected.") - self._validate_fps() - self._validate_width_and_height() + if self.fps is None: + self.fps = self.videocapture.get(cv2.CAP_PROP_FPS) + logger.info(f"FPS set to camera default: {self.fps}.") + else: + self._validate_fps() - def connect(self, do_warmup_read: bool = True): - """ - Connects to the OpenCV camera specified in the configuration. + default_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH))) + default_height = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_HEIGHT))) - Initializes the OpenCV VideoCapture object, sets desired camera properties - (FPS, width, height), and performs initial checks. - - Raises: - DeviceAlreadyConnectedError: If the camera is already connected. - ValueError: If the specified camera index/path is not found or accessible. - ConnectionError: If the camera is found but fails to open. - RuntimeError: If the camera opens but fails to apply requested FPS/resolution settings. - """ - if self.is_connected: - raise DeviceAlreadyConnectedError(f"{self} is already connected.") - - # Use 1 thread for OpenCV operations to avoid potential conflicts or - # blocking in multi-threaded applications, especially during data collection. - cv2.setNumThreads(1) - - logger.debug(f"Attempting to connect to camera {self.index_or_path} using backend {self.backend}...") - self.videocapture_camera = cv2.VideoCapture(self.index_or_path) - - if not self.videocapture_camera.isOpened(): - self.videocapture_camera.release() - self.videocapture_camera = None - raise ConnectionError( - f"Failed to open OpenCV camera {self.index_or_path}." - f"Run 'python -m find_cameras list-cameras' for details." - ) - - logger.debug(f"Successfully opened camera {self.index_or_path}. Applying configuration...") - self._configure_capture_settings() - - if do_warmup_read: - logger.debug(f"Reading a warm-up frame for {self.index_or_path}...") - self.read() # NOTE(Steven): For now we just read one frame, we could also loop for X frames/secs - - logger.debug(f"Camera {self.index_or_path} connected and configured successfully.") + if self.width is None or self.height is None: + if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]: + self.width, self.height = default_height, default_width + self.capture_width, self.capture_height = default_width, default_height + else: + self.width, self.height = default_width, default_height + self.capture_width, self.capture_height = default_width, default_height + logger.info(f"Capture width set to camera default: {self.width}.") + logger.info(f"Capture height set to camera default: {self.height}.") + else: + self._validate_width_and_height() def _validate_fps(self) -> None: """Validates and sets the camera's frames per second (FPS).""" - if self.fps is None: - self.fps = self.videocapture_camera.get(cv2.CAP_PROP_FPS) - logger.info(f"FPS set to camera default: {self.fps}.") - return - - success = self.videocapture_camera.set(cv2.CAP_PROP_FPS, float(self.fps)) - actual_fps = self.videocapture_camera.get(cv2.CAP_PROP_FPS) + success = self.videocapture.set(cv2.CAP_PROP_FPS, float(self.fps)) + actual_fps = self.videocapture.get(cv2.CAP_PROP_FPS) # Use math.isclose for robust float comparison if not success or not math.isclose(self.fps, actual_fps, rel_tol=1e-3): - logger.warning( - f"Requested FPS {self.fps} for {self}, but camera reported {actual_fps} (set success: {success}). " - "This might be due to camera limitations." - ) raise RuntimeError( - f"Failed to set requested FPS {self.fps} for {self}. Actual value reported: {actual_fps}." + f"Failed to set requested FPS {self.fps} for {self}. Actual value reported: {actual_fps} set success: {success})." ) logger.debug(f"FPS set to {actual_fps} for {self}.") def _validate_width_and_height(self) -> None: """Validates and sets the camera's frame capture width and height.""" - actual_width = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_WIDTH))) - actual_height = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_HEIGHT))) - - if self.width is None or self.height is None: - if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]: - self.width, self.height = actual_height, actual_width - self.prerotated_width, self.prerotated_height = actual_width, actual_height - else: - self.width, self.height = actual_width, actual_height - self.prerotated_width, self.prerotated_height = actual_width, actual_height - logger.info(f"Capture width set to camera default: {self.width}.") - logger.info(f"Capture height set to camera default: {self.height}.") - return - - success = self.videocapture_camera.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.prerotated_width)) - if not success or self.prerotated_width != actual_width: - logger.warning( - f"Requested capture width {self.prerotated_width} for {self}, but camera reported {actual_width} (set success: {success})." - ) + success = self.videocapture.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.capture_width)) + actual_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH))) + if not success or self.capture_width != actual_width: raise RuntimeError( - f"Failed to set requested capture width {self.prerotated_width} for {self}. Actual value: {actual_width}." + f"Failed to set requested capture width {self.capture_width} for {self}. Actual value: {actual_width} (set success: {success})." ) logger.debug(f"Capture width set to {actual_width} for {self}.") - success = self.videocapture_camera.set(cv2.CAP_PROP_FRAME_HEIGHT, float(self.prerotated_height)) - if not success or self.prerotated_height != actual_height: - logger.warning( - f"Requested capture height {self.prerotated_height} for {self}, but camera reported {actual_height} (set success: {success})." - ) + success = self.videocapture.set(cv2.CAP_PROP_FRAME_HEIGHT, float(self.capture_height)) + actual_height = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_HEIGHT))) + if not success or self.capture_height != actual_height: raise RuntimeError( - f"Failed to set requested capture height {self.prerotated_height} for {self}. Actual value: {actual_height}." + f"Failed to set requested capture height {self.capture_height} for {self}. Actual value: {actual_height} (set success: {success})." ) logger.debug(f"Capture height set to {actual_height} for {self}.") @staticmethod - def find_cameras( - max_index_search_range=MAX_OPENCV_INDEX, raise_when_empty: bool = True - ) -> List[Dict[str, Any]]: + def find_cameras() -> List[Dict[str, Any]]: """ Detects available OpenCV cameras connected to the system. On Linux, it scans '/dev/video*' paths. On other systems (like macOS, Windows), - it checks indices from 0 up to `max_index_search_range`. - - Args: - max_index_search_range (int): The maximum index to check on non-Linux systems. - raise_when_empty (bool): If True, raises an OSError if no cameras are found. + it checks indices from 0 up to `MAX_OPENCV_INDEX`. Returns: List[Dict[str, Any]]: A list of dictionaries, @@ -290,16 +272,12 @@ class OpenCVCamera(Camera): found_cameras_info = [] if platform.system() == "Linux": - logger.info("Linux detected. Scanning '/dev/video*' device paths...") possible_paths = sorted(Path("/dev").glob("video*"), key=lambda p: p.name) targets_to_scan = [str(p) for p in possible_paths] - logger.debug(f"Found potential paths: {targets_to_scan}") else: - logger.info( - f"{platform.system()} system detected. Scanning indices from 0 to {max_index_search_range}..." - ) - targets_to_scan = list(range(max_index_search_range)) + targets_to_scan = list(range(MAX_OPENCV_INDEX)) + logger.debug(f"Found potential paths: {targets_to_scan}") for target in targets_to_scan: camera = cv2.VideoCapture(target) if camera.isOpened(): @@ -321,13 +299,10 @@ class OpenCVCamera(Camera): } found_cameras_info.append(camera_info) - logger.debug(f"Found OpenCV camera:: {camera_info}") camera.release() if not found_cameras_info: logger.warning("No OpenCV devices detected.") - if raise_when_empty: - raise OSError("No OpenCV devices detected. Ensure cameras are connected.") logger.info(f"Detected OpenCV cameras: {[cam['id'] for cam in found_cameras_info]}") return found_cameras_info @@ -361,7 +336,7 @@ class OpenCVCamera(Camera): start_time = time.perf_counter() # NOTE(Steven): Are we okay with this blocking an undefined amount of time? - ret, frame = self.videocapture_camera.read() + ret, frame = self.videocapture.read() if not ret or frame is None: raise RuntimeError( @@ -374,7 +349,6 @@ class OpenCVCamera(Camera): read_duration_ms = (time.perf_counter() - start_time) * 1e3 logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms") - self.logs["timestamp_utc"] = capture_timestamp_utc() return processed_frame def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray: @@ -403,13 +377,9 @@ class OpenCVCamera(Camera): h, w, c = image.shape - if h != self.prerotated_height or w != self.prerotated_width: + if h != self.capture_height or w != self.capture_width: raise RuntimeError( - f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.prerotated_height}x{self.prerotated_width}) for {self}." - ) - if c != self.channels: - logger.warning( - f"Captured frame channels ({c}) do not match configured channels ({self.channels}) for {self}." + f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.capture_height}x{self.capture_width}) for {self}." ) processed_image = image @@ -449,7 +419,7 @@ class OpenCVCamera(Camera): logger.debug(f"Stopping read loop thread for {self}.") - def _ensure_read_thread_running(self): + 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) @@ -464,6 +434,22 @@ class OpenCVCamera(Camera): self.thread.start() logger.debug(f"Read thread started for {self}.") + 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) + if self.thread.is_alive(): + logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.") + else: + logger.debug(f"Read thread for {self} joined successfully.") + + self.thread = None + self.stop_event = None + logger.debug(f"Read thread stopped for {self}.") + def async_read(self, timeout_ms: float = 2000) -> np.ndarray: """ Reads the latest available frame asynchronously. @@ -490,41 +476,20 @@ class OpenCVCamera(Camera): raise DeviceNotConnectedError(f"{self} is not connected.") if self.thread is None or not self.thread.is_alive(): - self._ensure_read_thread_running() + self._start_read_thread() try: return self.frame_queue.get(timeout=timeout_ms / 1000.0) except queue.Empty as e: thread_alive = self.thread is not None and self.thread.is_alive() - logger.error( - f"Timeout waiting for frame from {self} queue after {timeout_ms}ms. " - f"(Read thread alive: {thread_alive})" - ) raise TimeoutError( - f"Timed out waiting for frame from camera {self.index_or_path} after {timeout_ms} ms. " + f"Timed out waiting for frame from camera {self} after {timeout_ms} ms. " f"Read thread alive: {thread_alive}." ) from e except Exception as e: logger.exception(f"Unexpected error getting frame from queue for {self}: {e}") raise RuntimeError(f"Error getting frame from queue for camera {self.index_or_path}: {e}") from e - def _shutdown_read_thread(self): - """Signals the background read thread to stop and waits for it to join.""" - if self.stop_event is not None: - logger.debug(f"Signaling stop event for read thread of {self}.") - self.stop_event.set() - - if self.thread is not None and self.thread.is_alive(): - logger.debug(f"Waiting for read thread of {self} to join...") - self.thread.join(timeout=2.0) - if self.thread.is_alive(): - logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.") - else: - logger.debug(f"Read thread for {self} joined successfully.") - - self.thread = None - self.stop_event = None - def disconnect(self): """ Disconnects from the camera and cleans up resources. @@ -536,18 +501,13 @@ class OpenCVCamera(Camera): DeviceNotConnectedError: If the camera is already disconnected. """ if not self.is_connected and self.thread is None: - raise DeviceNotConnectedError( - f"Attempted to disconnect {self}, but it appears already disconnected." - ) - - logger.debug(f"Disconnecting from camera {self.index_or_path}...") + raise DeviceNotConnectedError(f"{self} not connected.") if self.thread is not None: - self._shutdown_read_thread() + self._stop_read_thread() - if self.videocapture_camera is not None: - logger.debug(f"Releasing OpenCV VideoCapture object for {self}.") - self.videocapture_camera.release() - self.videocapture_camera = None + if self.videocapture is not None: + self.videocapture.release() + self.videocapture = None - logger.info(f"Camera {self.index_or_path} disconnected successfully.") + logger.info(f"{self} disconnected.") diff --git a/lerobot/common/cameras/opencv/configuration_opencv.py b/lerobot/common/cameras/opencv/configuration_opencv.py index 5d7c4195..0f781086 100644 --- a/lerobot/common/cameras/opencv/configuration_opencv.py +++ b/lerobot/common/cameras/opencv/configuration_opencv.py @@ -44,7 +44,6 @@ class OpenCVCameraConfig(CameraConfig): 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. - channels: Number of color channels (currently only 3 is supported). rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation. Note: @@ -53,7 +52,6 @@ class OpenCVCameraConfig(CameraConfig): index_or_path: int | Path color_mode: ColorMode = ColorMode.RGB - channels: int = 3 # NOTE(Steven): Why is this a config? rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION def __post_init__(self): @@ -71,6 +69,3 @@ class OpenCVCameraConfig(CameraConfig): raise ValueError( f"`rotation` is expected to be in {(Cv2Rotation.NO_ROTATION, Cv2Rotation.ROTATE_90, Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270)}, but {self.rotation} is provided." ) - - if self.channels != 3: - raise NotImplementedError(f"Unsupported number of channels: {self.channels}") diff --git a/lerobot/common/cameras/intel/__init__.py b/lerobot/common/cameras/realsense/__init__.py similarity index 100% rename from lerobot/common/cameras/intel/__init__.py rename to lerobot/common/cameras/realsense/__init__.py diff --git a/lerobot/common/cameras/intel/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py similarity index 68% rename from lerobot/common/cameras/intel/camera_realsense.py rename to lerobot/common/cameras/realsense/camera_realsense.py index 1f39e299..d06eb3dd 100644 --- a/lerobot/common/cameras/intel/camera_realsense.py +++ b/lerobot/common/cameras/realsense/camera_realsense.py @@ -18,7 +18,6 @@ Provides the RealSenseCamera class for capturing frames from Intel RealSense cam import contextlib import logging -import math import queue import time from threading import Event, Thread @@ -26,10 +25,13 @@ from typing import Any, Dict, List import cv2 import numpy as np -import pyrealsense2 as rs + +try: + import pyrealsense2 as rs +except Exception as e: + logging.info(f"Could not import realsense: {e}") from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError -from lerobot.common.utils.utils import capture_timestamp_utc from ..camera import Camera from ..configs import ColorMode @@ -51,7 +53,7 @@ class RealSenseCamera(Camera): Use the provided utility script to find available camera indices and default profiles: ```bash - python -m lerobot.find_cameras + python -m lerobot.find_cameras realsense ``` A `RealSenseCamera` instance requires a configuration object specifying the @@ -61,53 +63,46 @@ class RealSenseCamera(Camera): The camera's default settings (FPS, resolution, color mode) from the stream profile are used unless overridden in the configuration. - Args: - config (RealSenseCameraConfig): Configuration object containing settings like - serial number or name, desired FPS, width, height, color mode, rotation, - and whether to capture depth. - Example: ```python - from lerobot.common.cameras.intel.camera_realsense import RealSenseCamera - from lerobot.common.cameras.intel.configuration_realsense import RealSenseCameraConfig - from lerobot.common.cameras.configs import ColorMode + from lerobot.common.cameras.realsense import RealSenseCamera, RealSenseCameraConfig + from lerobot.common.cameras import ColorMode, Cv2Rotation # Basic usage with serial number - config = RealSenseCameraConfig(serial_number="1234567890") # Replace with actual SN + config = RealSenseCameraConfig(serial_number_or_name="1234567890") # Replace with actual SN camera = RealSenseCamera(config) - try: - camera.connect() - print(f"Connected to {camera}") - color_image = camera.read() # Synchronous read (color only) - print(f"Read frame shape: {color_image.shape}") - async_image = camera.async_read() # Asynchronous read - print(f"Async read frame shape: {async_image.shape}") - except Exception as e: - print(f"An error occurred: {e}") - finally: - camera.disconnect() - print(f"Disconnected from {camera}") + camera.connect() + + # Read 1 frame synchronously + color_image = camera.read() + print(color_image.shape) + + # Read 1 frame asynchronously + async_image = camera.async_read() + + # When done, properly disconnect the camera using + camera.disconnect() # Example with depth capture and custom settings custom_config = RealSenseCameraConfig( - serial_number="1234567890", # Replace with actual SN + serial_number_or_name="1234567890", # Replace with actual SN fps=30, width=1280, height=720, color_mode=ColorMode.BGR, # Request BGR output - rotation=0, + rotation=Cv2Rotation.NO_ROTATION, use_depth=True ) depth_camera = RealSenseCamera(custom_config) try: depth_camera.connect() - color_image, depth_map = depth_camera.read() # Returns tuple - print(f"Color shape: {color_image.shape}, Depth shape: {depth_map.shape}") + depth_map = depth_camera.read_depth() + print(f"Depth shape: {depth_map.shape}") finally: depth_camera.disconnect() # Example using a unique camera name - name_config = RealSenseCameraConfig(name="Intel RealSense D435") # If unique + name_config = RealSenseCameraConfig(serial_number_or_name="Intel RealSense D435") # If unique name_camera = RealSenseCamera(name_config) # ... connect, read, disconnect ... ``` @@ -125,17 +120,15 @@ class RealSenseCamera(Camera): self.config = config - if config.name is not None: # NOTE(Steven): Do we want to continue supporting this? - self.serial_number = self._find_serial_number_from_name(config.name) - elif config.serial_number is not None: - self.serial_number = str(config.serial_number) + if isinstance(config.serial_number_or_name, int): + self.serial_number = str(config.serial_number_or_name) else: - raise ValueError("RealSenseCameraConfig must provide either 'serial_number' or 'name'.") + self.serial_number = self._find_serial_number_from_name(config.serial_number_or_name) - self.fps: int | None = config.fps - self.channels: int = config.channels - self.color_mode: ColorMode = config.color_mode - self.use_depth: bool = config.use_depth + self.fps = config.fps + self.color_mode = config.color_mode + self.use_depth = config.use_depth + self.warmup_time = config.warmup_time self.rs_pipeline: rs.pipeline | None = None self.rs_profile: rs.pipeline_profile | None = None @@ -144,18 +137,15 @@ class RealSenseCamera(Camera): self.stop_event: Event | None = None self.frame_queue: queue.Queue = queue.Queue(maxsize=1) - self.logs: dict = {} # For timestamping or other metadata - self.rotation: int | None = get_cv2_rotation(config.rotation) if self.height and self.width: if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]: - self.prerotated_width, self.prerotated_height = self.height, self.width + self.capture_width, self.capture_height = self.height, self.width else: - self.prerotated_width, self.prerotated_height = self.width, self.height + self.capture_width, self.capture_height = self.width, self.height def __str__(self) -> str: - """Returns a string representation of the camera instance.""" return f"{self.__class__.__name__}({self.serial_number})" @property @@ -163,36 +153,69 @@ class RealSenseCamera(Camera): """Checks if the camera pipeline is started and streams are active.""" return self.rs_pipeline is not None and self.rs_profile is not None + def connect(self, warmup: bool = True): + """ + Connects to the RealSense camera specified in the configuration. + + Initializes the RealSense pipeline, configures the required streams (color + and optionally depth), starts the pipeline, and validates the actual stream settings. + + Raises: + DeviceAlreadyConnectedError: If the camera is already connected. + ValueError: If the configuration is invalid (e.g., missing serial/name, name not unique). + ConnectionError: If the camera is found but fails to start the pipeline or no RealSense devices are detected at all. + RuntimeError: If the pipeline starts but fails to apply requested settings. + """ + if self.is_connected: + raise DeviceAlreadyConnectedError(f"{self} is already connected.") + + self.rs_pipeline = rs.pipeline() + rs_config = self._make_rs_pipeline_config() + + try: + self.rs_profile = self.rs_pipeline.start(rs_config) + logger.debug(f"Successfully started pipeline for camera {self.serial_number}.") + except RuntimeError as e: + self.rs_profile = None + self.rs_pipeline = None + raise ConnectionError( + f"Failed to open {self} camera. Run 'python -m lerobot.find_cameras realsense' for details about the available cameras in your system." + ) from e + + logger.debug(f"Validating stream configuration for {self}...") + self._validate_capture_settings() + + if warmup: + if self.warmup_time is None: + raise ValueError( + f"Warmup time is not set for {self}. Please set a warmup time in the configuration." + ) + logger.debug(f"Reading a warm-up frames for {self} for {self.warmup_time} seconds...") + start_time = time.time() + while time.time() - start_time < self.warmup_time: + self.read() + time.sleep(0.1) + + logger.info(f"{self} connected.") + @staticmethod - def find_cameras(raise_when_empty: bool = True) -> List[Dict[str, Any]]: + def find_cameras() -> List[Dict[str, Any]]: """ Detects available Intel RealSense cameras connected to the system. - Args: - raise_when_empty (bool): If True, raises an OSError if no cameras are found. - Returns: List[Dict[str, Any]]: A list of dictionaries, where each dictionary contains 'type', 'id' (serial number), 'name', firmware version, USB type, and other available specs, and the default profile properties (width, height, fps, format). Raises: - OSError: If `raise_when_empty` is True and no cameras are detected, - or if pyrealsense2 is not installed. + OSError: If pyrealsense2 is not installed. ImportError: If pyrealsense2 is not installed. """ found_cameras_info = [] context = rs.context() devices = context.query_devices() - if not devices: - logger.warning("No RealSense devices detected.") - if raise_when_empty: - raise OSError( - "No RealSense devices detected. Ensure cameras are connected, " - "library (`pyrealsense2`) is installed, and firmware is up-to-date." - ) - for device in devices: camera_info = { "name": device.get_info(rs.camera_info.name), @@ -223,14 +246,13 @@ class RealSenseCamera(Camera): camera_info["default_stream_profile"] = stream_info found_cameras_info.append(camera_info) - logger.debug(f"Found RealSense camera: {camera_info}") logger.info(f"Detected RealSense cameras: {[cam['id'] for cam in found_cameras_info]}") return found_cameras_info def _find_serial_number_from_name(self, name: str) -> str: """Finds the serial number for a given unique camera name.""" - camera_infos = self.find_cameras(raise_when_empty=True) + camera_infos = self.find_cameras() found_devices = [cam for cam in camera_infos if str(cam["name"]) == name] if not found_devices: @@ -250,24 +272,24 @@ class RealSenseCamera(Camera): logger.info(f"Found serial number '{serial_number}' for camera name '{name}'.") return serial_number - def _configure_realsense_settings(self) -> rs.config: + def _make_rs_pipeline_config(self) -> rs.config: """Creates and configures the RealSense pipeline configuration object.""" rs_config = rs.config() rs.config.enable_device(rs_config, self.serial_number) if self.width and self.height and self.fps: logger.debug( - f"Requesting Color Stream: {self.prerotated_width}x{self.prerotated_height} @ {self.fps} FPS, Format: {rs.format.rgb8}" + f"Requesting Color Stream: {self.capture_width}x{self.capture_height} @ {self.fps} FPS, Format: {rs.format.rgb8}" ) rs_config.enable_stream( - rs.stream.color, self.prerotated_width, self.prerotated_height, rs.format.rgb8, self.fps + rs.stream.color, self.capture_width, self.capture_height, rs.format.rgb8, self.fps ) if self.use_depth: logger.debug( - f"Requesting Depth Stream: {self.prerotated_width}x{self.prerotated_height} @ {self.fps} FPS, Format: {rs.format.z16}" + f"Requesting Depth Stream: {self.capture_width}x{self.capture_height} @ {self.fps} FPS, Format: {rs.format.z16}" ) rs_config.enable_stream( - rs.stream.depth, self.prerotated_width, self.prerotated_height, rs.format.z16, self.fps + rs.stream.depth, self.capture_width, self.capture_height, rs.format.z16, self.fps ) else: logger.debug(f"Requesting Color Stream: Default settings, Format: {rs.stream.color}") @@ -295,111 +317,47 @@ class RealSenseCamera(Camera): if not self.is_connected: raise DeviceNotConnectedError(f"Cannot validate settings for {self} as it is not connected.") - self._validate_fps(self.rs_profile.get_stream(rs.stream.color).as_video_stream_profile()) - self._validate_width_and_height(self.rs_profile.get_stream(rs.stream.color).as_video_stream_profile()) - - if self.use_depth: - self._validate_fps(self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile()) - self._validate_width_and_height( - self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile() - ) - - def connect(self, do_warmup_read: bool = True): - """ - Connects to the RealSense camera specified in the configuration. - - Initializes the RealSense pipeline, configures the required streams (color - and optionally depth), starts the pipeline, and validates the actual stream settings. - - Raises: - DeviceAlreadyConnectedError: If the camera is already connected. - ValueError: If the configuration is invalid (e.g., missing serial/name, name not unique). - ConnectionError: If the camera is found but fails to start the pipeline. - RuntimeError: If the pipeline starts but fails to apply requested settings. - OSError: If no RealSense devices are detected at all. - """ - if self.is_connected: - raise DeviceAlreadyConnectedError(f"{self} is already connected.") - - logger.debug(f"Attempting to connect to camera {self.serial_number}...") - self.rs_pipeline = rs.pipeline() - rs_config = self._configure_realsense_settings() - - try: - self.rs_profile = self.rs_pipeline.start(rs_config) - logger.debug(f"Successfully started pipeline for camera {self.serial_number}.") - except RuntimeError as e: - self.rs_profile = None - self.rs_pipeline = None - raise ConnectionError( - f"Failed to open RealSense camera {self.serial_number}. Error: {e}. " - f"Run 'python -m find_cameras list-cameras' for details." - ) from e - - logger.debug(f"Validating stream configuration for {self.serial_number}...") - self._validate_capture_settings() - - if do_warmup_read: - logger.debug(f"Reading a warm-up frame for {self.serial_number}...") - self.read() # NOTE(Steven): For now we just read one frame, we could also loop for X frames/secs - - logger.info(f"Camera {self.serial_number} connected and configured successfully.") - - def _validate_fps(self, stream) -> None: - """Validates and sets the internal FPS based on actual stream FPS.""" - actual_fps = stream.fps() + stream = self.rs_profile.get_stream(rs.stream.color).as_video_stream_profile() if self.fps is None: - self.fps = actual_fps - logger.info(f"FPS not specified, using camera default: {self.fps} FPS.") - return + self.fps = stream.fps() - # Use math.isclose for robust float comparison - if not math.isclose(self.fps, actual_fps, rel_tol=1e-3): - logger.warning( - f"Requested FPS {self.fps} for {self}, but camera reported {actual_fps}. " - "This might be due to camera limitations." - ) - raise RuntimeError( - f"Failed to set requested FPS {self.fps} for {self}. Actual value reported: {actual_fps}." - ) - logger.debug(f"FPS set to {actual_fps} for {self}.") + if self.width is None or self.height is None: + actual_width = int(round(stream.width())) + actual_height = int(round(stream.height())) + if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]: + self.width, self.height = actual_height, actual_width + self.capture_width, self.capture_height = actual_width, actual_height + else: + self.width, self.height = actual_width, actual_height + self.capture_width, self.capture_height = actual_width, actual_height + logger.info(f"Capture width set to camera default: {self.width}.") + logger.info(f"Capture height set to camera default: {self.height}.") + else: + self._validate_width_and_height(stream) + + if self.use_depth: + stream = self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile() + self._validate_width_and_height(stream) def _validate_width_and_height(self, stream) -> None: """Validates and sets the internal capture width and height based on actual stream width.""" actual_width = int(round(stream.width())) actual_height = int(round(stream.height())) - if self.width is None or self.height is None: - if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]: - self.width, self.height = actual_height, actual_width - self.prerotated_width, self.prerotated_height = actual_width, actual_height - else: - self.width, self.height = actual_width, actual_height - self.prerotated_width, self.prerotated_height = actual_width, actual_height - logger.info(f"Capture width set to camera default: {self.width}.") - logger.info(f"Capture height set to camera default: {self.height}.") - return - - if self.prerotated_width != actual_width: - logger.warning( - f"Requested capture width {self.prerotated_width} for {self}, but camera reported {actual_width}." - ) + if self.capture_width != actual_width: raise RuntimeError( - f"Failed to set requested capture width {self.prerotated_width} for {self}. Actual value: {actual_width}." + f"Failed to set requested capture width {self.capture_width} for {self}. Actual value: {actual_width}." ) logger.debug(f"Capture width set to {actual_width} for {self}.") - if self.prerotated_height != actual_height: - logger.warning( - f"Requested capture height {self.prerotated_height} for {self}, but camera reported {actual_height}." - ) + if self.capture_height != actual_height: raise RuntimeError( - f"Failed to set requested capture height {self.prerotated_height} for {self}. Actual value: {actual_height}." + f"Failed to set requested capture height {self.capture_height} for {self}. Actual value: {actual_height}." ) logger.debug(f"Capture height set to {actual_height} for {self}.") - def read_depth(self, timeout_ms: int = 5000) -> np.ndarray: + def read_depth(self, timeout_ms: int = 100) -> np.ndarray: """ Reads a single frame (depth) synchronously from the camera. @@ -407,7 +365,7 @@ class RealSenseCamera(Camera): from the camera hardware via the RealSense pipeline. Args: - timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 5000ms. + timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 100ms. Returns: np.ndarray: The depth map as a NumPy array (height, width) @@ -420,35 +378,29 @@ class RealSenseCamera(Camera): if not self.is_connected: raise DeviceNotConnectedError(f"{self} is not connected.") - if not self.use_depth: raise RuntimeError( - f"Failed to capture depth frame from {self}. '.read_depth()'. Depth stream is not enabled." + f"Failed to capture depth frame '.read_depth()'. Depth stream is not enabled for {self}." ) start_time = time.perf_counter() - ret, frame = self.rs_pipeline.try_wait_for_frames( - timeout_ms=timeout_ms - ) # NOTE(Steven): This read has a timeout + ret, frame = self.rs_pipeline.try_wait_for_frames(timeout_ms=timeout_ms) if not ret or frame is None: - raise RuntimeError( - f"Failed to capture frame from {self}. '.read_depth()' returned status={ret} and frame is None." - ) + raise RuntimeError(f"{self} failed to capture frame. Returned status='{ret}'.") depth_frame = frame.get_depth_frame() depth_map = np.asanyarray(depth_frame.get_data()) - depth_map_processed = self._postprocess_image(depth_map) + depth_map_processed = self._postprocess_image(depth_map, depth_frame=True) read_duration_ms = (time.perf_counter() - start_time) * 1e3 logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms") - self.logs["timestamp_utc"] = capture_timestamp_utc() return depth_map_processed - def read(self, color_mode: ColorMode | None = None, timeout_ms: int = 5000) -> np.ndarray: + def read(self, color_mode: ColorMode | None = None, timeout_ms: int = 100) -> np.ndarray: """ Reads a single frame (color) synchronously from the camera. @@ -456,7 +408,7 @@ class RealSenseCamera(Camera): from the camera hardware via the RealSense pipeline. Args: - timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 5000ms. + timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 100ms. Returns: np.ndarray: The captured color frame as a NumPy array @@ -490,10 +442,11 @@ class RealSenseCamera(Camera): read_duration_ms = (time.perf_counter() - start_time) * 1e3 logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms") - self.logs["timestamp_utc"] = capture_timestamp_utc() return color_image_processed - def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray: + def _postprocess_image( + self, image: np.ndarray, color_mode: ColorMode | None = None, depth_frame: bool = False + ) -> np.ndarray: """ Applies color conversion, dimension validation, and rotation to a raw color frame. @@ -516,15 +469,14 @@ class RealSenseCamera(Camera): f"Invalid requested color mode '{color_mode}'. Expected {ColorMode.RGB} or {ColorMode.BGR}." ) - h, w, c = image.shape + if depth_frame: + h, w = image.shape + else: + h, w, _c = image.shape - if h != self.prerotated_height or w != self.prerotated_width: + if h != self.capture_height or w != self.capture_width: raise RuntimeError( - f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.prerotated_height}x{self.prerotated_width}) for {self}." - ) - if c != self.channels: - logger.warning( - f"Captured frame channels ({c}) do not match configured channels ({self.channels}) for {self}." + f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.capture_height}x{self.capture_width}) for {self}." ) processed_image = image @@ -564,7 +516,7 @@ class RealSenseCamera(Camera): logger.debug(f"Stopping read loop thread for {self}.") - def _ensure_read_thread_running(self): + 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) @@ -572,15 +524,29 @@ class RealSenseCamera(Camera): self.stop_event.set() self.stop_event = Event() - self.thread = Thread( - target=self._read_loop, args=(), name=f"RealSenseReadLoop-{self}-{self.serial_number}" - ) + self.thread = Thread(target=self._read_loop, args=(), name=f"{self}_read_loop") self.thread.daemon = True self.thread.start() logger.debug(f"Read thread started for {self}.") + def _stop_read_thread(self): + """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) + if self.thread.is_alive(): + logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.") + else: + logger.debug(f"Read thread for {self} joined successfully.") + + self.thread = None + self.stop_event = None + logger.debug(f"Read thread stopped for {self}.") + # NOTE(Steven): Missing implementation for depth for now - def async_read(self, timeout_ms: float = 2000) -> np.ndarray: + def async_read(self, timeout_ms: float = 100) -> np.ndarray: """ Reads the latest available frame data (color or color+depth) asynchronously. @@ -591,7 +557,7 @@ class RealSenseCamera(Camera): Args: timeout_ms (float): Maximum time in milliseconds to wait for a frame - to become available in the queue. Defaults to 2000ms (2 seconds). + to become available in the queue. Defaults to 100ms (0.1 seconds). Returns: np.ndarray: @@ -606,42 +572,18 @@ class RealSenseCamera(Camera): raise DeviceNotConnectedError(f"{self} is not connected.") if self.thread is None or not self.thread.is_alive(): - self._ensure_read_thread_running() + self._start_read_thread() try: return self.frame_queue.get(timeout=timeout_ms / 1000.0) except queue.Empty as e: thread_alive = self.thread is not None and self.thread.is_alive() - logger.error( - f"Timeout waiting for frame from {self} queue after {timeout_ms}ms. " - f"(Read thread alive: {thread_alive})" - ) raise TimeoutError( - f"Timed out waiting for frame from camera {self.serial_number} after {timeout_ms} ms. " + f"Timed out waiting for frame from camera {self} after {timeout_ms} ms. " f"Read thread alive: {thread_alive}." ) from e except Exception as e: - logger.exception(f"Unexpected error getting frame data from queue for {self}: {e}") - raise RuntimeError( - f"Error getting frame data from queue for camera {self.serial_number}: {e}" - ) from e - - def _shutdown_read_thread(self): - """Signals the background read thread to stop and waits for it to join.""" - if self.stop_event is not None: - logger.debug(f"Signaling stop event for read thread of {self}.") - self.stop_event.set() - - if self.thread is not None and self.thread.is_alive(): - logger.debug(f"Waiting for read thread of {self} to join...") - self.thread.join(timeout=2.0) - if self.thread.is_alive(): - logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.") - else: - logger.debug(f"Read thread for {self} joined successfully.") - - self.thread = None - self.stop_event = None + raise RuntimeError(f"Error getting frame data from queue for camera {self}: {e}") from e def disconnect(self): """ @@ -658,10 +600,8 @@ class RealSenseCamera(Camera): f"Attempted to disconnect {self}, but it appears already disconnected." ) - logger.debug(f"Disconnecting from camera {self.serial_number}...") - if self.thread is not None: - self._shutdown_read_thread() + self._stop_read_thread() if self.rs_pipeline is not None: logger.debug(f"Stopping RealSense pipeline object for {self}.") @@ -669,4 +609,4 @@ class RealSenseCamera(Camera): self.rs_pipeline = None self.rs_profile = None - logger.info(f"Camera {self.serial_number} disconnected successfully.") + logger.info(f"{self} disconnected.") diff --git a/lerobot/common/cameras/intel/configuration_realsense.py b/lerobot/common/cameras/realsense/configuration_realsense.py similarity index 79% rename from lerobot/common/cameras/intel/configuration_realsense.py rename to lerobot/common/cameras/realsense/configuration_realsense.py index 5d9f8a80..64c7063e 100644 --- a/lerobot/common/cameras/intel/configuration_realsense.py +++ b/lerobot/common/cameras/realsense/configuration_realsense.py @@ -40,25 +40,20 @@ class RealSenseCameraConfig(CameraConfig): 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. - name: Optional human-readable name to identify the camera. - serial_number: Optional unique serial number to identify the camera. - Either name or serial_number must be provided. + serial_number_or_name: Unique serial number or human-readable name to identify the camera. color_mode: Color mode for image output (RGB or BGR). Defaults to RGB. - channels: Number of color channels (currently only 3 is supported). use_depth: Whether to enable depth stream. Defaults to False. rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation. Note: - - Either name or serial_number must be specified, but not both. + - Either name or serial_number must be specified. - Depth stream configuration (if enabled) will use the same FPS as the color stream. - The actual resolution and FPS may be adjusted by the camera to the nearest supported mode. - - Only 3-channel color output (RGB/BGR) is currently supported. + - For `fps`, `width` and `height`, either all of them need to be set, or none of them. """ - name: str | None = None - serial_number: int | None = None + serial_number_or_name: int | str color_mode: ColorMode = ColorMode.RGB - channels: int | None = 3 use_depth: bool = False rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION # NOTE(Steven): Check if draccus can parse to an enum @@ -78,10 +73,8 @@ class RealSenseCameraConfig(CameraConfig): f"`rotation` is expected to be in {(Cv2Rotation.NO_ROTATION, Cv2Rotation.ROTATE_90, Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270)}, but {self.rotation} is provided." ) - if self.channels != 3: - raise NotImplementedError(f"Unsupported number of channels: {self.channels}") - - if bool(self.name) and bool(self.serial_number): + values = (self.fps, self.width, self.height) + if any(v is not None for v in values) and any(v is None for v in values): raise ValueError( - f"One of them must be set: name or serial_number, but {self.name=} and {self.serial_number=} provided." + "For `fps`, `width` and `height`, either all of them need to be set, or none of them." ) diff --git a/lerobot/common/cameras/utils.py b/lerobot/common/cameras/utils.py index 30f5dd69..72f1dd3e 100644 --- a/lerobot/common/cameras/utils.py +++ b/lerobot/common/cameras/utils.py @@ -37,7 +37,7 @@ def make_cameras_from_configs(camera_configs: dict[str, CameraConfig]) -> dict[s cameras[key] = OpenCVCamera(cfg) elif cfg.type == "intelrealsense": - from .intel.camera_realsense import RealSenseCamera + from .realsense.camera_realsense import RealSenseCamera cameras[key] = RealSenseCamera(cfg) else: @@ -46,24 +46,26 @@ def make_cameras_from_configs(camera_configs: dict[str, CameraConfig]) -> dict[s return cameras -def get_cv2_rotation(rotation: Cv2Rotation) -> int: +def get_cv2_rotation(rotation: Cv2Rotation) -> int | None: import cv2 - return { - Cv2Rotation.ROTATE_270: cv2.ROTATE_90_COUNTERCLOCKWISE, - Cv2Rotation.ROTATE_90: cv2.ROTATE_90_CLOCKWISE, - Cv2Rotation.ROTATE_180: cv2.ROTATE_180, - }.get(rotation) + if rotation == Cv2Rotation.ROTATE_90: + return cv2.ROTATE_90_CLOCKWISE + elif rotation == Cv2Rotation.ROTATE_180: + return cv2.ROTATE_180 + elif rotation == Cv2Rotation.ROTATE_270: + return cv2.ROTATE_90_COUNTERCLOCKWISE + else: + return None def get_cv2_backend() -> int: import cv2 - return { - "Linux": cv2.CAP_DSHOW, - "Windows": cv2.CAP_AVFOUNDATION, - "Darwin": cv2.CAP_ANY, - }.get(platform.system(), cv2.CAP_V4L2) + if platform.system() == "Windows": + return cv2.CAP_AVFOUNDATION + else: + return cv2.CAP_ANY def save_image(img_array: np.ndarray, camera_index: int, frame_index: int, images_dir: Path): diff --git a/lerobot/common/envs/configs.py b/lerobot/common/envs/configs.py index 6de3cf03..c99fba81 100644 --- a/lerobot/common/envs/configs.py +++ b/lerobot/common/envs/configs.py @@ -32,7 +32,8 @@ class EnvConfig(draccus.ChoiceRegistry, abc.ABC): def type(self) -> str: return self.get_choice_name(self.__class__) - @abc.abstractproperty + @property + @abc.abstractmethod def gym_kwargs(self) -> dict: raise NotImplementedError() diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index 2a444d45..3801226a 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/lerobot/common/motors/motors_bus.py @@ -749,7 +749,10 @@ class MotorsBus(abc.ABC): # Move cursor up to overwrite the previous output move_cursor_up(len(motors) + 3) - # TODO(Steven, aliberts): add check to ensure mins and maxes are different + same_min_max = [motor for motor in motors if mins[motor] == maxes[motor]] + if same_min_max: + raise ValueError(f"Some motors have the same min and max values:\n{pformat(same_min_max)}") + return mins, maxes def _normalize(self, data_name: str, ids_values: dict[int, int]) -> dict[int, float]: diff --git a/lerobot/common/robots/config.py b/lerobot/common/robots/config.py index 31fd0bf5..a85a8316 100644 --- a/lerobot/common/robots/config.py +++ b/lerobot/common/robots/config.py @@ -27,15 +27,13 @@ class RobotConfig(draccus.ChoiceRegistry, abc.ABC): calibration_dir: Path | None = None def __post_init__(self): - if hasattr(self, "cameras"): - cameras = self.cameras - if cameras: - for cam_name, cam_config in cameras.items(): - for attr in ["width", "height", "fps"]: - if getattr(cam_config, attr) is None: - raise ValueError( - f"Camera config for '{cam_name}' has None value for required attribute '{attr}'" - ) + if hasattr(self, "cameras") and self.cameras: + for _, config in self.cameras.items(): + for attr in ["width", "height", "fps"]: + if getattr(config, attr) is None: + raise ValueError( + f"Specifying '{attr}' is required for the camera to be used in a robot" + ) @property def type(self) -> str: diff --git a/lerobot/common/robots/lekiwi/lekiwi.py b/lerobot/common/robots/lekiwi/lekiwi.py index 02474898..986b9d1e 100644 --- a/lerobot/common/robots/lekiwi/lekiwi.py +++ b/lerobot/common/robots/lekiwi/lekiwi.py @@ -16,6 +16,7 @@ import logging import time +from itertools import chain from typing import Any from lerobot.common.cameras.utils import make_cameras_from_configs @@ -183,6 +184,12 @@ class LeKiwi(Robot): self.bus.enable_torque() + def setup_motors(self) -> None: + for motor in chain(reversed(self.arm_motors), reversed(self.base_motors)): + input(f"Connect the controller board to the '{motor}' motor only and press enter.") + self.bus.setup_motor(motor) + print(f"'{motor}' motor id set to {self.bus.motors[motor].id}") + def get_observation(self) -> dict[str, Any]: if not self.is_connected: raise DeviceNotConnectedError(f"{self} is not connected.") diff --git a/lerobot/common/robots/robot.py b/lerobot/common/robots/robot.py index 7368863e..e5af9e79 100644 --- a/lerobot/common/robots/robot.py +++ b/lerobot/common/robots/robot.py @@ -49,15 +49,18 @@ class Robot(abc.ABC): return f"{self.id} {self.__class__.__name__}" # TODO(aliberts): create a proper Feature class for this that links with datasets - @abc.abstractproperty + @property + @abc.abstractmethod def observation_features(self) -> dict: pass - @abc.abstractproperty + @property + @abc.abstractmethod def action_features(self) -> dict: pass - @abc.abstractproperty + @property + @abc.abstractmethod def is_connected(self) -> bool: pass @@ -66,7 +69,8 @@ class Robot(abc.ABC): """Connects to the robot.""" pass - @abc.abstractproperty + @property + @abc.abstractmethod def is_calibrated(self) -> bool: pass diff --git a/lerobot/common/robots/stretch3/configuration_stretch3.py b/lerobot/common/robots/stretch3/configuration_stretch3.py index 2c0a8b2c..e62e4fa0 100644 --- a/lerobot/common/robots/stretch3/configuration_stretch3.py +++ b/lerobot/common/robots/stretch3/configuration_stretch3.py @@ -15,8 +15,8 @@ from dataclasses import dataclass, field from lerobot.common.cameras import CameraConfig -from lerobot.common.cameras.intel import RealSenseCameraConfig from lerobot.common.cameras.opencv import OpenCVCameraConfig +from lerobot.common.cameras.realsense import RealSenseCameraConfig from ..config import RobotConfig diff --git a/lerobot/common/teleoperators/teleoperator.py b/lerobot/common/teleoperators/teleoperator.py index 5a1ed184..d8715a55 100644 --- a/lerobot/common/teleoperators/teleoperator.py +++ b/lerobot/common/teleoperators/teleoperator.py @@ -47,15 +47,18 @@ class Teleoperator(abc.ABC): def __str__(self) -> str: return f"{self.id} {self.__class__.__name__}" - @abc.abstractproperty + @property + @abc.abstractmethod def action_features(self) -> dict: pass - @abc.abstractproperty + @property + @abc.abstractmethod def feedback_features(self) -> dict: pass - @abc.abstractproperty + @property + @abc.abstractmethod def is_connected(self) -> bool: pass @@ -64,7 +67,8 @@ class Teleoperator(abc.ABC): """Connects to the teleoperator.""" pass - @abc.abstractproperty + @property + @abc.abstractmethod def is_calibrated(self) -> bool: pass diff --git a/lerobot/common/utils/control_utils.py b/lerobot/common/utils/control_utils.py index 730144f3..3ac792a5 100644 --- a/lerobot/common/utils/control_utils.py +++ b/lerobot/common/utils/control_utils.py @@ -38,7 +38,6 @@ from lerobot.common.utils.robot_utils import busy_wait from lerobot.common.utils.utils import get_safe_torch_device, has_method -# NOTE(Steven): Consider integrating this in camera class def log_control_info(robot: Robot, dt_s, episode_index=None, frame_index=None, fps=None): log_items = [] if episode_index is not None: diff --git a/lerobot/configs/policies.py b/lerobot/configs/policies.py index 22eae05f..1302db1f 100644 --- a/lerobot/configs/policies.py +++ b/lerobot/configs/policies.py @@ -78,15 +78,18 @@ class PreTrainedConfig(draccus.ChoiceRegistry, HubMixin, abc.ABC): def type(self) -> str: return self.get_choice_name(self.__class__) - @abc.abstractproperty + @property + @abc.abstractmethod def observation_delta_indices(self) -> list | None: raise NotImplementedError - @abc.abstractproperty + @property + @abc.abstractmethod def action_delta_indices(self) -> list | None: raise NotImplementedError - @abc.abstractproperty + @property + @abc.abstractmethod def reward_delta_indices(self) -> list | None: raise NotImplementedError diff --git a/lerobot/find_cameras.py b/lerobot/find_cameras.py index 6227eaba..5cff0fe5 100644 --- a/lerobot/find_cameras.py +++ b/lerobot/find_cameras.py @@ -14,6 +14,16 @@ # See the License for the specific language governing permissions and # limitations under the License. +""" +Helper to find the camera devices available in your system. + +Example: + +```shell +python -m lerobot.find_cameras +``` +""" + import argparse import concurrent.futures import logging @@ -26,13 +36,12 @@ import numpy as np from PIL import Image from lerobot.common.cameras.configs import ColorMode -from lerobot.common.cameras.intel.camera_realsense import RealSenseCamera -from lerobot.common.cameras.intel.configuration_realsense import RealSenseCameraConfig from lerobot.common.cameras.opencv.camera_opencv import OpenCVCamera from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig +from lerobot.common.cameras.realsense.camera_realsense import RealSenseCamera +from lerobot.common.cameras.realsense.configuration_realsense import RealSenseCameraConfig logger = logging.getLogger(__name__) -# logging.basicConfig(level=logging.INFO, format="%(asctime)s - %(levelname)s - %(module)s - %(message)s") def find_all_opencv_cameras() -> List[Dict[str, Any]]: @@ -45,7 +54,7 @@ def find_all_opencv_cameras() -> List[Dict[str, Any]]: all_opencv_cameras_info: List[Dict[str, Any]] = [] logger.info("Searching for OpenCV cameras...") try: - opencv_cameras = OpenCVCamera.find_cameras(raise_when_empty=False) + opencv_cameras = OpenCVCamera.find_cameras() for cam_info in opencv_cameras: all_opencv_cameras_info.append(cam_info) logger.info(f"Found {len(opencv_cameras)} OpenCV cameras.") @@ -65,7 +74,7 @@ def find_all_realsense_cameras() -> List[Dict[str, Any]]: all_realsense_cameras_info: List[Dict[str, Any]] = [] logger.info("Searching for RealSense cameras...") try: - realsense_cameras = RealSenseCamera.find_cameras(raise_when_empty=False) + realsense_cameras = RealSenseCamera.find_cameras() for cam_info in realsense_cameras: all_realsense_cameras_info.append(cam_info) logger.info(f"Found {len(realsense_cameras)} RealSense cameras.") @@ -170,7 +179,7 @@ def create_camera_instance(cam_meta: Dict[str, Any]) -> Dict[str, Any] | None: instance = OpenCVCamera(cv_config) elif cam_type == "RealSense": rs_config = RealSenseCameraConfig( - serial_number=str(cam_id), + serial_number_or_name=int(cam_id), color_mode=ColorMode.RGB, ) instance = RealSenseCamera(rs_config) @@ -180,7 +189,7 @@ def create_camera_instance(cam_meta: Dict[str, Any]) -> Dict[str, Any] | None: if instance: logger.info(f"Connecting to {cam_type} camera: {cam_id}...") - instance.connect(do_warmup_read=False) + instance.connect(warmup=False) return {"instance": instance, "meta": cam_meta} except Exception as e: logger.error(f"Failed to connect or configure {cam_type} camera {cam_id}: {e}") @@ -293,28 +302,8 @@ if __name__ == "__main__": parser = argparse.ArgumentParser( description="Unified camera utility script for listing cameras and capturing images." ) - subparsers = parser.add_subparsers(dest="command", help="Available commands") - # List cameras command - list_parser = subparsers.add_parser( - "list-cameras", help="Shows connected cameras. Optionally filter by type (realsense or opencv)." - ) - list_parser.add_argument( - "camera_type", - type=str, - nargs="?", - default=None, - choices=["realsense", "opencv"], - help="Specify camera type to list (e.g., 'realsense', 'opencv'). Lists all if omitted.", - ) - list_parser.set_defaults(func=lambda args: find_and_print_cameras(args.camera_type)) - - # Capture images command - capture_parser = subparsers.add_parser( - "capture-images", - help="Saves images from detected cameras (optionally filtered by type) using their default stream profiles.", - ) - capture_parser.add_argument( + parser.add_argument( "camera_type", type=str, nargs="?", @@ -322,19 +311,19 @@ if __name__ == "__main__": choices=["realsense", "opencv"], help="Specify camera type to capture from (e.g., 'realsense', 'opencv'). Captures from all if omitted.", ) - capture_parser.add_argument( + parser.add_argument( "--output-dir", type=Path, default="outputs/captured_images", help="Directory to save images. Default: outputs/captured_images", ) - capture_parser.add_argument( + parser.add_argument( "--record-time-s", type=float, default=6.0, help="Time duration to attempt capturing frames. Default: 6 seconds.", ) - capture_parser.set_defaults( + parser.set_defaults( func=lambda args: save_images_from_all_cameras( output_dir=args.output_dir, record_time_s=args.record_time_s, @@ -344,14 +333,4 @@ if __name__ == "__main__": args = parser.parse_args() - if args.command is None: - default_output_dir = capture_parser.get_default("output_dir") - default_record_time_s = capture_parser.get_default("record_time_s") - - save_images_from_all_cameras( - output_dir=default_output_dir, - record_time_s=default_record_time_s, - camera_type_filter=None, - ) - else: - args.func(args) + args.func(args) diff --git a/lerobot/record.py b/lerobot/record.py index d4103e36..508b0929 100644 --- a/lerobot/record.py +++ b/lerobot/record.py @@ -45,6 +45,8 @@ import rerun as rr from lerobot.common.cameras import ( # noqa: F401 CameraConfig, # noqa: F401 ) +from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig # noqa: F401 +from lerobot.common.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401 from lerobot.common.datasets.image_writer import safe_stop_image_writer from lerobot.common.datasets.lerobot_dataset import LeRobotDataset from lerobot.common.datasets.utils import build_dataset_frame, hw_to_dataset_features diff --git a/lerobot/teleoperate.py b/lerobot/teleoperate.py index e7e617fb..f3496a69 100644 --- a/lerobot/teleoperate.py +++ b/lerobot/teleoperate.py @@ -38,6 +38,8 @@ import draccus import numpy as np import rerun as rr +from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig # noqa: F401 +from lerobot.common.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401 from lerobot.common.robots import ( # noqa: F401 Robot, RobotConfig, diff --git a/pyproject.toml b/pyproject.toml index 381f5bbc..c4faa400 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -86,7 +86,7 @@ dynamixel = ["dynamixel-sdk>=3.7.31"] feetech = ["feetech-servo-sdk>=1.0.0"] intelrealsense = [ "pyrealsense2>=2.55.1.6486 ; sys_platform != 'darwin'", - "pyrealsense2-macosx>=2.54 ; sys_platform == 'darwin'", #NOTE(Steven): Check previous version for sudo issue + "pyrealsense2-macosx>=2.54 ; sys_platform == 'darwin'", ] pi0 = ["transformers>=4.48.0"] pusht = ["gym-pusht>=0.1.5 ; python_version < '4.0'"] diff --git a/tests/artifacts/cameras/fakecam_square_128x128.png b/tests/artifacts/cameras/image_128x128.png similarity index 100% rename from tests/artifacts/cameras/fakecam_square_128x128.png rename to tests/artifacts/cameras/image_128x128.png diff --git a/tests/artifacts/cameras/fakecam_sd_160x120.png b/tests/artifacts/cameras/image_160x120.png similarity index 100% rename from tests/artifacts/cameras/fakecam_sd_160x120.png rename to tests/artifacts/cameras/image_160x120.png diff --git a/tests/artifacts/cameras/fakecam_hd_320x180.png b/tests/artifacts/cameras/image_320x180.png similarity index 100% rename from tests/artifacts/cameras/fakecam_hd_320x180.png rename to tests/artifacts/cameras/image_320x180.png diff --git a/tests/artifacts/cameras/fakecam_fullhd_480x270.png b/tests/artifacts/cameras/image_480x270.png similarity index 100% rename from tests/artifacts/cameras/fakecam_fullhd_480x270.png rename to tests/artifacts/cameras/image_480x270.png diff --git a/tests/cameras/test_opencv.py b/tests/cameras/test_opencv.py index 44cb9bb8..dda2a4fb 100644 --- a/tests/cameras/test_opencv.py +++ b/tests/cameras/test_opencv.py @@ -19,7 +19,7 @@ # pytest tests/cameras/test_opencv.py::test_connect # ``` -import os +from pathlib import Path import numpy as np import pytest @@ -29,17 +29,18 @@ from lerobot.common.cameras.opencv import OpenCVCamera, OpenCVCameraConfig from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError # NOTE(Steven): more tests + assertions? -TEST_ARTIFACTS_DIR = os.path.join(os.path.dirname(os.path.dirname(__file__)), "artifacts", "cameras") -DEFAULT_PNG_FILE_PATH = os.path.join(TEST_ARTIFACTS_DIR, "fakecam_sd_160x120.png") +TEST_ARTIFACTS_DIR = Path(__file__).parent.parent / "artifacts" / "cameras" +DEFAULT_PNG_FILE_PATH = TEST_ARTIFACTS_DIR / "image_160x120.png" TEST_IMAGE_PATHS = [ - os.path.join(TEST_ARTIFACTS_DIR, "fakecam_sd_160x120.png"), - os.path.join(TEST_ARTIFACTS_DIR, "fakecam_hd_320x180.png"), - os.path.join(TEST_ARTIFACTS_DIR, "fakecam_fullhd_480x270.png"), - os.path.join(TEST_ARTIFACTS_DIR, "fakecam_square_128x128.png"), + TEST_ARTIFACTS_DIR / "image_160x120.png", + TEST_ARTIFACTS_DIR / "image_320x180.png", + TEST_ARTIFACTS_DIR / "image_480x270.png", + TEST_ARTIFACTS_DIR / "image_128x128.png", ] -def test_base_class_implementation(): +def test_abc_implementation(): + """Instantiation should raise an error if the class doesn't implement abstract methods/properties.""" config = OpenCVCameraConfig(index_or_path=0) _ = OpenCVCamera(config) @@ -49,7 +50,7 @@ def test_connect(): config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH) camera = OpenCVCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) assert camera.is_connected @@ -57,10 +58,10 @@ def test_connect(): def test_connect_already_connected(): config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH) camera = OpenCVCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) with pytest.raises(DeviceAlreadyConnectedError): - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) def test_connect_invalid_camera_path(): @@ -68,7 +69,7 @@ def test_connect_invalid_camera_path(): camera = OpenCVCamera(config) with pytest.raises(ConnectionError): - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) def test_invalid_width_connect(): @@ -80,14 +81,14 @@ def test_invalid_width_connect(): camera = OpenCVCamera(config) with pytest.raises(RuntimeError): - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) -@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS) +@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS, ids=[str(path.name) for path in TEST_IMAGE_PATHS]) def test_read(index_or_path): config = OpenCVCameraConfig(index_or_path=index_or_path) camera = OpenCVCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) img = camera.read() @@ -105,7 +106,7 @@ def test_read_before_connect(): def test_disconnect(): config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH) camera = OpenCVCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) camera.disconnect() @@ -120,11 +121,11 @@ def test_disconnect_before_connect(): _ = camera.disconnect() -@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS) +@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS, ids=[str(path.name) for path in TEST_IMAGE_PATHS]) def test_async_read(index_or_path): config = OpenCVCameraConfig(index_or_path=index_or_path) camera = OpenCVCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) try: img = camera.async_read() @@ -140,11 +141,13 @@ def test_async_read(index_or_path): def test_async_read_timeout(): config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH) camera = OpenCVCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) try: with pytest.raises(TimeoutError): - camera.async_read(timeout_ms=0) + camera.async_read( + timeout_ms=0 + ) # NOTE(Steven): This is flaky as sdometimes we actually get a frame finally: if camera.is_connected: camera.disconnect() @@ -158,7 +161,7 @@ def test_async_read_before_connect(): _ = camera.async_read() -@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS) +@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS, ids=[str(path.name) for path in TEST_IMAGE_PATHS]) @pytest.mark.parametrize( "rotation", [ @@ -167,15 +170,16 @@ def test_async_read_before_connect(): Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270, ], + ids=["no_rot", "rot90", "rot180", "rot270"], ) -def test_all_rotations(rotation, index_or_path): - filename = os.path.basename(index_or_path) +def test_rotation(rotation, index_or_path): + filename = Path(index_or_path).name dimensions = filename.split("_")[-1].split(".")[0] # Assumes filenames format (_wxh.png) original_width, original_height = map(int, dimensions.split("x")) config = OpenCVCameraConfig(index_or_path=index_or_path, rotation=rotation) camera = OpenCVCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) img = camera.read() assert isinstance(img, np.ndarray) diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index fb5e5009..8e582cd5 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -19,7 +19,7 @@ # pytest tests/cameras/test_opencv.py::test_connect # ``` -import os +from pathlib import Path from unittest.mock import patch import numpy as np @@ -29,95 +29,103 @@ from lerobot.common.cameras.configs import Cv2Rotation from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError try: - from lerobot.common.cameras.intel import RealSenseCamera, RealSenseCameraConfig -except (ImportError, ModuleNotFoundError): + from lerobot.common.cameras.realsense import RealSenseCamera, RealSenseCameraConfig +except (ImportError, ModuleNotFoundError, NameError): pytest.skip("pyrealsense2 not available", allow_module_level=True) -TEST_ARTIFACTS_DIR = os.path.join(os.path.dirname(os.path.dirname(__file__)), "artifacts", "cameras") -BAG_FILE_PATH = os.path.join(TEST_ARTIFACTS_DIR, "test_rs.bag") +TEST_ARTIFACTS_DIR = Path(__file__).parent.parent / "artifacts" / "cameras" +BAG_FILE_PATH = TEST_ARTIFACTS_DIR / "test_rs.bag" -# NOTE(Steven): Missing tests for depth -# NOTE(Steven): Takes 20sec, the patch being the biggest bottleneck -# NOTE(Steven): more tests + assertions? -if not os.path.exists(BAG_FILE_PATH): - print(f"Warning: Bag file not found at {BAG_FILE_PATH}. Some tests might fail or be skipped.") +# NOTE(Steven): For some reason these tests take ~20sec in macOS but only ~2sec in Linux. -def mock_rs_config_enable_device_from_file(rs_config_instance, sn): - if not os.path.exists(BAG_FILE_PATH): - raise FileNotFoundError(f"Test bag file not found: {BAG_FILE_PATH}") - return rs_config_instance.enable_device_from_file(BAG_FILE_PATH, repeat_playback=True) +def mock_rs_config_enable_device_from_file(rs_config_instance, _sn): + return rs_config_instance.enable_device_from_file(str(BAG_FILE_PATH), repeat_playback=True) -def mock_rs_config_enable_device_bad_file(rs_config_instance, sn): +def mock_rs_config_enable_device_bad_file(rs_config_instance, _sn): return rs_config_instance.enable_device_from_file("non_existent_file.bag", repeat_playback=True) -def test_base_class_implementation(): - config = RealSenseCameraConfig(serial_number=42) +@pytest.fixture(name="patch_realsense", autouse=True) +def fixture_patch_realsense(): + """Automatically mock pyrealsense2.config.enable_device for all tests.""" + with patch( + "pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file + ) as mock: + yield mock + + +def test_abc_implementation(): + """Instantiation should raise an error if the class doesn't implement abstract methods/properties.""" + config = RealSenseCameraConfig(serial_number_or_name=42) _ = RealSenseCamera(config) -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_connect(mock_enable_device): - config = RealSenseCameraConfig(serial_number=42) +def test_connect(): + config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) assert camera.is_connected -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_connect_already_connected(mock_enable_device): - config = RealSenseCameraConfig(serial_number=42) +def test_connect_already_connected(): + config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) with pytest.raises(DeviceAlreadyConnectedError): - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_bad_file) -def test_connect_invalid_camera_path(mock_enable_device): - config = RealSenseCameraConfig(serial_number=42) +def test_connect_invalid_camera_path(patch_realsense): + patch_realsense.side_effect = mock_rs_config_enable_device_bad_file + config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) with pytest.raises(ConnectionError): - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_invalid_width_connect(mock_enable_device): - config = RealSenseCameraConfig(serial_number=42, width=99999, height=480, fps=30) +def test_invalid_width_connect(): + config = RealSenseCameraConfig(serial_number_or_name=42, width=99999, height=480, fps=30) camera = RealSenseCamera(config) with pytest.raises(ConnectionError): - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_read(mock_enable_device): - config = RealSenseCameraConfig(serial_number=42, width=640, height=480, fps=30) +def test_read(): + config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30) camera = RealSenseCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) img = camera.read() assert isinstance(img, np.ndarray) +def test_read_depth(): + config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30, use_depth=True) + camera = RealSenseCamera(config) + camera.connect(warmup=False) + + img = camera.read_depth(timeout_ms=1000) # NOTE(Steven): Reading depth takes longer + assert isinstance(img, np.ndarray) + + def test_read_before_connect(): - config = RealSenseCameraConfig(serial_number=42) + config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) with pytest.raises(DeviceNotConnectedError): _ = camera.read() -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_disconnect(mock_enable_device): - config = RealSenseCameraConfig(serial_number=42) +def test_disconnect(): + config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) camera.disconnect() @@ -125,18 +133,17 @@ def test_disconnect(mock_enable_device): def test_disconnect_before_connect(): - config = RealSenseCameraConfig(serial_number=42) + config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) with pytest.raises(DeviceNotConnectedError): camera.disconnect() -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_async_read(mock_enable_device): - config = RealSenseCameraConfig(serial_number=42, width=640, height=480, fps=30) +def test_async_read(): + config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30) camera = RealSenseCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) try: img = camera.async_read() @@ -149,22 +156,23 @@ def test_async_read(mock_enable_device): camera.disconnect() # To stop/join the thread. Otherwise get warnings when the test ends -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_async_read_timeout(mock_enable_device): - config = RealSenseCameraConfig(serial_number=42, width=640, height=480, fps=30) +def test_async_read_timeout(): + config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30) camera = RealSenseCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) try: with pytest.raises(TimeoutError): - camera.async_read(timeout_ms=0) + camera.async_read( + timeout_ms=0 + ) # NOTE(Steven): This is flaky as sdometimes we actually get a frame finally: if camera.is_connected: camera.disconnect() def test_async_read_before_connect(): - config = RealSenseCameraConfig(serial_number=42) + config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) with pytest.raises(DeviceNotConnectedError): @@ -179,12 +187,12 @@ def test_async_read_before_connect(): Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270, ], + ids=["no_rot", "rot90", "rot180", "rot270"], ) -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_all_rotations(mock_enable_device, rotation): - config = RealSenseCameraConfig(serial_number=42, rotation=rotation) +def test_rotation(rotation): + config = RealSenseCameraConfig(serial_number_or_name=42, rotation=rotation) camera = RealSenseCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) img = camera.read() assert isinstance(img, np.ndarray) diff --git a/tests/mocks/mock_motors_bus.py b/tests/mocks/mock_motors_bus.py new file mode 100644 index 00000000..2a35dfb3 --- /dev/null +++ b/tests/mocks/mock_motors_bus.py @@ -0,0 +1,137 @@ +# ruff: noqa: N802 + +from lerobot.common.motors.motors_bus import ( + Motor, + MotorsBus, +) + +DUMMY_CTRL_TABLE_1 = { + "Firmware_Version": (0, 1), + "Model_Number": (1, 2), + "Present_Position": (3, 4), + "Goal_Position": (11, 2), +} + +DUMMY_CTRL_TABLE_2 = { + "Model_Number": (0, 2), + "Firmware_Version": (2, 1), + "Present_Position": (3, 4), + "Present_Velocity": (7, 4), + "Goal_Position": (11, 4), + "Goal_Velocity": (15, 4), + "Lock": (19, 1), +} + +DUMMY_MODEL_CTRL_TABLE = { + "model_1": DUMMY_CTRL_TABLE_1, + "model_2": DUMMY_CTRL_TABLE_2, + "model_3": DUMMY_CTRL_TABLE_2, +} + +DUMMY_BAUDRATE_TABLE = { + 0: 1_000_000, + 1: 500_000, + 2: 250_000, +} + +DUMMY_MODEL_BAUDRATE_TABLE = { + "model_1": DUMMY_BAUDRATE_TABLE, + "model_2": DUMMY_BAUDRATE_TABLE, + "model_3": DUMMY_BAUDRATE_TABLE, +} + +DUMMY_ENCODING_TABLE = { + "Present_Position": 8, + "Goal_Position": 10, +} + +DUMMY_MODEL_ENCODING_TABLE = { + "model_1": DUMMY_ENCODING_TABLE, + "model_2": DUMMY_ENCODING_TABLE, + "model_3": DUMMY_ENCODING_TABLE, +} + +DUMMY_MODEL_NUMBER_TABLE = { + "model_1": 1234, + "model_2": 5678, + "model_3": 5799, +} + +DUMMY_MODEL_RESOLUTION_TABLE = { + "model_1": 4096, + "model_2": 1024, + "model_3": 4096, +} + + +class MockPortHandler: + def __init__(self, port_name): + self.is_open: bool = False + self.baudrate: int + self.packet_start_time: float + self.packet_timeout: float + self.tx_time_per_byte: float + self.is_using: bool = False + self.port_name: str = port_name + self.ser = None + + def openPort(self): + self.is_open = True + return self.is_open + + def closePort(self): + self.is_open = False + + def clearPort(self): ... + def setPortName(self, port_name): + self.port_name = port_name + + def getPortName(self): + return self.port_name + + def setBaudRate(self, baudrate): + self.baudrate: baudrate + + def getBaudRate(self): + return self.baudrate + + def getBytesAvailable(self): ... + def readPort(self, length): ... + def writePort(self, packet): ... + def setPacketTimeout(self, packet_length): ... + def setPacketTimeoutMillis(self, msec): ... + def isPacketTimeout(self): ... + def getCurrentTime(self): ... + def getTimeSinceStart(self): ... + def setupPort(self, cflag_baud): ... + def getCFlagBaud(self, baudrate): ... + + +class MockMotorsBus(MotorsBus): + available_baudrates = [500_000, 1_000_000] + default_timeout = 1000 + model_baudrate_table = DUMMY_MODEL_BAUDRATE_TABLE + model_ctrl_table = DUMMY_MODEL_CTRL_TABLE + model_encoding_table = DUMMY_MODEL_ENCODING_TABLE + model_number_table = DUMMY_MODEL_NUMBER_TABLE + model_resolution_table = DUMMY_MODEL_RESOLUTION_TABLE + normalized_data = ["Present_Position", "Goal_Position"] + + def __init__(self, port: str, motors: dict[str, Motor]): + super().__init__(port, motors) + self.port_handler = MockPortHandler(port) + + def _assert_protocol_is_compatible(self, instruction_name): ... + def _handshake(self): ... + def _find_single_motor(self, motor, initial_baudrate): ... + def configure_motors(self): ... + def read_calibration(self): ... + def write_calibration(self, calibration_dict): ... + def disable_torque(self, motors, num_retry): ... + def _disable_torque(self, motor, model, num_retry): ... + def enable_torque(self, motors, num_retry): ... + def _get_half_turn_homings(self, positions): ... + def _encode_sign(self, data_name, ids_values): ... + def _decode_sign(self, data_name, ids_values): ... + def _split_into_byte_chunks(self, value, length): ... + def broadcast_ping(self, num_retry, raise_on_error): ... diff --git a/tests/motors/test_motors_bus.py b/tests/motors/test_motors_bus.py index 9056e870..fab2d6a1 100644 --- a/tests/motors/test_motors_bus.py +++ b/tests/motors/test_motors_bus.py @@ -1,5 +1,3 @@ -# ruff: noqa: N802 - import re from unittest.mock import patch @@ -8,142 +6,16 @@ import pytest from lerobot.common.motors.motors_bus import ( Motor, MotorNormMode, - MotorsBus, assert_same_address, get_address, get_ctrl_table, ) - -DUMMY_CTRL_TABLE_1 = { - "Firmware_Version": (0, 1), - "Model_Number": (1, 2), - "Present_Position": (3, 4), - "Goal_Position": (11, 2), -} - -DUMMY_CTRL_TABLE_2 = { - "Model_Number": (0, 2), - "Firmware_Version": (2, 1), - "Present_Position": (3, 4), - "Present_Velocity": (7, 4), - "Goal_Position": (11, 4), - "Goal_Velocity": (15, 4), - "Lock": (19, 1), -} - -DUMMY_MODEL_CTRL_TABLE = { - "model_1": DUMMY_CTRL_TABLE_1, - "model_2": DUMMY_CTRL_TABLE_2, - "model_3": DUMMY_CTRL_TABLE_2, -} - -DUMMY_BAUDRATE_TABLE = { - 0: 1_000_000, - 1: 500_000, - 2: 250_000, -} - -DUMMY_MODEL_BAUDRATE_TABLE = { - "model_1": DUMMY_BAUDRATE_TABLE, - "model_2": DUMMY_BAUDRATE_TABLE, - "model_3": DUMMY_BAUDRATE_TABLE, -} - -DUMMY_ENCODING_TABLE = { - "Present_Position": 8, - "Goal_Position": 10, -} - -DUMMY_MODEL_ENCODING_TABLE = { - "model_1": DUMMY_ENCODING_TABLE, - "model_2": DUMMY_ENCODING_TABLE, - "model_3": DUMMY_ENCODING_TABLE, -} - -DUMMY_MODEL_NUMBER_TABLE = { - "model_1": 1234, - "model_2": 5678, - "model_3": 5799, -} - -DUMMY_MODEL_RESOLUTION_TABLE = { - "model_1": 4096, - "model_2": 1024, - "model_3": 4096, -} - - -class MockPortHandler: - def __init__(self, port_name): - self.is_open: bool = False - self.baudrate: int - self.packet_start_time: float - self.packet_timeout: float - self.tx_time_per_byte: float - self.is_using: bool = False - self.port_name: str = port_name - self.ser = None - - def openPort(self): - self.is_open = True - return self.is_open - - def closePort(self): - self.is_open = False - - def clearPort(self): ... - def setPortName(self, port_name): - self.port_name = port_name - - def getPortName(self): - return self.port_name - - def setBaudRate(self, baudrate): - self.baudrate: baudrate - - def getBaudRate(self): - return self.baudrate - - def getBytesAvailable(self): ... - def readPort(self, length): ... - def writePort(self, packet): ... - def setPacketTimeout(self, packet_length): ... - def setPacketTimeoutMillis(self, msec): ... - def isPacketTimeout(self): ... - def getCurrentTime(self): ... - def getTimeSinceStart(self): ... - def setupPort(self, cflag_baud): ... - def getCFlagBaud(self, baudrate): ... - - -class MockMotorsBus(MotorsBus): - available_baudrates = [500_000, 1_000_000] - default_timeout = 1000 - model_baudrate_table = DUMMY_MODEL_BAUDRATE_TABLE - model_ctrl_table = DUMMY_MODEL_CTRL_TABLE - model_encoding_table = DUMMY_MODEL_ENCODING_TABLE - model_number_table = DUMMY_MODEL_NUMBER_TABLE - model_resolution_table = DUMMY_MODEL_RESOLUTION_TABLE - normalized_data = ["Present_Position", "Goal_Position"] - - def __init__(self, port: str, motors: dict[str, Motor]): - super().__init__(port, motors) - self.port_handler = MockPortHandler(port) - - def _assert_protocol_is_compatible(self, instruction_name): ... - def _handshake(self): ... - def _find_single_motor(self, motor, initial_baudrate): ... - def configure_motors(self): ... - def read_calibration(self): ... - def write_calibration(self, calibration_dict): ... - def disable_torque(self, motors, num_retry): ... - def _disable_torque(self, motor, model, num_retry): ... - def enable_torque(self, motors, num_retry): ... - def _get_half_turn_homings(self, positions): ... - def _encode_sign(self, data_name, ids_values): ... - def _decode_sign(self, data_name, ids_values): ... - def _split_into_byte_chunks(self, value, length): ... - def broadcast_ping(self, num_retry, raise_on_error): ... +from tests.mocks.mock_motors_bus import ( + DUMMY_CTRL_TABLE_1, + DUMMY_CTRL_TABLE_2, + DUMMY_MODEL_CTRL_TABLE, + MockMotorsBus, +) @pytest.fixture diff --git a/tests/robots/test_so100_follower.py b/tests/robots/test_so100_follower.py new file mode 100644 index 00000000..81d9d6a9 --- /dev/null +++ b/tests/robots/test_so100_follower.py @@ -0,0 +1,95 @@ +from contextlib import contextmanager +from unittest.mock import MagicMock, patch + +import pytest + +from lerobot.common.robots.so100_follower import ( + SO100Follower, + SO100FollowerConfig, +) + + +def _make_bus_mock() -> MagicMock: + """Return a bus mock with just the attributes used by the robot.""" + bus = MagicMock(name="FeetechBusMock") + bus.is_connected = False + + def _connect(): + bus.is_connected = True + + def _disconnect(_disable=True): + bus.is_connected = False + + bus.connect.side_effect = _connect + bus.disconnect.side_effect = _disconnect + + @contextmanager + def _dummy_cm(): + yield + + bus.torque_disabled.side_effect = _dummy_cm + + return bus + + +@pytest.fixture +def follower(): + bus_mock = _make_bus_mock() + + def _bus_side_effect(*_args, **kwargs): + bus_mock.motors = kwargs["motors"] + motors_order: list[str] = list(bus_mock.motors) + + bus_mock.sync_read.return_value = {motor: idx for idx, motor in enumerate(motors_order, 1)} + bus_mock.sync_write.return_value = None + bus_mock.write.return_value = None + bus_mock.disable_torque.return_value = None + bus_mock.enable_torque.return_value = None + bus_mock.is_calibrated = True + return bus_mock + + with ( + patch( + "lerobot.common.robots.so100_follower.so100_follower.FeetechMotorsBus", + side_effect=_bus_side_effect, + ), + patch.object(SO100Follower, "configure", lambda self: None), + ): + cfg = SO100FollowerConfig(port="/dev/null") + robot = SO100Follower(cfg) + yield robot + if robot.is_connected: + robot.disconnect() + + +def test_connect_disconnect(follower): + assert not follower.is_connected + + follower.connect() + assert follower.is_connected + + follower.disconnect() + assert not follower.is_connected + + +def test_get_observation(follower): + follower.connect() + obs = follower.get_observation() + + expected_keys = {f"{m}.pos" for m in follower.bus.motors} + assert set(obs.keys()) == expected_keys + + for idx, motor in enumerate(follower.bus.motors, 1): + assert obs[f"{motor}.pos"] == idx + + +def test_send_action(follower): + follower.connect() + + action = {f"{m}.pos": i * 10 for i, m in enumerate(follower.bus.motors, 1)} + returned = follower.send_action(action) + + assert returned == action + + goal_pos = {m: (i + 1) * 10 for i, m in enumerate(follower.bus.motors)} + follower.bus.sync_write.assert_called_once_with("Goal_Position", goal_pos) diff --git a/tests/utils.py b/tests/utils.py index 918dc03b..bf3de0ed 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -223,7 +223,7 @@ def make_camera(camera_type: str, **kwargs) -> Camera: elif camera_type == "intelrealsense": serial_number = kwargs.pop("serial_number", INTELREALSENSE_SERIAL_NUMBER) kwargs["serial_number"] = serial_number - from lerobot.common.cameras.intel import RealSenseCamera, RealSenseCameraConfig + from lerobot.common.cameras.realsense import RealSenseCamera, RealSenseCameraConfig config = RealSenseCameraConfig(**kwargs) return RealSenseCamera(config)