From 51efe6dfee35caf9050c6b4f1a47273ad48a7cf8 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Thu, 15 May 2025 11:46:41 +0200 Subject: [PATCH 01/43] Add setup_motors for lekiwi --- lerobot/common/robots/lekiwi/lekiwi.py | 7 +++++++ 1 file changed, 7 insertions(+) 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.") From 6797e1d92b77988b8e2036b6f9f08e126f1f5c14 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 15 May 2025 16:30:18 +0200 Subject: [PATCH 02/43] fix(cameras): correct validate_width_height logic --- lerobot/common/cameras/opencv/camera_opencv.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index 117b8500..e80b9eb2 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -234,21 +234,22 @@ class OpenCVCamera(Camera): 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))) + default_width = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_WIDTH))) + default_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 + self.width, self.height = default_height, default_width + self.prerotated_width, self.prerotated_height = default_width, default_height else: - self.width, self.height = actual_width, actual_height - self.prerotated_width, self.prerotated_height = actual_width, actual_height + self.width, self.height = default_width, default_height + self.prerotated_width, self.prerotated_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}.") return success = self.videocapture_camera.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.prerotated_width)) + actual_width = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_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})." @@ -259,6 +260,7 @@ class OpenCVCamera(Camera): 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)) + actual_height = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_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})." From 5daf552127d35f8c907113f2889ab70be371ab48 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 15 May 2025 16:48:52 +0200 Subject: [PATCH 03/43] fix(cameras): realsense depth post_process channel check --- .../common/cameras/intel/camera_realsense.py | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/lerobot/common/cameras/intel/camera_realsense.py b/lerobot/common/cameras/intel/camera_realsense.py index 1f39e299..4aadadaa 100644 --- a/lerobot/common/cameras/intel/camera_realsense.py +++ b/lerobot/common/cameras/intel/camera_realsense.py @@ -440,7 +440,7 @@ class RealSenseCamera(Camera): 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") @@ -493,7 +493,9 @@ class RealSenseCamera(Camera): 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,16 +518,19 @@ 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 c != self.channels: + logger.warning( + f"Captured frame channels ({c}) do not match configured channels ({self.channels}) for {self}." + ) if h != self.prerotated_height or w != self.prerotated_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}." - ) processed_image = image if self.color_mode == ColorMode.BGR: From c7ef189da03ddcdfdc4146b5b7e0ed4bd8dfb9a6 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Fri, 16 May 2025 10:48:45 +0200 Subject: [PATCH 04/43] Add check for same min and max during calibration --- lerobot/common/motors/motors_bus.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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]: From 10119c1a5955dc855c2f630caede7b8cdd5f0b2a Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Sun, 18 May 2025 11:51:47 +0200 Subject: [PATCH 05/43] Move MockMotorsBus --- tests/mocks/mock_motors_bus.py | 137 +++++++++++++++++++++++++++++++ tests/motors/test_motors_bus.py | 140 ++------------------------------ 2 files changed, 143 insertions(+), 134 deletions(-) create mode 100644 tests/mocks/mock_motors_bus.py 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 From edbba48e816a1dd3e1ac91e0257019403ccdc711 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Mon, 19 May 2025 10:58:35 +0200 Subject: [PATCH 06/43] Add so100_follower tests --- tests/robots/test_so100_follower.py | 90 +++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) create mode 100644 tests/robots/test_so100_follower.py diff --git a/tests/robots/test_so100_follower.py b/tests/robots/test_so100_follower.py new file mode 100644 index 00000000..75c8b993 --- /dev/null +++ b/tests/robots/test_so100_follower.py @@ -0,0 +1,90 @@ +from contextlib import contextmanager +from unittest.mock import MagicMock, patch + +import pytest + +from lerobot.common.robots.so100_follower import ( + SO100Follower, + SO100FollowerConfig, +) + +_MOTORS = SO100Follower(SO100FollowerConfig("")).bus.motors + + +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 + bus.motors = _MOTORS + bus.is_calibrated = True + bus.sync_read.return_value = {m: i for i, m in enumerate(_MOTORS, 1)} + bus.sync_write.return_value = None + bus.write.return_value = None + bus.disable_torque.return_value = None + bus.enable_torque.return_value = None + + @contextmanager + def _dummy_cm(): + yield + + bus.torque_disabled.side_effect = _dummy_cm + + return bus + + +@pytest.fixture +def follower(): + with ( + patch( + "lerobot.common.robots.so100_follower.so100_follower.FeetechMotorsBus", + return_value=_make_bus_mock(), + ), + 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 _MOTORS} + assert set(obs.keys()) == expected_keys + + for idx, motor in enumerate(_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(_MOTORS, 1)} + returned = follower.send_action(action) + + assert returned == action + + goal_pos = {m: (i + 1) * 10 for i, m in enumerate(_MOTORS)} + follower.bus.sync_write.assert_called_once_with("Goal_Position", goal_pos) From 05dfa26c541542818932536c860214de0775dee0 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Mon, 19 May 2025 11:24:10 +0200 Subject: [PATCH 07/43] Fix test --- tests/robots/test_so100_follower.py | 33 +++++++++++++++++------------ 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/tests/robots/test_so100_follower.py b/tests/robots/test_so100_follower.py index 75c8b993..81d9d6a9 100644 --- a/tests/robots/test_so100_follower.py +++ b/tests/robots/test_so100_follower.py @@ -8,8 +8,6 @@ from lerobot.common.robots.so100_follower import ( SO100FollowerConfig, ) -_MOTORS = SO100Follower(SO100FollowerConfig("")).bus.motors - def _make_bus_mock() -> MagicMock: """Return a bus mock with just the attributes used by the robot.""" @@ -24,13 +22,6 @@ def _make_bus_mock() -> MagicMock: bus.connect.side_effect = _connect bus.disconnect.side_effect = _disconnect - bus.motors = _MOTORS - bus.is_calibrated = True - bus.sync_read.return_value = {m: i for i, m in enumerate(_MOTORS, 1)} - bus.sync_write.return_value = None - bus.write.return_value = None - bus.disable_torque.return_value = None - bus.enable_torque.return_value = None @contextmanager def _dummy_cm(): @@ -43,10 +34,24 @@ def _make_bus_mock() -> MagicMock: @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", - return_value=_make_bus_mock(), + side_effect=_bus_side_effect, ), patch.object(SO100Follower, "configure", lambda self: None), ): @@ -71,20 +76,20 @@ def test_get_observation(follower): follower.connect() obs = follower.get_observation() - expected_keys = {f"{m}.pos" for m in _MOTORS} + expected_keys = {f"{m}.pos" for m in follower.bus.motors} assert set(obs.keys()) == expected_keys - for idx, motor in enumerate(_MOTORS, 1): + 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(_MOTORS, 1)} + 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(_MOTORS)} + 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) From 9dab08dfbc2e7efac7b4c69b272bdbca5c7e7311 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Tue, 20 May 2025 09:53:01 +0200 Subject: [PATCH 08/43] Remove old .cache folder --- .../aloha_default/left_follower.json | 68 ------------------- .../aloha_default/left_leader.json | 68 ------------------- .../aloha_default/right_follower.json | 68 ------------------- .../aloha_default/right_leader.json | 68 ------------------- .gitignore | 7 +- 5 files changed, 4 insertions(+), 275 deletions(-) delete mode 100644 .cache/calibration/aloha_default/left_follower.json delete mode 100644 .cache/calibration/aloha_default/left_leader.json delete mode 100644 .cache/calibration/aloha_default/right_follower.json delete mode 100644 .cache/calibration/aloha_default/right_leader.json 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/.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 From f27f5f84b0a94cf80c0881d2a3682039aa60814e Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 13:05:41 +0200 Subject: [PATCH 09/43] Apply suggestions from code review 1 Co-authored-by: Simon Alibert <75076266+aliberts@users.noreply.github.com> --- .gitattributes | 2 +- lerobot/common/cameras/__init__.py | 2 +- lerobot/common/cameras/camera.py | 4 ++-- lerobot/common/cameras/opencv/configuration_opencv.py | 1 - lerobot/common/utils/control_utils.py | 1 - lerobot/find_cameras.py | 1 - tests/cameras/test_opencv.py | 5 +++-- tests/cameras/test_realsense.py | 10 +++------- 8 files changed, 10 insertions(+), 16 deletions(-) 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/lerobot/common/cameras/__init__.py b/lerobot/common/cameras/__init__.py index 272c4e9e..917bb789 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 from .utils import make_cameras_from_configs diff --git a/lerobot/common/cameras/camera.py b/lerobot/common/cameras/camera.py index 8d88794c..47233571 100644 --- a/lerobot/common/cameras/camera.py +++ b/lerobot/common/cameras/camera.py @@ -33,7 +33,7 @@ class Camera(abc.ABC): pass @abc.abstractmethod - def connect(self, do_warmup_read: bool = True) -> None: + def connect(self, warmup: bool = True) -> None: pass @abc.abstractmethod @@ -41,7 +41,7 @@ class Camera(abc.ABC): pass @abc.abstractmethod - def async_read(self, timeout_ms: float = 2000) -> np.ndarray: + def async_read(self, timeout_ms: float = ...) -> np.ndarray: pass @abc.abstractmethod diff --git a/lerobot/common/cameras/opencv/configuration_opencv.py b/lerobot/common/cameras/opencv/configuration_opencv.py index 5d7c4195..8cced062 100644 --- a/lerobot/common/cameras/opencv/configuration_opencv.py +++ b/lerobot/common/cameras/opencv/configuration_opencv.py @@ -53,7 +53,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): 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/find_cameras.py b/lerobot/find_cameras.py index 6227eaba..f24c0d14 100644 --- a/lerobot/find_cameras.py +++ b/lerobot/find_cameras.py @@ -32,7 +32,6 @@ from lerobot.common.cameras.opencv.camera_opencv import OpenCVCamera from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig 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]]: diff --git a/tests/cameras/test_opencv.py b/tests/cameras/test_opencv.py index 44cb9bb8..f17cb013 100644 --- a/tests/cameras/test_opencv.py +++ b/tests/cameras/test_opencv.py @@ -39,7 +39,8 @@ TEST_IMAGE_PATHS = [ ] -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) @@ -168,7 +169,7 @@ def test_async_read_before_connect(): Cv2Rotation.ROTATE_270, ], ) -def test_all_rotations(rotation, index_or_path): +def test_rotation(rotation, index_or_path): filename = os.path.basename(index_or_path) dimensions = filename.split("_")[-1].split(".")[0] # Assumes filenames format (_wxh.png) original_width, original_height = map(int, dimensions.split("x")) diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index fb5e5009..bb6ad667 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -39,13 +39,8 @@ BAG_FILE_PATH = os.path.join(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.") - 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) @@ -53,7 +48,8 @@ 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(): +def test_abc_implementation(): + """Instantiation should raise an error if the class doesn't implement abstract methods/properties.""" config = RealSenseCameraConfig(serial_number=42) _ = RealSenseCamera(config) @@ -181,7 +177,7 @@ def test_async_read_before_connect(): ], ) @patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_all_rotations(mock_enable_device, rotation): +def test_rotation(mock_enable_device, rotation): config = RealSenseCameraConfig(serial_number=42, rotation=rotation) camera = RealSenseCamera(config) camera.connect(do_warmup_read=False) From 295b96c539154a1d83666341c016b3c6f3cf81c4 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 20 May 2025 11:05:56 +0000 Subject: [PATCH 10/43] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- tests/cameras/test_realsense.py | 1 + 1 file changed, 1 insertion(+) diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index bb6ad667..dd9f5c0a 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -40,6 +40,7 @@ BAG_FILE_PATH = os.path.join(TEST_ARTIFACTS_DIR, "test_rs.bag") # NOTE(Steven): Takes 20sec, the patch being the biggest bottleneck # NOTE(Steven): more tests + assertions? + def mock_rs_config_enable_device_from_file(rs_config_instance, sn): return rs_config_instance.enable_device_from_file(BAG_FILE_PATH, repeat_playback=True) From 8ab22271481b4ab65ad8037eb16e1ebe6c1a5ed2 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Tue, 20 May 2025 13:16:34 +0200 Subject: [PATCH 11/43] Replace deprecated abc.abstractproperty --- lerobot/common/envs/configs.py | 3 ++- lerobot/common/robots/robot.py | 12 ++++++++---- lerobot/common/teleoperators/teleoperator.py | 12 ++++++++---- lerobot/configs/policies.py | 9 ++++++--- 4 files changed, 24 insertions(+), 12 deletions(-) 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/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/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/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 From 1f2cfd38280c24de0442f2bb5d6d3edd00bb089f Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 13:40:45 +0200 Subject: [PATCH 12/43] Apply suggestions from code review camera_realsense.py Co-authored-by: Simon Alibert <75076266+aliberts@users.noreply.github.com> --- .../common/cameras/intel/camera_realsense.py | 104 +++++++----------- 1 file changed, 39 insertions(+), 65 deletions(-) diff --git a/lerobot/common/cameras/intel/camera_realsense.py b/lerobot/common/cameras/intel/camera_realsense.py index 4aadadaa..236e0ff5 100644 --- a/lerobot/common/cameras/intel/camera_realsense.py +++ b/lerobot/common/cameras/intel/camera_realsense.py @@ -68,25 +68,23 @@ class RealSenseCamera(Camera): 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.intel import RealSenseCamera, RealSenseCameraConfig + from lerobot.common.cameras import ColorMode # Basic usage with serial number config = RealSenseCameraConfig(serial_number="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( @@ -132,10 +130,10 @@ class RealSenseCamera(Camera): else: raise ValueError("RealSenseCameraConfig must provide either '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.channels = config.channels + self.color_mode = config.color_mode + self.use_depth = config.use_depth self.rs_pipeline: rs.pipeline | None = None self.rs_profile: rs.pipeline_profile | None = None @@ -144,7 +142,6 @@ 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) @@ -155,7 +152,6 @@ class RealSenseCamera(Camera): self.prerotated_width, self.prerotated_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 @@ -164,34 +160,23 @@ class RealSenseCamera(Camera): return self.rs_pipeline is not None and self.rs_profile is not None @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 = { @@ -250,7 +235,7 @@ 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) @@ -304,7 +289,7 @@ class RealSenseCamera(Camera): self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile() ) - def connect(self, do_warmup_read: bool = True): + def connect(self, warmup: bool = True): """ Connects to the RealSense camera specified in the configuration. @@ -314,14 +299,12 @@ class RealSenseCamera(Camera): 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. + 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. - 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() @@ -332,34 +315,29 @@ class RealSenseCamera(Camera): self.rs_profile = None self.rs_pipeline = None raise ConnectionError( - f"Failed to open RealSense camera {self.serial_number}. Error: {e}. " + f"Failed to open {self} camera. " f"Run 'python -m find_cameras list-cameras' for details." ) from e - logger.debug(f"Validating stream configuration for {self.serial_number}...") + logger.debug(f"Validating stream configuration for {self}...") self._validate_capture_settings() - if do_warmup_read: - logger.debug(f"Reading a warm-up frame for {self.serial_number}...") + if warmup: + logger.debug(f"Reading a warm-up frame for {self}...") 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.") + logger.info(f"{self} connected.") - def _validate_fps(self, stream) -> None: + def _validate_fps(self, stream: rs.video_stream_profile) -> None: """Validates and sets the internal FPS based on actual stream FPS.""" actual_fps = stream.fps() if self.fps is None: self.fps = actual_fps - logger.info(f"FPS not specified, using camera default: {self.fps} FPS.") return # 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}." ) @@ -399,7 +377,7 @@ class RealSenseCamera(Camera): ) 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 +385,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) @@ -428,13 +406,11 @@ class RealSenseCamera(Camera): 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." + f"{self} failed to capture frame. Returned status='{ret}'." ) depth_frame = frame.get_depth_frame() @@ -448,7 +424,7 @@ class RealSenseCamera(Camera): 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 +432,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 @@ -569,7 +545,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) @@ -578,14 +554,14 @@ class RealSenseCamera(Camera): self.stop_event = Event() self.thread = Thread( - target=self._read_loop, args=(), name=f"RealSenseReadLoop-{self}-{self.serial_number}" + 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}.") # 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. @@ -596,7 +572,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: @@ -663,8 +639,6 @@ 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() @@ -674,4 +648,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.") From 05d8825bcb949c716f8b09f275af7f9582f79a11 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 20 May 2025 11:40:59 +0000 Subject: [PATCH 13/43] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- lerobot/common/cameras/intel/camera_realsense.py | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/lerobot/common/cameras/intel/camera_realsense.py b/lerobot/common/cameras/intel/camera_realsense.py index 236e0ff5..a7d9a059 100644 --- a/lerobot/common/cameras/intel/camera_realsense.py +++ b/lerobot/common/cameras/intel/camera_realsense.py @@ -142,7 +142,6 @@ class RealSenseCamera(Camera): self.stop_event: Event | None = None self.frame_queue: queue.Queue = queue.Queue(maxsize=1) - self.rotation: int | None = get_cv2_rotation(config.rotation) if self.height and self.width: @@ -177,7 +176,6 @@ class RealSenseCamera(Camera): context = rs.context() devices = context.query_devices() - for device in devices: camera_info = { "name": device.get_info(rs.camera_info.name), @@ -315,8 +313,7 @@ class RealSenseCamera(Camera): self.rs_profile = None self.rs_pipeline = None raise ConnectionError( - f"Failed to open {self} camera. " - f"Run 'python -m find_cameras list-cameras' for details." + f"Failed to open {self} camera. Run 'python -m find_cameras list-cameras' for details." ) from e logger.debug(f"Validating stream configuration for {self}...") @@ -409,9 +406,7 @@ class RealSenseCamera(Camera): ret, frame = self.rs_pipeline.try_wait_for_frames(timeout_ms=timeout_ms) if not ret or frame is None: - raise RuntimeError( - f"{self} failed to capture frame. Returned status='{ret}'." - ) + 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()) @@ -553,9 +548,7 @@ class RealSenseCamera(Camera): self.stop_event.set() self.stop_event = Event() - self.thread = Thread( - target=self._read_loop, args=(), name=f"{self}_read_loop" - ) + 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}.") From 3890c38c12a53cf1a041a773b65b3b4b02e3d7a2 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 13:47:00 +0200 Subject: [PATCH 14/43] Apply suggestions from code review camera_opencv Co-authored-by: Simon Alibert <75076266+aliberts@users.noreply.github.com> --- .../common/cameras/opencv/camera_opencv.py | 28 ++++++++----------- 1 file changed, 11 insertions(+), 17 deletions(-) diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index e80b9eb2..7af910ec 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -114,19 +114,18 @@ 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.channels = config.channels + self.color_mode = config.color_mode - 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 @@ -138,7 +137,6 @@ class OpenCVCamera(Camera): self.prerotated_width, self.prerotated_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 @@ -170,7 +168,7 @@ class OpenCVCamera(Camera): self._validate_fps() self._validate_width_and_height() - def connect(self, do_warmup_read: bool = True): + def connect(self, warmup: bool = True): """ Connects to the OpenCV camera specified in the configuration. @@ -190,7 +188,6 @@ class OpenCVCamera(Camera): # 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(): @@ -201,7 +198,6 @@ class OpenCVCamera(Camera): 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: @@ -272,7 +268,7 @@ class OpenCVCamera(Camera): @staticmethod def find_cameras( - max_index_search_range=MAX_OPENCV_INDEX, raise_when_empty: bool = True + max_index_search_range=MAX_OPENCV_INDEX ) -> List[Dict[str, Any]]: """ Detects available OpenCV cameras connected to the system. @@ -451,7 +447,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) @@ -510,7 +506,7 @@ class OpenCVCamera(Camera): 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): + 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: logger.debug(f"Signaling stop event for read thread of {self}.") @@ -539,17 +535,15 @@ class OpenCVCamera(Camera): """ if not self.is_connected and self.thread is None: raise DeviceNotConnectedError( - f"Attempted to disconnect {self}, but it appears already disconnected." + f"{self} not connected." ) - logger.debug(f"Disconnecting from camera {self.index_or_path}...") if self.thread is not None: self._shutdown_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 - logger.info(f"Camera {self.index_or_path} disconnected successfully.") + logger.info(f"{self} disconnected.") From 3a08eddeabd9d762538ac84adc6d5a74171c266b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 20 May 2025 11:47:12 +0000 Subject: [PATCH 15/43] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- lerobot/common/cameras/opencv/camera_opencv.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index 7af910ec..13da48f7 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -33,7 +33,7 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte 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, @@ -126,7 +126,6 @@ class OpenCVCamera(Camera): self.stop_event: Event | None = None self.frame_queue: queue.Queue = queue.Queue(maxsize=1) - 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 @@ -267,9 +266,7 @@ class OpenCVCamera(Camera): logger.debug(f"Capture height set to {actual_height} for {self}.") @staticmethod - def find_cameras( - max_index_search_range=MAX_OPENCV_INDEX - ) -> List[Dict[str, Any]]: + def find_cameras(max_index_search_range=MAX_OPENCV_INDEX) -> List[Dict[str, Any]]: """ Detects available OpenCV cameras connected to the system. @@ -534,10 +531,7 @@ class OpenCVCamera(Camera): DeviceNotConnectedError: If the camera is already disconnected. """ if not self.is_connected and self.thread is None: - raise DeviceNotConnectedError( - f"{self} not connected." - ) - + raise DeviceNotConnectedError(f"{self} not connected.") if self.thread is not None: self._shutdown_read_thread() From 41179d0996122b912e10e9989f8a05e91faca68b Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 14:42:05 +0200 Subject: [PATCH 16/43] chore(cameras): address review comments + make test pass again --- lerobot/common/cameras/camera.py | 58 +++++++++++++++++++ .../common/cameras/intel/camera_realsense.py | 22 +++---- .../cameras/intel/configuration_realsense.py | 5 -- .../common/cameras/opencv/camera_opencv.py | 54 +++++++---------- .../cameras/opencv/configuration_opencv.py | 4 -- lerobot/find_cameras.py | 56 ++++++------------ pyproject.toml | 2 +- tests/artifacts/cameras/image_128x128.png | 3 + tests/artifacts/cameras/image_160x120.png | 3 + tests/artifacts/cameras/image_320x180.png | 3 + tests/artifacts/cameras/image_480x270.png | 3 + tests/cameras/test_opencv.py | 36 ++++++------ tests/cameras/test_realsense.py | 28 ++++----- 13 files changed, 150 insertions(+), 127 deletions(-) create mode 100644 tests/artifacts/cameras/image_128x128.png create mode 100644 tests/artifacts/cameras/image_160x120.png create mode 100644 tests/artifacts/cameras/image_320x180.png create mode 100644 tests/artifacts/cameras/image_480x270.png diff --git a/lerobot/common/cameras/camera.py b/lerobot/common/cameras/camera.py index 47233571..c06439c2 100644 --- a/lerobot/common/cameras/camera.py +++ b/lerobot/common/cameras/camera.py @@ -22,7 +22,35 @@ 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 + + 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 @@ -30,20 +58,50 @@ class Camera(abc.ABC): @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 @abc.abstractmethod def connect(self, warmup: bool = True) -> None: + """Establish connection to the camera. + + Args: + warmup: If True (default), captures a warmup frame before returning. + """ 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 = ...) -> 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/intel/camera_realsense.py b/lerobot/common/cameras/intel/camera_realsense.py index a7d9a059..90881f11 100644 --- a/lerobot/common/cameras/intel/camera_realsense.py +++ b/lerobot/common/cameras/intel/camera_realsense.py @@ -29,7 +29,6 @@ import numpy as np import pyrealsense2 as rs from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError -from lerobot.common.utils.utils import capture_timestamp_utc from ..camera import Camera from ..configs import ColorMode @@ -131,7 +130,6 @@ class RealSenseCamera(Camera): raise ValueError("RealSenseCameraConfig must provide either 'serial_number' or 'name'.") self.fps = config.fps - self.channels = config.channels self.color_mode = config.color_mode self.use_depth = config.use_depth @@ -213,7 +211,7 @@ class RealSenseCamera(Camera): 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: @@ -304,7 +302,7 @@ class RealSenseCamera(Camera): raise DeviceAlreadyConnectedError(f"{self} is already connected.") self.rs_pipeline = rs.pipeline() - rs_config = self._configure_realsense_settings() + rs_config = self._make_rs_pipeline_config() try: self.rs_profile = self.rs_pipeline.start(rs_config) @@ -313,7 +311,7 @@ class RealSenseCamera(Camera): self.rs_profile = None self.rs_pipeline = None raise ConnectionError( - f"Failed to open {self} camera. Run 'python -m find_cameras list-cameras' for details." + f"Failed to open {self} camera. Run 'python -m find_cameras' for details about the available cameras in your system." ) from e logger.debug(f"Validating stream configuration for {self}...") @@ -416,7 +414,6 @@ 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 depth_map_processed def read(self, color_mode: ColorMode | None = None, timeout_ms: int = 100) -> np.ndarray: @@ -461,7 +458,6 @@ 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( @@ -492,11 +488,7 @@ class RealSenseCamera(Camera): if depth_frame: h, w = image.shape else: - h, w, c = image.shape - if c != self.channels: - logger.warning( - f"Captured frame channels ({c}) do not match configured channels ({self.channels}) for {self}." - ) + h, w, _c = image.shape if h != self.prerotated_height or w != self.prerotated_width: raise RuntimeError( @@ -580,7 +572,7 @@ 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) @@ -600,7 +592,7 @@ class RealSenseCamera(Camera): f"Error getting frame data from queue for camera {self.serial_number}: {e}" ) from e - def _shutdown_read_thread(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: logger.debug(f"Signaling stop event for read thread of {self}.") @@ -633,7 +625,7 @@ class RealSenseCamera(Camera): ) 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}.") diff --git a/lerobot/common/cameras/intel/configuration_realsense.py b/lerobot/common/cameras/intel/configuration_realsense.py index 5d9f8a80..c3eae8f0 100644 --- a/lerobot/common/cameras/intel/configuration_realsense.py +++ b/lerobot/common/cameras/intel/configuration_realsense.py @@ -44,7 +44,6 @@ class RealSenseCameraConfig(CameraConfig): serial_number: Optional unique serial number to identify the camera. Either name or serial_number must be provided. 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. @@ -58,7 +57,6 @@ class RealSenseCameraConfig(CameraConfig): name: str | None = None serial_number: int | None = None 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,9 +76,6 @@ 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): raise ValueError( f"One of them must be set: name or serial_number, but {self.name=} and {self.serial_number=} provided." diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index 13da48f7..8485dc27 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -30,7 +30,6 @@ 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 get_cv2_backend, get_cv2_rotation @@ -117,7 +116,6 @@ class OpenCVCamera(Camera): self.index_or_path = config.index_or_path self.fps = config.fps - self.channels = config.channels self.color_mode = config.color_mode self.videocapture: cv2.VideoCapture | None = None @@ -141,7 +139,7 @@ class OpenCVCamera(Camera): @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 _configure_capture_settings(self) -> None: """ @@ -187,19 +185,19 @@ class OpenCVCamera(Camera): # blocking in multi-threaded applications, especially during data collection. cv2.setNumThreads(1) - self.videocapture_camera = cv2.VideoCapture(self.index_or_path) + self.videocapture = cv2.VideoCapture(self.index_or_path) - if not self.videocapture_camera.isOpened(): - self.videocapture_camera.release() - self.videocapture_camera = None + 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 find_cameras list-cameras' for details." + f"Run 'python -m find_cameras Run 'python -m find_cameras' for details about the available cameras in your system." ) self._configure_capture_settings() - if do_warmup_read: + if warmup: 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 @@ -209,12 +207,12 @@ class OpenCVCamera(Camera): """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) + self.fps = self.videocapture.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( @@ -229,8 +227,8 @@ class OpenCVCamera(Camera): def _validate_width_and_height(self) -> None: """Validates and sets the camera's frame capture width and height.""" - default_width = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_WIDTH))) - default_height = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_HEIGHT))) + default_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH))) + default_height = int(round(self.videocapture.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]: @@ -243,8 +241,8 @@ class OpenCVCamera(Camera): 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)) - actual_width = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_WIDTH))) + success = self.videocapture.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.prerotated_width)) + actual_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_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})." @@ -254,8 +252,8 @@ class OpenCVCamera(Camera): ) 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)) - actual_height = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_HEIGHT))) + success = self.videocapture.set(cv2.CAP_PROP_FRAME_HEIGHT, float(self.prerotated_height)) + actual_height = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_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})." @@ -275,7 +273,6 @@ class OpenCVCamera(Camera): 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. Returns: List[Dict[str, Any]]: A list of dictionaries, @@ -321,8 +318,6 @@ class OpenCVCamera(Camera): 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 @@ -356,7 +351,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( @@ -369,7 +364,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: @@ -402,10 +396,6 @@ class OpenCVCamera(Camera): 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}." - ) processed_image = image if requested_color_mode == ColorMode.RGB: @@ -485,7 +475,7 @@ 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) @@ -534,10 +524,10 @@ class OpenCVCamera(Camera): 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: - self.videocapture_camera.release() - self.videocapture_camera = None + if self.videocapture is not None: + self.videocapture.release() + self.videocapture = None logger.info(f"{self} disconnected.") diff --git a/lerobot/common/cameras/opencv/configuration_opencv.py b/lerobot/common/cameras/opencv/configuration_opencv.py index 8cced062..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: @@ -70,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/find_cameras.py b/lerobot/find_cameras.py index f24c0d14..19495733 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 @@ -44,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.") @@ -64,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.") @@ -179,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}") @@ -292,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="?", @@ -321,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, @@ -343,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/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/image_128x128.png b/tests/artifacts/cameras/image_128x128.png new file mode 100644 index 00000000..b117f49f --- /dev/null +++ b/tests/artifacts/cameras/image_128x128.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9dc9df05797dc0e7b92edc845caab2e4c37c3cfcabb4ee6339c67212b5baba3b +size 38023 diff --git a/tests/artifacts/cameras/image_160x120.png b/tests/artifacts/cameras/image_160x120.png new file mode 100644 index 00000000..cdc681d1 --- /dev/null +++ b/tests/artifacts/cameras/image_160x120.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7e11af87616b83c1cdb30330e951b91e86b51c64a1326e1ba5b4a3fbcdec1a11 +size 55698 diff --git a/tests/artifacts/cameras/image_320x180.png b/tests/artifacts/cameras/image_320x180.png new file mode 100644 index 00000000..4cfd511a --- /dev/null +++ b/tests/artifacts/cameras/image_320x180.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b8840fb643afe903191248703b1f95a57faf5812ecd9978ac502ee939646fdb2 +size 121115 diff --git a/tests/artifacts/cameras/image_480x270.png b/tests/artifacts/cameras/image_480x270.png new file mode 100644 index 00000000..b564d542 --- /dev/null +++ b/tests/artifacts/cameras/image_480x270.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f79d14daafb1c0cf2fec5d46ee8029a73fe357402fdd31a7cd4a4794d7319a7c +size 260367 diff --git a/tests/cameras/test_opencv.py b/tests/cameras/test_opencv.py index f17cb013..609c10c3 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,13 +29,13 @@ 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 / "fakecam_sd_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", ] @@ -50,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 @@ -58,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(): @@ -69,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(): @@ -81,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) 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() @@ -106,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() @@ -125,7 +125,7 @@ def test_disconnect_before_connect(): 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() @@ -141,7 +141,7 @@ 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): @@ -170,13 +170,13 @@ def test_async_read_before_connect(): ], ) def test_rotation(rotation, index_or_path): - filename = os.path.basename(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 dd9f5c0a..df11987b 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 @@ -33,8 +33,8 @@ try: except (ImportError, ModuleNotFoundError): 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 @@ -42,7 +42,7 @@ BAG_FILE_PATH = os.path.join(TEST_ARTIFACTS_DIR, "test_rs.bag") def mock_rs_config_enable_device_from_file(rs_config_instance, sn): - return rs_config_instance.enable_device_from_file(BAG_FILE_PATH, repeat_playback=True) + 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): @@ -60,7 +60,7 @@ def test_connect(mock_enable_device): config = RealSenseCameraConfig(serial_number=42) camera = RealSenseCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) assert camera.is_connected @@ -68,10 +68,10 @@ def test_connect(mock_enable_device): def test_connect_already_connected(mock_enable_device): config = RealSenseCameraConfig(serial_number=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) @@ -80,7 +80,7 @@ def test_connect_invalid_camera_path(mock_enable_device): 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) @@ -89,14 +89,14 @@ def test_invalid_width_connect(mock_enable_device): 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) camera = RealSenseCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) img = camera.read() assert isinstance(img, np.ndarray) @@ -114,7 +114,7 @@ def test_read_before_connect(): def test_disconnect(mock_enable_device): config = RealSenseCameraConfig(serial_number=42) camera = RealSenseCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) camera.disconnect() @@ -133,7 +133,7 @@ def test_disconnect_before_connect(): def test_async_read(mock_enable_device): config = RealSenseCameraConfig(serial_number=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() @@ -150,7 +150,7 @@ def test_async_read(mock_enable_device): def test_async_read_timeout(mock_enable_device): config = RealSenseCameraConfig(serial_number=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): @@ -181,7 +181,7 @@ def test_async_read_before_connect(): def test_rotation(mock_enable_device, rotation): config = RealSenseCameraConfig(serial_number=42, rotation=rotation) camera = RealSenseCamera(config) - camera.connect(do_warmup_read=False) + camera.connect(warmup=False) img = camera.read() assert isinstance(img, np.ndarray) From 85b0dd0ec1d8b1145bfeb0a8f8da5a16f6cae882 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 14:43:23 +0200 Subject: [PATCH 17/43] chore(tests): delete fakecam images --- tests/artifacts/cameras/fakecam_fullhd_480x270.png | 3 --- tests/artifacts/cameras/fakecam_hd_320x180.png | 3 --- tests/artifacts/cameras/fakecam_sd_160x120.png | 3 --- tests/artifacts/cameras/fakecam_square_128x128.png | 3 --- 4 files changed, 12 deletions(-) delete mode 100644 tests/artifacts/cameras/fakecam_fullhd_480x270.png delete mode 100644 tests/artifacts/cameras/fakecam_hd_320x180.png delete mode 100644 tests/artifacts/cameras/fakecam_sd_160x120.png delete mode 100644 tests/artifacts/cameras/fakecam_square_128x128.png diff --git a/tests/artifacts/cameras/fakecam_fullhd_480x270.png b/tests/artifacts/cameras/fakecam_fullhd_480x270.png deleted file mode 100644 index b564d542..00000000 --- a/tests/artifacts/cameras/fakecam_fullhd_480x270.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f79d14daafb1c0cf2fec5d46ee8029a73fe357402fdd31a7cd4a4794d7319a7c -size 260367 diff --git a/tests/artifacts/cameras/fakecam_hd_320x180.png b/tests/artifacts/cameras/fakecam_hd_320x180.png deleted file mode 100644 index 4cfd511a..00000000 --- a/tests/artifacts/cameras/fakecam_hd_320x180.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b8840fb643afe903191248703b1f95a57faf5812ecd9978ac502ee939646fdb2 -size 121115 diff --git a/tests/artifacts/cameras/fakecam_sd_160x120.png b/tests/artifacts/cameras/fakecam_sd_160x120.png deleted file mode 100644 index cdc681d1..00000000 --- a/tests/artifacts/cameras/fakecam_sd_160x120.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7e11af87616b83c1cdb30330e951b91e86b51c64a1326e1ba5b4a3fbcdec1a11 -size 55698 diff --git a/tests/artifacts/cameras/fakecam_square_128x128.png b/tests/artifacts/cameras/fakecam_square_128x128.png deleted file mode 100644 index b117f49f..00000000 --- a/tests/artifacts/cameras/fakecam_square_128x128.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9dc9df05797dc0e7b92edc845caab2e4c37c3cfcabb4ee6339c67212b5baba3b -size 38023 From 9812f2db09f2c1f04bad89e41b001cea287fff86 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 16:18:49 +0200 Subject: [PATCH 18/43] chore(cameras): rename intel -> realsense --- lerobot/common/cameras/{intel => realsense}/__init__.py | 0 lerobot/common/cameras/{intel => realsense}/camera_realsense.py | 0 .../cameras/{intel => realsense}/configuration_realsense.py | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename lerobot/common/cameras/{intel => realsense}/__init__.py (100%) rename lerobot/common/cameras/{intel => realsense}/camera_realsense.py (100%) rename lerobot/common/cameras/{intel => realsense}/configuration_realsense.py (100%) 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 100% rename from lerobot/common/cameras/intel/camera_realsense.py rename to lerobot/common/cameras/realsense/camera_realsense.py diff --git a/lerobot/common/cameras/intel/configuration_realsense.py b/lerobot/common/cameras/realsense/configuration_realsense.py similarity index 100% rename from lerobot/common/cameras/intel/configuration_realsense.py rename to lerobot/common/cameras/realsense/configuration_realsense.py From 525cd68e62c61667860018d354b6c32d83874a78 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 16:39:17 +0200 Subject: [PATCH 19/43] chore(cameras): fix imports renamed folder + add camera configs in utility scripts --- lerobot/calibrate.py | 2 ++ lerobot/common/cameras/realsense/camera_realsense.py | 2 +- lerobot/common/cameras/utils.py | 2 +- lerobot/common/robots/stretch3/configuration_stretch3.py | 2 +- lerobot/find_cameras.py | 4 ++-- lerobot/record.py | 2 ++ lerobot/teleoperate.py | 2 ++ tests/cameras/test_opencv.py | 2 +- tests/cameras/test_realsense.py | 2 +- tests/utils.py | 2 +- 10 files changed, 14 insertions(+), 8 deletions(-) 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/realsense/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py index 90881f11..5db6d417 100644 --- a/lerobot/common/cameras/realsense/camera_realsense.py +++ b/lerobot/common/cameras/realsense/camera_realsense.py @@ -67,7 +67,7 @@ class RealSenseCamera(Camera): Example: ```python - from lerobot.common.cameras.intel import RealSenseCamera, RealSenseCameraConfig + from lerobot.common.cameras.realsense import RealSenseCamera, RealSenseCameraConfig from lerobot.common.cameras import ColorMode # Basic usage with serial number diff --git a/lerobot/common/cameras/utils.py b/lerobot/common/cameras/utils.py index 30f5dd69..7ed3149e 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: 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/find_cameras.py b/lerobot/find_cameras.py index 19495733..1d051d69 100644 --- a/lerobot/find_cameras.py +++ b/lerobot/find_cameras.py @@ -36,10 +36,10 @@ 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__) 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/tests/cameras/test_opencv.py b/tests/cameras/test_opencv.py index 609c10c3..06db1ce7 100644 --- a/tests/cameras/test_opencv.py +++ b/tests/cameras/test_opencv.py @@ -30,7 +30,7 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte # NOTE(Steven): more tests + assertions? TEST_ARTIFACTS_DIR = Path(__file__).parent.parent / "artifacts" / "cameras" -DEFAULT_PNG_FILE_PATH = TEST_ARTIFACTS_DIR / "fakecam_sd_160x120.png" +DEFAULT_PNG_FILE_PATH = TEST_ARTIFACTS_DIR / "image_160x120.png" TEST_IMAGE_PATHS = [ TEST_ARTIFACTS_DIR / "image_160x120.png", TEST_ARTIFACTS_DIR / "image_320x180.png", diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index df11987b..8e6f736e 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -29,7 +29,7 @@ from lerobot.common.cameras.configs import Cv2Rotation from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError try: - from lerobot.common.cameras.intel import RealSenseCamera, RealSenseCameraConfig + from lerobot.common.cameras.realsense import RealSenseCamera, RealSenseCameraConfig except (ImportError, ModuleNotFoundError): pytest.skip("pyrealsense2 not available", allow_module_level=True) 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) From ab7565a666024c3ed276c93ec317f4eb79c44df0 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 16:43:58 +0200 Subject: [PATCH 20/43] chore(cameras): refactor utility function dictionary fetching style --- lerobot/common/cameras/utils.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/lerobot/common/cameras/utils.py b/lerobot/common/cameras/utils.py index 7ed3149e..809fe085 100644 --- a/lerobot/common/cameras/utils.py +++ b/lerobot/common/cameras/utils.py @@ -49,21 +49,25 @@ def make_cameras_from_configs(camera_configs: dict[str, CameraConfig]) -> dict[s def get_cv2_rotation(rotation: Cv2Rotation) -> int: import cv2 - return { + available_rotation_dict = { Cv2Rotation.ROTATE_270: cv2.ROTATE_90_COUNTERCLOCKWISE, Cv2Rotation.ROTATE_90: cv2.ROTATE_90_CLOCKWISE, Cv2Rotation.ROTATE_180: cv2.ROTATE_180, - }.get(rotation) + } + + return available_rotation_dict.get(rotation) def get_cv2_backend() -> int: import cv2 - return { + available_cv2_backends = { "Linux": cv2.CAP_DSHOW, "Windows": cv2.CAP_AVFOUNDATION, "Darwin": cv2.CAP_ANY, - }.get(platform.system(), cv2.CAP_V4L2) + } + + return available_cv2_backends.get(platform.system(), cv2.CAP_V4L2) def save_image(img_array: np.ndarray, camera_index: int, frame_index: int, images_dir: Path): From efd2849184b1b005ddd91c270655caac8640f49e Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 16:54:52 +0200 Subject: [PATCH 21/43] chore(tests): add ids to parametrize --- tests/cameras/test_opencv.py | 7 ++++--- tests/cameras/test_realsense.py | 1 + 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/tests/cameras/test_opencv.py b/tests/cameras/test_opencv.py index 06db1ce7..56ba3bb2 100644 --- a/tests/cameras/test_opencv.py +++ b/tests/cameras/test_opencv.py @@ -84,7 +84,7 @@ def test_invalid_width_connect(): 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) @@ -121,7 +121,7 @@ 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) @@ -159,7 +159,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", [ @@ -168,6 +168,7 @@ def test_async_read_before_connect(): Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270, ], + ids=["no_rot", "rot90", "rot180", "rot270"], ) def test_rotation(rotation, index_or_path): filename = Path(index_or_path).name diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index 8e6f736e..6adede76 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -176,6 +176,7 @@ 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_rotation(mock_enable_device, rotation): From 99ea24d6a3cb8e06c60e1669d2c29ff4c3e3714f Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 17:09:16 +0200 Subject: [PATCH 22/43] chore(cameras): move connect under is_connected --- .../common/cameras/opencv/camera_opencv.py | 48 ++++++------ .../cameras/realsense/camera_realsense.py | 76 +++++++++---------- 2 files changed, 62 insertions(+), 62 deletions(-) diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index 8485dc27..a7439832 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -141,30 +141,6 @@ class OpenCVCamera(Camera): """Checks if the camera is currently connected and opened.""" return isinstance(self.videocapture, cv2.VideoCapture) and self.videocapture.isOpened() - def _configure_capture_settings(self) -> None: - """ - Applies the specified FPS, width, and height settings to the connected camera. - - This method attempts to set the camera properties via OpenCV. It checks if - the camera successfully applied the settings and raises an error if not. - - Args: - fps: The desired frames per second. If None, the setting is skipped. - width: The desired capture width. If None, the setting is skipped. - height: The desired capture height. If None, the setting is skipped. - - Raises: - RuntimeError: If the camera fails to set any of the specified properties - to the requested value. - DeviceNotConnectedError: If the camera is not connected when attempting - to configure settings. - """ - 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() - def connect(self, warmup: bool = True): """ Connects to the OpenCV camera specified in the configuration. @@ -203,6 +179,30 @@ class OpenCVCamera(Camera): logger.debug(f"Camera {self.index_or_path} connected and configured successfully.") + def _configure_capture_settings(self) -> None: + """ + Applies the specified FPS, width, and height settings to the connected camera. + + This method attempts to set the camera properties via OpenCV. It checks if + the camera successfully applied the settings and raises an error if not. + + Args: + fps: The desired frames per second. If None, the setting is skipped. + width: The desired capture width. If None, the setting is skipped. + height: The desired capture height. If None, the setting is skipped. + + Raises: + RuntimeError: If the camera fails to set any of the specified properties + to the requested value. + DeviceNotConnectedError: If the camera is not connected when attempting + to configure settings. + """ + 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() + def _validate_fps(self) -> None: """Validates and sets the camera's frames per second (FPS).""" diff --git a/lerobot/common/cameras/realsense/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py index 5db6d417..178b17fd 100644 --- a/lerobot/common/cameras/realsense/camera_realsense.py +++ b/lerobot/common/cameras/realsense/camera_realsense.py @@ -156,6 +156,44 @@ 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 find_cameras' 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: + logger.debug(f"Reading a warm-up frame for {self}...") + self.read() # NOTE(Steven): For now we just read one frame, we could also loop for X frames/secs + + logger.info(f"{self} connected.") + @staticmethod def find_cameras() -> List[Dict[str, Any]]: """ @@ -285,44 +323,6 @@ class RealSenseCamera(Camera): self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile() ) - 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 find_cameras' 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: - logger.debug(f"Reading a warm-up frame for {self}...") - self.read() # NOTE(Steven): For now we just read one frame, we could also loop for X frames/secs - - logger.info(f"{self} connected.") - def _validate_fps(self, stream: rs.video_stream_profile) -> None: """Validates and sets the internal FPS based on actual stream FPS.""" actual_fps = stream.fps() From 39f908e9db68e82a8e667a3ff4373dd06c587a99 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 17:24:51 +0200 Subject: [PATCH 23/43] chore(cameras): move if check out of _validate functions --- .../common/cameras/opencv/camera_opencv.py | 41 +++++++-------- .../cameras/realsense/camera_realsense.py | 52 ++++++++++--------- tests/cameras/test_realsense.py | 4 +- 3 files changed, 50 insertions(+), 47 deletions(-) diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index a7439832..e680982d 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -200,16 +200,29 @@ 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() - - def _validate_fps(self) -> None: - """Validates and sets the camera's frames per second (FPS).""" - if self.fps is None: self.fps = self.videocapture.get(cv2.CAP_PROP_FPS) logger.info(f"FPS set to camera default: {self.fps}.") - return + else: + self._validate_fps() + + default_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH))) + default_height = int(round(self.videocapture.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 = default_height, default_width + self.prerotated_width, self.prerotated_height = default_width, default_height + else: + self.width, self.height = default_width, default_height + self.prerotated_width, self.prerotated_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).""" success = self.videocapture.set(cv2.CAP_PROP_FPS, float(self.fps)) actual_fps = self.videocapture.get(cv2.CAP_PROP_FPS) @@ -227,20 +240,6 @@ class OpenCVCamera(Camera): def _validate_width_and_height(self) -> None: """Validates and sets the camera's frame capture width and height.""" - default_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH))) - default_height = int(round(self.videocapture.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 = default_height, default_width - self.prerotated_width, self.prerotated_height = default_width, default_height - else: - self.width, self.height = default_width, default_height - self.prerotated_width, self.prerotated_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}.") - return - success = self.videocapture.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.prerotated_width)) actual_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH))) if not success or self.prerotated_width != actual_width: diff --git a/lerobot/common/cameras/realsense/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py index 178b17fd..00bba2d1 100644 --- a/lerobot/common/cameras/realsense/camera_realsense.py +++ b/lerobot/common/cameras/realsense/camera_realsense.py @@ -314,32 +314,12 @@ 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 _validate_fps(self, stream: rs.video_stream_profile) -> 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 - return + self.fps = stream.fps() + else: + self._validate_fps(stream) - # Use math.isclose for robust float comparison - if not math.isclose(self.fps, actual_fps, rel_tol=1e-3): - 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}.") - - 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())) @@ -352,7 +332,29 @@ class RealSenseCamera(Camera): 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 + 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_fps(stream) + self._validate_width_and_height(stream) + + def _validate_fps(self, stream: rs.video_stream_profile) -> None: + """Validates and sets the internal FPS based on actual stream FPS.""" + actual_fps = stream.fps() + + # Use math.isclose for robust float comparison + if not math.isclose(self.fps, actual_fps, rel_tol=1e-3): + 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}.") + + 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.prerotated_width != actual_width: logger.warning( diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index 6adede76..9f198564 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -154,7 +154,9 @@ def test_async_read_timeout(mock_enable_device): 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() From b3dafcfb077a489816ee4b271abe472840663dc7 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 17:35:24 +0200 Subject: [PATCH 24/43] docs(cameras): update depth related example --- lerobot/common/cameras/realsense/camera_realsense.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lerobot/common/cameras/realsense/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py index 00bba2d1..383f9efd 100644 --- a/lerobot/common/cameras/realsense/camera_realsense.py +++ b/lerobot/common/cameras/realsense/camera_realsense.py @@ -98,8 +98,8 @@ class RealSenseCamera(Camera): 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() From 39a93a7b28ab76a2835c9c0164880928ae3c0b5d Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 17:49:19 +0200 Subject: [PATCH 25/43] docs(cameras): update arg in class doc --- lerobot/common/cameras/opencv/camera_opencv.py | 4 ---- lerobot/common/cameras/realsense/camera_realsense.py | 5 ----- 2 files changed, 9 deletions(-) diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index e680982d..eda86eb1 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -64,10 +64,6 @@ class OpenCVCamera(Camera): 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 diff --git a/lerobot/common/cameras/realsense/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py index 383f9efd..9242b8d0 100644 --- a/lerobot/common/cameras/realsense/camera_realsense.py +++ b/lerobot/common/cameras/realsense/camera_realsense.py @@ -60,11 +60,6 @@ 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.realsense import RealSenseCamera, RealSenseCameraConfig From 18b56e15336e5c133ba58bc33bf65fcddf246e73 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 18:03:53 +0200 Subject: [PATCH 26/43] refactor(cameras): realsense config arg serial_number_or_name --- .../cameras/realsense/camera_realsense.py | 14 +++++----- .../realsense/configuration_realsense.py | 12 ++------- tests/cameras/test_realsense.py | 26 +++++++++---------- 3 files changed, 21 insertions(+), 31 deletions(-) diff --git a/lerobot/common/cameras/realsense/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py index 9242b8d0..c39e3469 100644 --- a/lerobot/common/cameras/realsense/camera_realsense.py +++ b/lerobot/common/cameras/realsense/camera_realsense.py @@ -66,7 +66,7 @@ class RealSenseCamera(Camera): from lerobot.common.cameras import ColorMode # 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) camera.connect() @@ -82,7 +82,7 @@ class RealSenseCamera(Camera): # 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, @@ -99,7 +99,7 @@ class RealSenseCamera(Camera): 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 ... ``` @@ -117,12 +117,10 @@ 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 = config.fps self.color_mode = config.color_mode diff --git a/lerobot/common/cameras/realsense/configuration_realsense.py b/lerobot/common/cameras/realsense/configuration_realsense.py index c3eae8f0..ed1f76e7 100644 --- a/lerobot/common/cameras/realsense/configuration_realsense.py +++ b/lerobot/common/cameras/realsense/configuration_realsense.py @@ -40,9 +40,7 @@ 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. use_depth: Whether to enable depth stream. Defaults to False. rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation. @@ -54,8 +52,7 @@ class RealSenseCameraConfig(CameraConfig): - Only 3-channel color output (RGB/BGR) is currently supported. """ - name: str | None = None - serial_number: int | None = None + serial_number_or_name: int | str color_mode: ColorMode = ColorMode.RGB use_depth: bool = False rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION # NOTE(Steven): Check if draccus can parse to an enum @@ -75,8 +72,3 @@ class RealSenseCameraConfig(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 bool(self.name) and bool(self.serial_number): - raise ValueError( - f"One of them must be set: name or serial_number, but {self.name=} and {self.serial_number=} provided." - ) diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index 9f198564..3ca9db97 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -51,13 +51,13 @@ def mock_rs_config_enable_device_bad_file(rs_config_instance, sn): def test_abc_implementation(): """Instantiation should raise an error if the class doesn't implement abstract methods/properties.""" - config = RealSenseCameraConfig(serial_number=42) + 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) + config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) camera.connect(warmup=False) @@ -66,7 +66,7 @@ def test_connect(mock_enable_device): @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) + config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) camera.connect(warmup=False) @@ -76,7 +76,7 @@ def test_connect_already_connected(mock_enable_device): @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) + config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) with pytest.raises(ConnectionError): @@ -85,7 +85,7 @@ def test_connect_invalid_camera_path(mock_enable_device): @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) + config = RealSenseCameraConfig(serial_number_or_name=42, width=99999, height=480, fps=30) camera = RealSenseCamera(config) with pytest.raises(ConnectionError): @@ -94,7 +94,7 @@ def test_invalid_width_connect(mock_enable_device): @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) + config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30) camera = RealSenseCamera(config) camera.connect(warmup=False) @@ -103,7 +103,7 @@ def test_read(mock_enable_device): def test_read_before_connect(): - config = RealSenseCameraConfig(serial_number=42) + config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) with pytest.raises(DeviceNotConnectedError): @@ -112,7 +112,7 @@ def test_read_before_connect(): @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) + config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) camera.connect(warmup=False) @@ -122,7 +122,7 @@ 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): @@ -131,7 +131,7 @@ def test_disconnect_before_connect(): @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) + config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30) camera = RealSenseCamera(config) camera.connect(warmup=False) @@ -148,7 +148,7 @@ def test_async_read(mock_enable_device): @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) + config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30) camera = RealSenseCamera(config) camera.connect(warmup=False) @@ -163,7 +163,7 @@ def test_async_read_timeout(mock_enable_device): 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): @@ -182,7 +182,7 @@ def test_async_read_before_connect(): ) @patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) def test_rotation(mock_enable_device, rotation): - config = RealSenseCameraConfig(serial_number=42, rotation=rotation) + config = RealSenseCameraConfig(serial_number_or_name=42, rotation=rotation) camera = RealSenseCamera(config) camera.connect(warmup=False) From 7a4b9f0cbfc9a0311098237cc56d850438b47b35 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 18:09:22 +0200 Subject: [PATCH 27/43] chore(cameras): rename prerotated_ to capture_ var names --- .../common/cameras/opencv/camera_opencv.py | 28 ++++++++-------- .../cameras/realsense/camera_realsense.py | 32 +++++++++---------- 2 files changed, 30 insertions(+), 30 deletions(-) diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index eda86eb1..9951cfb1 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -125,9 +125,9 @@ class OpenCVCamera(Camera): 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: return f"{self.__class__.__name__}({self.index_or_path})" @@ -208,10 +208,10 @@ class OpenCVCamera(Camera): 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.prerotated_width, self.prerotated_height = default_width, default_height + self.capture_width, self.capture_height = default_width, default_height else: self.width, self.height = default_width, default_height - self.prerotated_width, self.prerotated_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: @@ -236,25 +236,25 @@ class OpenCVCamera(Camera): def _validate_width_and_height(self) -> None: """Validates and sets the camera's frame capture width and height.""" - success = self.videocapture.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.prerotated_width)) + 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.prerotated_width != actual_width: + if not success or self.capture_width != actual_width: logger.warning( - f"Requested capture width {self.prerotated_width} for {self}, but camera reported {actual_width} (set success: {success})." + f"Requested capture width {self.capture_width} for {self}, but camera reported {actual_width} (set success: {success})." ) 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}.") - success = self.videocapture.set(cv2.CAP_PROP_FRAME_HEIGHT, float(self.prerotated_height)) + 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.prerotated_height != actual_height: + if not success or self.capture_height != actual_height: logger.warning( - f"Requested capture height {self.prerotated_height} for {self}, but camera reported {actual_height} (set success: {success})." + f"Requested capture height {self.capture_height} for {self}, but camera reported {actual_height} (set success: {success})." ) 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}.") @@ -387,9 +387,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}." + 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 diff --git a/lerobot/common/cameras/realsense/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py index c39e3469..e97dd561 100644 --- a/lerobot/common/cameras/realsense/camera_realsense.py +++ b/lerobot/common/cameras/realsense/camera_realsense.py @@ -137,9 +137,9 @@ class RealSenseCamera(Camera): 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: return f"{self.__class__.__name__}({self.serial_number})" @@ -269,17 +269,17 @@ class RealSenseCamera(Camera): 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}") @@ -319,10 +319,10 @@ class RealSenseCamera(Camera): 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 + self.capture_width, self.capture_height = actual_width, actual_height else: self.width, self.height = actual_width, actual_height - self.prerotated_width, self.prerotated_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: @@ -349,21 +349,21 @@ class RealSenseCamera(Camera): actual_width = int(round(stream.width())) actual_height = int(round(stream.height())) - if self.prerotated_width != actual_width: + if self.capture_width != actual_width: logger.warning( - f"Requested capture width {self.prerotated_width} for {self}, but camera reported {actual_width}." + f"Requested capture width {self.capture_width} for {self}, but camera reported {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: + if self.capture_height != actual_height: logger.warning( - f"Requested capture height {self.prerotated_height} for {self}, but camera reported {actual_height}." + f"Requested capture height {self.capture_height} for {self}, but camera reported {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}.") @@ -485,9 +485,9 @@ class RealSenseCamera(Camera): 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}." + 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 From eb2d967c85b996784e83bc48b5b9487c8b2f70d1 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 18:18:13 +0200 Subject: [PATCH 28/43] docs(cameras): add explanation of warmup arg in connect --- lerobot/common/cameras/camera.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/lerobot/common/cameras/camera.py b/lerobot/common/cameras/camera.py index c06439c2..291efe2e 100644 --- a/lerobot/common/cameras/camera.py +++ b/lerobot/common/cameras/camera.py @@ -71,7 +71,9 @@ class Camera(abc.ABC): """Establish connection to the camera. Args: - warmup: If True (default), captures a warmup frame before returning. + 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 From a65321017358b2a94840ea985239ac2abdf7d0b6 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 18:31:55 +0200 Subject: [PATCH 29/43] chore(cameras): move _stop_thread under _start_thread methods --- .../common/cameras/opencv/camera_opencv.py | 34 ++++++++-------- .../cameras/realsense/camera_realsense.py | 40 +++++++++---------- 2 files changed, 37 insertions(+), 37 deletions(-) diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index 9951cfb1..101940c0 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -444,6 +444,23 @@ 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: + 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 async_read(self, timeout_ms: float = 2000) -> np.ndarray: """ Reads the latest available frame asynchronously. @@ -488,23 +505,6 @@ class OpenCVCamera(Camera): 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 _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: - 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. diff --git a/lerobot/common/cameras/realsense/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py index e97dd561..d791a7e6 100644 --- a/lerobot/common/cameras/realsense/camera_realsense.py +++ b/lerobot/common/cameras/realsense/camera_realsense.py @@ -308,15 +308,15 @@ class RealSenseCamera(Camera): raise DeviceNotConnectedError(f"Cannot validate settings for {self} as it is not connected.") stream = self.rs_profile.get_stream(rs.stream.color).as_video_stream_profile() + if self.fps is None: self.fps = stream.fps() else: self._validate_fps(stream) - actual_width = int(round(stream.width())) - actual_height = int(round(stream.height())) - 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 @@ -540,6 +540,23 @@ class RealSenseCamera(Camera): 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: + 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 + # NOTE(Steven): Missing implementation for depth for now def async_read(self, timeout_ms: float = 100) -> np.ndarray: """ @@ -587,23 +604,6 @@ class RealSenseCamera(Camera): f"Error getting frame data from queue for camera {self.serial_number}: {e}" ) from e - 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: - 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, stops the pipeline, and cleans up resources. From 91a5fafdaebcf6550a53378f1e16800d637a319f Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 18:40:20 +0200 Subject: [PATCH 30/43] feat(cameras): add check for fps, height and width in realsenseconfig (all or none) --- .../cameras/realsense/configuration_realsense.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/lerobot/common/cameras/realsense/configuration_realsense.py b/lerobot/common/cameras/realsense/configuration_realsense.py index ed1f76e7..64c7063e 100644 --- a/lerobot/common/cameras/realsense/configuration_realsense.py +++ b/lerobot/common/cameras/realsense/configuration_realsense.py @@ -40,16 +40,16 @@ 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. - serial_number_or_name: unique serial number or human-readable name to identify the camera. + 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. 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. """ serial_number_or_name: int | str @@ -72,3 +72,9 @@ class RealSenseCameraConfig(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." ) + + 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( + "For `fps`, `width` and `height`, either all of them need to be set, or none of them." + ) From 33b85590607e2ee39d43f83bf3d5b705ea8433cf Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 19:01:13 +0200 Subject: [PATCH 31/43] refactor(cameras): move find_cameras to the base class --- lerobot/common/cameras/camera.py | 11 +++++++++++ lerobot/common/cameras/opencv/camera_opencv.py | 11 ++++------- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/lerobot/common/cameras/camera.py b/lerobot/common/cameras/camera.py index 291efe2e..1937205b 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 @@ -66,6 +67,16 @@ class Camera(abc.ABC): """ 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, warmup: bool = True) -> None: """Establish connection to the camera. diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index 101940c0..48d3adad 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -259,15 +259,12 @@ class OpenCVCamera(Camera): logger.debug(f"Capture height set to {actual_height} for {self}.") @staticmethod - def find_cameras(max_index_search_range=MAX_OPENCV_INDEX) -> 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. + it checks indices from 0 up to `MAX_OPENCV_INDEX`. Returns: List[Dict[str, Any]]: A list of dictionaries, @@ -283,9 +280,9 @@ class OpenCVCamera(Camera): 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}..." + f"{platform.system()} system detected. Scanning indices from 0 to {MAX_OPENCV_INDEX}..." ) - targets_to_scan = list(range(max_index_search_range)) + targets_to_scan = list(range(MAX_OPENCV_INDEX)) for target in targets_to_scan: camera = cv2.VideoCapture(target) From a36536dc078cf7f04ccb2b302cb458506d3634af Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 19:10:06 +0200 Subject: [PATCH 32/43] Apply suggestions from code review 3 Co-authored-by: Simon Alibert <75076266+aliberts@users.noreply.github.com> --- lerobot/common/robots/config.py | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/lerobot/common/robots/config.py b/lerobot/common/robots/config.py index 31fd0bf5..dc933f33 100644 --- a/lerobot/common/robots/config.py +++ b/lerobot/common/robots/config.py @@ -27,14 +27,12 @@ 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 name, 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 From 1c90cd12698db6a0b6733f916d6b2c1a7e969ce6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 20 May 2025 17:10:37 +0000 Subject: [PATCH 33/43] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- lerobot/common/robots/config.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lerobot/common/robots/config.py b/lerobot/common/robots/config.py index dc933f33..d95e543e 100644 --- a/lerobot/common/robots/config.py +++ b/lerobot/common/robots/config.py @@ -33,7 +33,7 @@ class RobotConfig(draccus.ChoiceRegistry, abc.ABC): 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: From 76f661f8b5f4f1326fb8f4c7e11da9d5b1d64186 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 20 May 2025 19:15:01 +0200 Subject: [PATCH 34/43] chore(robots): fix pre-commit --- lerobot/common/robots/config.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lerobot/common/robots/config.py b/lerobot/common/robots/config.py index d95e543e..a85a8316 100644 --- a/lerobot/common/robots/config.py +++ b/lerobot/common/robots/config.py @@ -28,7 +28,7 @@ class RobotConfig(draccus.ChoiceRegistry, abc.ABC): def __post_init__(self): if hasattr(self, "cameras") and self.cameras: - for name, config in self.cameras.items(): + for _, config in self.cameras.items(): for attr in ["width", "height", "fps"]: if getattr(config, attr) is None: raise ValueError( From 7f7c4310613358dab86b90f2bf4b9c99b1a6a2a6 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 21 May 2025 13:29:30 +0200 Subject: [PATCH 35/43] docs(cameras): update find_cameras instructions --- lerobot/common/cameras/__init__.py | 2 +- lerobot/common/cameras/opencv/camera_opencv.py | 8 ++++---- lerobot/common/cameras/realsense/camera_realsense.py | 11 +++++------ lerobot/find_cameras.py | 2 +- 4 files changed, 11 insertions(+), 12 deletions(-) diff --git a/lerobot/common/cameras/__init__.py b/lerobot/common/cameras/__init__.py index 917bb789..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, ColorMode +from .configs import CameraConfig, ColorMode, Cv2Rotation from .utils import make_cameras_from_configs diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index 48d3adad..614c6675 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -58,7 +58,7 @@ 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 @@ -67,7 +67,7 @@ class OpenCVCamera(Camera): 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) @@ -92,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 ... @@ -164,7 +164,7 @@ class OpenCVCamera(Camera): self.videocapture = None raise ConnectionError( f"Failed to open OpenCV camera {self.index_or_path}." - f"Run 'python -m find_cameras Run 'python -m find_cameras' for details about the available cameras in your system." + f"Run 'python -m lerobot.find_cameras opencv' for details about the available cameras in your system." ) self._configure_capture_settings() diff --git a/lerobot/common/cameras/realsense/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py index d791a7e6..9e89cd64 100644 --- a/lerobot/common/cameras/realsense/camera_realsense.py +++ b/lerobot/common/cameras/realsense/camera_realsense.py @@ -50,7 +50,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 @@ -63,7 +63,7 @@ class RealSenseCamera(Camera): Example: ```python from lerobot.common.cameras.realsense import RealSenseCamera, RealSenseCameraConfig - from lerobot.common.cameras import ColorMode + from lerobot.common.cameras import ColorMode, Cv2Rotation # Basic usage with serial number config = RealSenseCameraConfig(serial_number_or_name="1234567890") # Replace with actual SN @@ -87,7 +87,7 @@ class RealSenseCamera(Camera): 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) @@ -175,7 +175,7 @@ class RealSenseCamera(Camera): self.rs_profile = None self.rs_pipeline = None raise ConnectionError( - f"Failed to open {self} camera. Run 'python -m find_cameras' for details about the available cameras in your system." + 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}...") @@ -388,10 +388,9 @@ 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() diff --git a/lerobot/find_cameras.py b/lerobot/find_cameras.py index 1d051d69..5cff0fe5 100644 --- a/lerobot/find_cameras.py +++ b/lerobot/find_cameras.py @@ -179,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) From f8c7e59f83999c18967e1d7792966d74c3742fcf Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 21 May 2025 13:29:53 +0200 Subject: [PATCH 36/43] test(cameras): use fixture for realsense patching + depth read test --- tests/cameras/test_realsense.py | 52 +++++++++++++++++++-------------- 1 file changed, 30 insertions(+), 22 deletions(-) diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index 3ca9db97..3db2a627 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -36,27 +36,33 @@ except (ImportError, ModuleNotFoundError): 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? -def mock_rs_config_enable_device_from_file(rs_config_instance, sn): +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) +@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): +def test_connect(): config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) @@ -64,8 +70,7 @@ def test_connect(mock_enable_device): 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): +def test_connect_already_connected(): config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) camera.connect(warmup=False) @@ -74,8 +79,8 @@ def test_connect_already_connected(mock_enable_device): 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): +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) @@ -83,8 +88,7 @@ def test_connect_invalid_camera_path(mock_enable_device): 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): +def test_invalid_width_connect(): config = RealSenseCameraConfig(serial_number_or_name=42, width=99999, height=480, fps=30) camera = RealSenseCamera(config) @@ -92,8 +96,7 @@ def test_invalid_width_connect(mock_enable_device): camera.connect(warmup=False) -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_read(mock_enable_device): +def test_read(): config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30) camera = RealSenseCamera(config) camera.connect(warmup=False) @@ -102,6 +105,15 @@ def test_read(mock_enable_device): 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=500) # NOTE(Steven): Reading depth takes longer + assert isinstance(img, np.ndarray) + + def test_read_before_connect(): config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) @@ -110,8 +122,7 @@ def test_read_before_connect(): _ = camera.read() -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_disconnect(mock_enable_device): +def test_disconnect(): config = RealSenseCameraConfig(serial_number_or_name=42) camera = RealSenseCamera(config) camera.connect(warmup=False) @@ -129,8 +140,7 @@ def test_disconnect_before_connect(): camera.disconnect() -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_async_read(mock_enable_device): +def test_async_read(): config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30) camera = RealSenseCamera(config) camera.connect(warmup=False) @@ -146,8 +156,7 @@ 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): +def test_async_read_timeout(): config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30) camera = RealSenseCamera(config) camera.connect(warmup=False) @@ -180,8 +189,7 @@ def test_async_read_before_connect(): ], ids=["no_rot", "rot90", "rot180", "rot270"], ) -@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) -def test_rotation(mock_enable_device, rotation): +def test_rotation(rotation): config = RealSenseCameraConfig(serial_number_or_name=42, rotation=rotation) camera = RealSenseCamera(config) camera.connect(warmup=False) From 73bb9709a7a6d4fc9ae983d7cc1bf6efea44199c Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 21 May 2025 13:50:52 +0200 Subject: [PATCH 37/43] chore(cameras): remove 14 logs --- .../common/cameras/opencv/camera_opencv.py | 32 ++++--------------- .../cameras/realsense/camera_realsense.py | 21 ++---------- 2 files changed, 9 insertions(+), 44 deletions(-) diff --git a/lerobot/common/cameras/opencv/camera_opencv.py b/lerobot/common/cameras/opencv/camera_opencv.py index 614c6675..d4ba26cd 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -224,12 +224,8 @@ class OpenCVCamera(Camera): 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}.") @@ -239,22 +235,16 @@ class OpenCVCamera(Camera): 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: - logger.warning( - f"Requested capture width {self.capture_width} for {self}, but camera reported {actual_width} (set success: {success})." - ) raise RuntimeError( - f"Failed to set requested capture width {self.capture_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.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: - logger.warning( - f"Requested capture height {self.capture_height} for {self}, but camera reported {actual_height} (set success: {success})." - ) raise RuntimeError( - f"Failed to set requested capture height {self.capture_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}.") @@ -274,16 +264,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_OPENCV_INDEX}..." - ) 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(): @@ -305,7 +291,6 @@ class OpenCVCamera(Camera): } found_cameras_info.append(camera_info) - logger.debug(f"Found OpenCV camera:: {camera_info}") camera.release() if not found_cameras_info: @@ -444,11 +429,9 @@ class OpenCVCamera(Camera): 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: - 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.") @@ -457,6 +440,7 @@ class OpenCVCamera(Camera): 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: """ @@ -490,12 +474,8 @@ class OpenCVCamera(Camera): 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: diff --git a/lerobot/common/cameras/realsense/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py index 9e89cd64..dda55301 100644 --- a/lerobot/common/cameras/realsense/camera_realsense.py +++ b/lerobot/common/cameras/realsense/camera_realsense.py @@ -235,7 +235,6 @@ 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 @@ -350,18 +349,12 @@ class RealSenseCamera(Camera): actual_height = int(round(stream.height())) if self.capture_width != actual_width: - logger.warning( - f"Requested capture width {self.capture_width} for {self}, but camera reported {actual_width}." - ) raise RuntimeError( 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.capture_height != actual_height: - logger.warning( - f"Requested capture height {self.capture_height} for {self}, but camera reported {actual_height}." - ) raise RuntimeError( f"Failed to set requested capture height {self.capture_height} for {self}. Actual value: {actual_height}." ) @@ -542,11 +535,9 @@ class RealSenseCamera(Camera): 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: - 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.") @@ -555,6 +546,7 @@ class RealSenseCamera(Camera): 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 = 100) -> np.ndarray: @@ -589,19 +581,12 @@ class RealSenseCamera(Camera): 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 + raise RuntimeError(f"Error getting frame data from queue for camera {self}: {e}") from e def disconnect(self): """ From 625bb9c9d80cb79550361297ecf5e6316b60f714 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 21 May 2025 15:09:55 +0200 Subject: [PATCH 38/43] chore(cameras): try import rs protect --- lerobot/common/cameras/realsense/camera_realsense.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/lerobot/common/cameras/realsense/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py index dda55301..2733315d 100644 --- a/lerobot/common/cameras/realsense/camera_realsense.py +++ b/lerobot/common/cameras/realsense/camera_realsense.py @@ -26,7 +26,11 @@ 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 95f3e53eba65b5a85044c4f5454dbb742a29cd4f Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 21 May 2025 15:16:20 +0200 Subject: [PATCH 39/43] chore(test): try import rs protect --- tests/cameras/test_realsense.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index 3db2a627..91882991 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -30,7 +30,7 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte try: from lerobot.common.cameras.realsense import RealSenseCamera, RealSenseCameraConfig -except (ImportError, ModuleNotFoundError): +except (ImportError, ModuleNotFoundError, NameError): pytest.skip("pyrealsense2 not available", allow_module_level=True) TEST_ARTIFACTS_DIR = Path(__file__).parent.parent / "artifacts" / "cameras" From 47d5008407af46a06cf9bfa69b0c7c7471e3ace8 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 21 May 2025 15:41:52 +0200 Subject: [PATCH 40/43] chore(cameras): address unresolved conversations --- lerobot/common/cameras/camera.py | 2 ++ lerobot/common/cameras/configs.py | 5 ++-- .../common/cameras/opencv/camera_opencv.py | 16 ++++++++--- .../cameras/realsense/camera_realsense.py | 27 +++++++------------ lerobot/common/cameras/utils.py | 2 +- tests/cameras/test_opencv.py | 4 ++- tests/cameras/test_realsense.py | 2 +- 7 files changed, 32 insertions(+), 26 deletions(-) diff --git a/lerobot/common/cameras/camera.py b/lerobot/common/cameras/camera.py index 1937205b..0ef85340 100644 --- a/lerobot/common/cameras/camera.py +++ b/lerobot/common/cameras/camera.py @@ -36,6 +36,7 @@ class Camera(abc.ABC): 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): @@ -55,6 +56,7 @@ class Camera(abc.ABC): 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 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 d4ba26cd..fd252bdc 100644 --- a/lerobot/common/cameras/opencv/camera_opencv.py +++ b/lerobot/common/cameras/opencv/camera_opencv.py @@ -113,6 +113,7 @@ class OpenCVCamera(Camera): self.fps = config.fps self.color_mode = config.color_mode + self.warmup_time = config.warmup_time self.videocapture: cv2.VideoCapture | None = None @@ -121,7 +122,7 @@ class OpenCVCamera(Camera): self.frame_queue: queue.Queue = queue.Queue(maxsize=1) 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]: @@ -157,7 +158,7 @@ class OpenCVCamera(Camera): # blocking in multi-threaded applications, especially during data collection. cv2.setNumThreads(1) - self.videocapture = cv2.VideoCapture(self.index_or_path) + self.videocapture = cv2.VideoCapture(self.index_or_path, self.backend) if not self.videocapture.isOpened(): self.videocapture.release() @@ -170,8 +171,15 @@ class OpenCVCamera(Camera): self._configure_capture_settings() if warmup: - 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 + 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.") diff --git a/lerobot/common/cameras/realsense/camera_realsense.py b/lerobot/common/cameras/realsense/camera_realsense.py index 2733315d..d06eb3dd 100644 --- a/lerobot/common/cameras/realsense/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 @@ -129,6 +128,7 @@ class RealSenseCamera(Camera): 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 @@ -186,8 +186,15 @@ class RealSenseCamera(Camera): self._validate_capture_settings() if warmup: - logger.debug(f"Reading a warm-up frame for {self}...") - self.read() # NOTE(Steven): For now we just read one frame, we could also loop for X frames/secs + 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.") @@ -314,8 +321,6 @@ class RealSenseCamera(Camera): if self.fps is None: self.fps = stream.fps() - else: - self._validate_fps(stream) if self.width is None or self.height is None: actual_width = int(round(stream.width())) @@ -333,20 +338,8 @@ class RealSenseCamera(Camera): if self.use_depth: stream = self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile() - self._validate_fps(stream) self._validate_width_and_height(stream) - def _validate_fps(self, stream: rs.video_stream_profile) -> None: - """Validates and sets the internal FPS based on actual stream FPS.""" - actual_fps = stream.fps() - - # Use math.isclose for robust float comparison - if not math.isclose(self.fps, actual_fps, rel_tol=1e-3): - 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}.") - 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())) diff --git a/lerobot/common/cameras/utils.py b/lerobot/common/cameras/utils.py index 809fe085..d95703da 100644 --- a/lerobot/common/cameras/utils.py +++ b/lerobot/common/cameras/utils.py @@ -62,7 +62,7 @@ def get_cv2_backend() -> int: import cv2 available_cv2_backends = { - "Linux": cv2.CAP_DSHOW, + "Linux": cv2.CAP_V4L2, "Windows": cv2.CAP_AVFOUNDATION, "Darwin": cv2.CAP_ANY, } diff --git a/tests/cameras/test_opencv.py b/tests/cameras/test_opencv.py index 56ba3bb2..dda2a4fb 100644 --- a/tests/cameras/test_opencv.py +++ b/tests/cameras/test_opencv.py @@ -145,7 +145,9 @@ def test_async_read_timeout(): 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() diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index 91882991..e0662fb2 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -110,7 +110,7 @@ def test_read_depth(): camera = RealSenseCamera(config) camera.connect(warmup=False) - img = camera.read_depth(timeout_ms=500) # NOTE(Steven): Reading depth takes longer + img = camera.read_depth(timeout_ms=1000) # NOTE(Steven): Reading depth takes longer assert isinstance(img, np.ndarray) From 4d9825bb1d6c8965a3685ded0e5b016a4d7b9c32 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 21 May 2025 16:06:16 +0200 Subject: [PATCH 41/43] fix(ci): change default opencv backend --- lerobot/common/cameras/utils.py | 28 +++++++++++++--------------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/lerobot/common/cameras/utils.py b/lerobot/common/cameras/utils.py index d95703da..72f1dd3e 100644 --- a/lerobot/common/cameras/utils.py +++ b/lerobot/common/cameras/utils.py @@ -46,28 +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 - available_rotation_dict = { - Cv2Rotation.ROTATE_270: cv2.ROTATE_90_COUNTERCLOCKWISE, - Cv2Rotation.ROTATE_90: cv2.ROTATE_90_CLOCKWISE, - Cv2Rotation.ROTATE_180: cv2.ROTATE_180, - } - - return available_rotation_dict.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 - available_cv2_backends = { - "Linux": cv2.CAP_V4L2, - "Windows": cv2.CAP_AVFOUNDATION, - "Darwin": cv2.CAP_ANY, - } - - return available_cv2_backends.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): From 0c411a68329caf950a731569ed357b4fee06d72a Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 21 May 2025 16:12:40 +0200 Subject: [PATCH 42/43] [skip ci] doc(tests): add comment related to test duration --- tests/cameras/test_realsense.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index e0662fb2..4fd57537 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -36,8 +36,7 @@ except (ImportError, ModuleNotFoundError, NameError): TEST_ARTIFACTS_DIR = Path(__file__).parent.parent / "artifacts" / "cameras" BAG_FILE_PATH = TEST_ARTIFACTS_DIR / "test_rs.bag" -# NOTE(Steven): Takes 20sec, the patch being the biggest bottleneck - +# 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): return rs_config_instance.enable_device_from_file(str(BAG_FILE_PATH), repeat_playback=True) From 65ad0650b03512aed3831d05fc1381f52d201619 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 21 May 2025 14:13:05 +0000 Subject: [PATCH 43/43] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- tests/cameras/test_realsense.py | 1 + 1 file changed, 1 insertion(+) diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index 4fd57537..8e582cd5 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -38,6 +38,7 @@ BAG_FILE_PATH = TEST_ARTIFACTS_DIR / "test_rs.bag" # 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): return rs_config_instance.enable_device_from_file(str(BAG_FILE_PATH), repeat_playback=True)