diff --git a/lerobot/__init__.py b/lerobot/__init__.py index aeae31008..2fedb1222 100644 --- a/lerobot/__init__.py +++ b/lerobot/__init__.py @@ -28,6 +28,8 @@ Example: print(lerobot.available_policies) print(lerobot.available_policies_per_env) print(lerobot.available_robots) + print(lerobot.available_cameras) + print(lerobot.available_motors) ``` When implementing a new dataset loadable with LeRobotDataset follow these steps: @@ -198,6 +200,17 @@ available_robots = [ "aloha", ] +# lists all available robots from `lerobot/common/robot_devices/cameras` +available_cameras = [ + "opencv", + "intelrealsense", +] + +# lists all available robots from `lerobot/common/robot_devices/motors` +available_motors = [ + "dynamixel", +] + # keys and values refer to yaml files available_policies_per_env = { "aloha": ["act"], diff --git a/lerobot/common/robot_devices/cameras/intelrealsense.py b/lerobot/common/robot_devices/cameras/intelrealsense.py index 21ac0b458..77a015cf1 100644 --- a/lerobot/common/robot_devices/cameras/intelrealsense.py +++ b/lerobot/common/robot_devices/cameras/intelrealsense.py @@ -5,6 +5,7 @@ This file contains utilities for recording frames from Intel Realsense cameras. import argparse import concurrent.futures import logging +import math import shutil import threading import time @@ -156,7 +157,9 @@ class IntelRealSenseCameraConfig: f"`color_mode` is expected to be 'rgb' or 'bgr', but {self.color_mode} is provided." ) - if (self.fps or self.width or self.height) and not (self.fps and self.width and self.height): + at_least_one_is_not_none = self.fps is not None or self.width is not None or self.height is not None + at_least_one_is_none = self.fps is None or self.width is None or self.height is None + if at_least_one_is_not_none and at_least_one_is_none: raise ValueError( "For `fps`, `width` and `height`, either all of them need to be set, or none of them, " f"but {self.fps=}, {self.width=}, {self.height=} were provided." @@ -260,7 +263,7 @@ class IntelRealSenseCamera: self.camera = rs.pipeline() try: - self.camera.start(config) + profile = self.camera.start(config) is_camera_open = True except RuntimeError: is_camera_open = False @@ -279,6 +282,29 @@ class IntelRealSenseCamera: raise OSError(f"Can't access IntelRealSenseCamera({self.camera_index}).") + color_stream = profile.get_stream(rs.stream.color) + color_profile = color_stream.as_video_stream_profile() + actual_fps = color_profile.fps() + actual_width = color_profile.width() + actual_height = color_profile.height() + + if self.fps is not None and not math.isclose(self.fps, actual_fps, rel_tol=1e-3): + raise OSError( + f"Can't set {self.fps=} for IntelRealSenseCamera({self.camera_index}). Actual value is {actual_fps}." + ) + if self.width is not None and self.width != actual_width: + raise OSError( + f"Can't set {self.width=} for IntelRealSenseCamera({self.camera_index}). Actual value is {actual_width}." + ) + if self.height is not None and self.height != actual_height: + raise OSError( + f"Can't set {self.height=} for IntelRealSenseCamera({self.camera_index}). Actual value is {actual_height}." + ) + + self.fps = actual_fps + self.width = actual_width + self.height = actual_height + self.is_connected = True def read(self, temporary_color: str | None = None) -> np.ndarray | tuple[np.ndarray, np.ndarray]: diff --git a/tests/conftest.py b/tests/conftest.py index 52006f331..8c754cb3f 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -18,8 +18,9 @@ import traceback import pytest from lerobot.common.utils.utils import init_hydra_config - -from .utils import DEVICE, ROBOT_CONFIG_PATH_TEMPLATE +from tests.test_cameras import make_camera +from tests.test_motors import make_motors_bus +from tests.utils import DEVICE, ROBOT_CONFIG_PATH_TEMPLATE def pytest_collection_finish(): @@ -41,3 +42,29 @@ def is_robot_available(robot_type): traceback.print_exc() print(f"\nA {robot_type} robot is not available.") return False + + +@pytest.fixture +def is_camera_available(camera_type): + try: + camera = make_camera(camera_type) + camera.connect() + del camera + return True + except Exception: + traceback.print_exc() + print(f"\nA {camera_type} camera is not available.") + return False + + +@pytest.fixture +def is_motor_available(motor_type): + try: + motors_bus = make_motors_bus(motor_type) + motors_bus.connect() + del motors_bus + return True + except Exception: + traceback.print_exc() + print(f"\nA {motor_type} motor is not available.") + return False diff --git a/tests/mock_dynamixel.py b/tests/mock_dynamixel.py new file mode 100644 index 000000000..16831a726 --- /dev/null +++ b/tests/mock_dynamixel.py @@ -0,0 +1,48 @@ +from dynamixel_sdk import COMM_SUCCESS + + +class PortHandler: + def __init__(self, port): + self.port = port + + def openPort(self): # noqa: N802 + return True + + def closePort(self): # noqa: N802 + pass + + def setPacketTimeoutMillis(self, timeout_ms): # noqa: N802 + del timeout_ms # unused + + +class PacketHandler: + def __init__(self, protocol_version): + del protocol_version # unused + + +class GroupSyncRead: + def __init__(self, port_handler, packet_handler, address, bytes): + pass + + def addParam(self, motor_index): # noqa: N802 + pass + + def txRxPacket(self): # noqa: N802 + return COMM_SUCCESS + + def getData(self, index, address, bytes): # noqa: N802 + return value # noqa: F821 + + +class GroupSyncWrite: + def __init__(self, port_handler, packet_handler, address, bytes): + pass + + def addParam(self, index, data): # noqa: N802 + pass + + def txPacket(self): # noqa: N802 + return COMM_SUCCESS + + def changeParam(self, index, data): # noqa: N802 + pass diff --git a/tests/mock_intelrealsense.py b/tests/mock_intelrealsense.py new file mode 100644 index 000000000..1e6c3f5f5 --- /dev/null +++ b/tests/mock_intelrealsense.py @@ -0,0 +1,110 @@ +import enum + +import numpy as np + + +class MockStream(enum.Enum): + color = 0 + depth = 1 + + +class MockFormat(enum.Enum): + rgb8 = 0 + z16 = 1 + + +class MockConfig: + def enable_device(self, device_id: str): + self.device_enabled = device_id + + def enable_stream( + self, stream_type: MockStream, width=None, height=None, color_format: MockFormat = None, fps=None + ): + self.stream_type = stream_type + # Overwrite default values when possible + self.width = 848 if width is None else width + self.height = 480 if height is None else height + self.color_format = MockFormat.rgb8 if color_format is None else color_format + self.fps = 30 if fps is None else fps + + +class MockColorProfile: + def __init__(self, config: MockConfig): + self.config = config + + def fps(self): + return self.config.fps + + def width(self): + return self.config.width + + def height(self): + return self.config.height + + +class MockColorStream: + def __init__(self, config: MockConfig): + self.config = config + + def as_video_stream_profile(self): + return MockColorProfile(self.config) + + +class MockProfile: + def __init__(self, config: MockConfig): + self.config = config + + def get_stream(self, color_format: MockFormat): + del color_format # unused + return MockColorStream(self.config) + + +class MockPipeline: + def __init__(self): + self.started = False + self.config = None + + def start(self, config: MockConfig): + self.started = True + self.config = config + return MockProfile(self.config) + + def stop(self): + if not self.started: + raise RuntimeError("You need to start the camera before stop.") + self.started = False + self.config = None + + def wait_for_frames(self, timeout_ms=50000): + del timeout_ms # unused + return MockFrames(self.config) + + +class MockFrames: + def __init__(self, config: MockConfig): + self.config = config + + def get_color_frame(self): + return MockColorFrame(self.config) + + def get_depth_frame(self): + return MockDepthFrame(self.config) + + +class MockColorFrame: + def __init__(self, config: MockConfig): + self.config = config + + def get_data(self): + data = np.ones((self.config.height, self.config.width, 3), dtype=np.uint8) + # Create a difference between rgb and bgr + data[:, :, 0] = 2 + return data + + +class MockDepthFrame: + def __init__(self, config: MockConfig): + self.config = config + + def get_data(self): + return np.ones((self.config.height, self.config.width), dtype=np.uint16) diff --git a/tests/test_cameras.py b/tests/test_cameras.py index 7c9d9e38a..161d9a190 100644 --- a/tests/test_cameras.py +++ b/tests/test_cameras.py @@ -1,25 +1,34 @@ """ -Tests meant to be used locally and launched manually. +Tests for physical cameras and their mocked versions. +If the physical camera is not connected to the computer, or not working, +the test will be skipped. -Example usage: +Example of running a specific test: ```bash pytest -sx tests/test_cameras.py::test_camera ``` -Example of running test on mocked_koch: +Example of running test on a real OpenCV camera connected to the computer: ```bash -python -m pytest -sx -k "mocked_koch" tests/test_cameras.py::test_camera +pytest -sx tests/test_cameras.py::test_camera[opencv] +``` + +Example of running test on a mocked version of an OpenCV camera: +```bash +pytest -sx -k "mocked_opencv" tests/test_cameras.py::test_camera ``` """ import numpy as np import pytest -from lerobot.common.robot_devices.cameras.opencv import OpenCVCamera, save_images_from_cameras from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError -from tests.utils import TEST_ROBOT_TYPES, require_robot +from tests.utils import TEST_CAMERA_TYPES, require_camera + +# Camera indices used for connecting physical cameras +OPENCV_CAMERA_INDEX = 0 +INTELREALSENSE_CAMERA_INDEX = 128422271614 -CAMERA_INDEX = 2 # Maximum absolute difference between two consecutive images recored by a camera. # This value differs with respect to the camera. MAX_PIXEL_DIFFERENCE = 25 @@ -29,9 +38,26 @@ def compute_max_pixel_difference(first_image, second_image): return np.abs(first_image.astype(float) - second_image.astype(float)).max() -@pytest.mark.parametrize("robot_type", TEST_ROBOT_TYPES) -@require_robot -def test_camera(request, robot_type): +def make_camera(camera_type, **kwargs): + if camera_type == "opencv": + from lerobot.common.robot_devices.cameras.opencv import OpenCVCamera + + camera_index = kwargs.pop("camera_index", OPENCV_CAMERA_INDEX) + return OpenCVCamera(camera_index, **kwargs) + + elif camera_type == "intelrealsense": + from lerobot.common.robot_devices.cameras.intelrealsense import IntelRealSenseCamera + + camera_index = kwargs.pop("camera_index", INTELREALSENSE_CAMERA_INDEX) + return IntelRealSenseCamera(camera_index, **kwargs) + + else: + raise ValueError(f"The camera type '{camera_type}' is not valid.") + + +@pytest.mark.parametrize("camera_type", TEST_CAMERA_TYPES) +@require_camera +def test_camera(request, camera_type): """Test assumes that `camera.read()` returns the same image when called multiple times in a row. So the environment should not change (you shouldnt be in front of the camera) and the camera should not be moving. @@ -40,10 +66,9 @@ def test_camera(request, robot_type): """ # TODO(rcadene): measure fps in nightly? # TODO(rcadene): test logs - # TODO(rcadene): add compatibility with other camera APIs # Test instantiating - camera = OpenCVCamera(CAMERA_INDEX) + camera = make_camera(camera_type) # Test reading, async reading, disconnecting before connecting raises an error with pytest.raises(RobotDeviceNotConnectedError): @@ -57,7 +82,7 @@ def test_camera(request, robot_type): del camera # Test connecting - camera = OpenCVCamera(CAMERA_INDEX) + camera = make_camera(camera_type) camera.connect() assert camera.is_connected assert camera.fps is not None @@ -94,12 +119,12 @@ def test_camera(request, robot_type): assert camera.thread is None # Test disconnecting with `__del__` - camera = OpenCVCamera(CAMERA_INDEX) + camera = make_camera(camera_type) camera.connect() del camera # Test acquiring a bgr image - camera = OpenCVCamera(CAMERA_INDEX, color_mode="bgr") + camera = make_camera(camera_type, color_mode="bgr") camera.connect() assert camera.color_mode == "bgr" bgr_color_image = camera.read() @@ -110,7 +135,7 @@ def test_camera(request, robot_type): # TODO(rcadene): Add a test for a camera that supports fps=60 # Test width and height can be set - camera = OpenCVCamera(CAMERA_INDEX, fps=30, width=1280, height=720) + camera = make_camera(camera_type, fps=30, width=1280, height=720) camera.connect() assert camera.fps == 30 assert camera.width == 1280 @@ -123,13 +148,19 @@ def test_camera(request, robot_type): del camera # Test not supported width and height raise an error - camera = OpenCVCamera(CAMERA_INDEX, fps=30, width=0, height=0) + camera = make_camera(camera_type, fps=30, width=0, height=0) with pytest.raises(OSError): camera.connect() del camera -@pytest.mark.parametrize("robot_type", TEST_ROBOT_TYPES) -@require_robot -def test_save_images_from_cameras(tmpdir, request, robot_type): +@pytest.mark.parametrize("camera_type", TEST_CAMERA_TYPES) +@require_camera +def test_save_images_from_cameras(tmpdir, request, camera_type): + # TODO(rcadene): refactor + if camera_type == "opencv": + from lerobot.common.robot_devices.cameras.opencv import save_images_from_cameras + elif camera_type == "intelrealsense": + from lerobot.common.robot_devices.cameras.intelrealsense import save_images_from_cameras + save_images_from_cameras(tmpdir, record_time_s=1) diff --git a/tests/test_motors.py b/tests/test_motors.py index 48c2e8d8d..63ee3f1db 100644 --- a/tests/test_motors.py +++ b/tests/test_motors.py @@ -1,11 +1,24 @@ """ -Tests meant to be used locally and launched manually. +Tests for physical motors and their mocked versions. +If the physical motors are not connected to the computer, or not working, +the test will be skipped. -Example usage: +Example of running a specific test: ```bash pytest -sx tests/test_motors.py::test_find_port pytest -sx tests/test_motors.py::test_motors_bus ``` + + +Example of running test on real dynamixel motors connected to the computer: +```bash +pytest -sx tests/test_motors.py::test_motors_bus[dynamixel] +``` + +Example of running test on a mocked version of dynamixel motors: +```bash +pytest -sx -k "mocked_dynamixel" tests/test_motors.py::test_motors_bus +``` """ # TODO(rcadene): measure fps in nightly? @@ -18,34 +31,46 @@ import time import numpy as np import pytest -from lerobot import available_robots +from lerobot import available_motors from lerobot.common.robot_devices.motors.utils import MotorsBus -from lerobot.common.robot_devices.robots.factory import make_robot from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError -from lerobot.common.utils.utils import init_hydra_config -from tests.utils import ROBOT_CONFIG_PATH_TEMPLATE, require_robot +from tests.utils import TEST_MOTOR_TYPES, require_motor + +DYNAMIXEL_PORT = "/dev/tty.usbmodem575E0032081" +DYNAMIXEL_MOTORS = { + "shoulder_pan": [1, "xl430-w250"], + "shoulder_lift": [2, "xl430-w250"], + "elbow_flex": [3, "xl330-m288"], + "wrist_flex": [4, "xl330-m288"], + "wrist_roll": [5, "xl330-m288"], + "gripper": [6, "xl330-m288"], +} -def make_motors_bus(robot_type: str) -> MotorsBus: - # Instantiate a robot and return one of its leader arms - config_path = ROBOT_CONFIG_PATH_TEMPLATE.format(robot=robot_type) - robot_cfg = init_hydra_config(config_path) - robot = make_robot(robot_cfg) - first_bus_name = list(robot.leader_arms.keys())[0] - motors_bus = robot.leader_arms[first_bus_name] - return motors_bus +def make_motors_bus(motor_type: str, **kwargs) -> MotorsBus: + if motor_type == "dynamixel": + from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus + + port = kwargs.pop("port", DYNAMIXEL_PORT) + motors = kwargs.pop("motors", DYNAMIXEL_MOTORS) + return DynamixelMotorsBus(port, motors, **kwargs) + + else: + raise ValueError(f"The motor type '{motor_type}' is not valid.") -@pytest.mark.parametrize("robot_type", available_robots) -@require_robot +# TODO(rcadene): implement mocked version of this test +@pytest.mark.parametrize("motor_type", available_motors) +@require_motor def test_find_port(request, robot_type): from lerobot.common.robot_devices.motors.dynamixel import find_port find_port() -@pytest.mark.parametrize("robot_type", available_robots) -@require_robot +# TODO(rcadene): implement mocked version of this test +@pytest.mark.parametrize("motor_type", available_motors) +@require_motor def test_configure_motors_all_ids_1(request, robot_type): input("Are you sure you want to re-configure the motors? Press enter to continue...") # This test expect the configuration was already correct. @@ -63,8 +88,8 @@ def test_configure_motors_all_ids_1(request, robot_type): del motors_bus -@pytest.mark.parametrize("robot_type", available_robots) -@require_robot +@pytest.mark.parametrize("motor_type", TEST_MOTOR_TYPES) +@require_motor def test_motors_bus(request, robot_type): motors_bus = make_motors_bus(robot_type) diff --git a/tests/utils.py b/tests/utils.py index 98b5a82a0..af8d7faf7 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -20,7 +20,7 @@ from functools import wraps import pytest import torch -from lerobot import available_robots +from lerobot import available_cameras, available_motors, available_robots from lerobot.common.utils.import_utils import is_package_available DEVICE = "cuda" if torch.cuda.is_available() else "cpu" @@ -31,6 +31,8 @@ DEFAULT_CONFIG_PATH = "lerobot/configs/default.yaml" ROBOT_CONFIG_PATH_TEMPLATE = "lerobot/configs/robot/{robot}.yaml" TEST_ROBOT_TYPES = available_robots + [f"mocked_{robot_type}" for robot_type in available_robots] +TEST_CAMERA_TYPES = available_cameras + [f"mocked_{camera_type}" for camera_type in available_cameras] +TEST_MOTOR_TYPES = available_motors + [f"mocked_{motor_type}" for motor_type in available_motors] def require_x86_64_kernel(func): @@ -178,32 +180,22 @@ def require_robot(func): request = kwargs.get("request") robot_type = kwargs.get("robot_type") + if robot_type is None: + raise ValueError("The 'robot_type' must be an argument of the test function.") + + if robot_type not in TEST_ROBOT_TYPES: + raise ValueError( + f"The camera type '{robot_type}' is not valid. Expected one of these '{TEST_ROBOT_TYPES}" + ) + if request is None: raise ValueError("The 'request' fixture must be an argument of the test function.") # Run test with a monkeypatched version of the robot devices. if robot_type.startswith("mocked_"): kwargs["robot_type"] = robot_type.replace("mocked_", "") - - monkeypatch = request.getfixturevalue("monkeypatch") - - try: - import cv2 - - from tests.mock_opencv import MockVideoCapture - - monkeypatch.setattr(cv2, "VideoCapture", MockVideoCapture) - except ImportError: - traceback.print_exc() - - try: - import pyrealsense2 as rs - - # TODO(rcadene): - mock_pipeline = None - monkeypatch.setattr(rs, "pipeline", mock_pipeline) - except ImportError: - traceback.print_exc() + mock_cameras(request) + mock_motors(request) # Run test with a real robot. Skip test if robot connection fails. else: @@ -214,3 +206,117 @@ def require_robot(func): return func(*args, **kwargs) return wrapper + + +def require_camera(func): + @wraps(func) + def wrapper(*args, **kwargs): + # Access the pytest request context to get the is_camera_available fixture + request = kwargs.get("request") + camera_type = kwargs.get("camera_type") + + if camera_type is None: + raise ValueError("The 'camera_type' must be an argument of the test function.") + + if camera_type not in TEST_CAMERA_TYPES: + raise ValueError( + f"The camera type '{camera_type}' is not valid. Expected one of these '{TEST_CAMERA_TYPES}" + ) + + if request is None: + raise ValueError("The 'request' fixture must be an argument of the test function.") + + # Run test with a monkeypatched version of the robot devices. + if camera_type.startswith("mocked_"): + kwargs["camera_type"] = camera_type.replace("mocked_", "") + mock_cameras(request) + + # Run test with a real robot. Skip test if robot connection fails. + else: + # `is_camera_available` is defined in `tests/conftest.py` + if not request.getfixturevalue("is_camera_available"): + pytest.skip(f"A {camera_type} camera is not available.") + + return func(*args, **kwargs) + + return wrapper + + +def require_motor(func): + @wraps(func) + def wrapper(*args, **kwargs): + # Access the pytest request context to get the is_motor_available fixture + request = kwargs.get("request") + motor_type = kwargs.get("motor_type") + + if motor_type is None: + raise ValueError("The 'motor_type' must be an argument of the test function.") + + if motor_type not in TEST_MOTOR_TYPES: + raise ValueError( + f"The motor type '{motor_type}' is not valid. Expected one of these '{TEST_MOTOR_TYPES}" + ) + + if request is None: + raise ValueError("The 'request' fixture must be an argument of the test function.") + + # Run test with a monkeypatched version of the robot devices. + if motor_type.startswith("mocked_"): + kwargs["motor_type"] = motor_type.replace("mocked_", "") + mock_motors(request) + + # Run test with a real robot. Skip test if robot connection fails. + else: + # `is_motor_available` is defined in `tests/conftest.py` + if not request.getfixturevalue("is_motor_available"): + pytest.skip(f"A {motor_type} motor is not available.") + + return func(*args, **kwargs) + + return wrapper + + +def mock_cameras(request): + monkeypatch = request.getfixturevalue("monkeypatch") + + try: + import cv2 + + from tests.mock_opencv import MockVideoCapture + + monkeypatch.setattr(cv2, "VideoCapture", MockVideoCapture) + except ImportError: + traceback.print_exc() + + try: + import pyrealsense2 as rs + + from tests.mock_intelrealsense import MockConfig, MockFormat, MockPipeline, MockStream + + monkeypatch.setattr(rs, "config", MockConfig) + monkeypatch.setattr(rs, "pipeline", MockPipeline) + monkeypatch.setattr(rs, "stream", MockStream) + monkeypatch.setattr(rs, "format", MockFormat) + except ImportError: + traceback.print_exc() + + +def mock_motors(request): + monkeypatch = request.getfixturevalue("monkeypatch") + + try: + import dynamixel_sdk + + from tests.mock_dynamixel import ( + MockGroupSyncRead, + MockGroupSyncWrite, + MockPacketHandler, + MockPortHandler, + ) + + monkeypatch.setattr(dynamixel_sdk, "GroupSyncRead", MockGroupSyncRead) + monkeypatch.setattr(dynamixel_sdk, "GroupSyncWrite", MockGroupSyncWrite) + monkeypatch.setattr(dynamixel_sdk, "PacketHandler", MockPacketHandler) + monkeypatch.setattr(dynamixel_sdk, "PortHandler", MockPortHandler) + except ImportError: + traceback.print_exc()