tangger/lerobot
1
0
Fork 1
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

Merge branch 'user/aliberts/2025_02_25_refactor_robots' into refactor/updated_api_docs

Browse Source
This commit is contained in:
Pepijn
2025-05-15 12:46:56 +02:00
committed by Steven Palma
parent 859a369b29 65ad0650b0
commit 3c1b657fdd
39 changed files with 780 additions and 967 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
.cache/calibration/aloha_default/left_follower.json
View File

@@ -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"
]
}

68
.cache/calibration/aloha_default/left_leader.json
View File

@@ -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"
]
}

68
.cache/calibration/aloha_default/right_follower.json
View File

@@ -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"
]
}

68
.cache/calibration/aloha_default/right_leader.json
View File

@@ -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"
]
}

2
.gitattributes vendored
View File

@@ -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

7
.gitignore vendored
View File

@@ -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

2
lerobot/calibrate.py
View File

@@ -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,

2
lerobot/common/cameras/__init__.py
View File

@@ -13,5 +13,5 @@
# limitations under the License.
from .camera import Camera
from .configs import CameraConfig
from .configs import CameraConfig, ColorMode, Cv2Rotation
from .utils import make_cameras_from_configs

77
lerobot/common/cameras/camera.py
View File

@@ -15,6 +15,7 @@
# limitations under the License.
import abc
from typing import Any, Dict, List
import numpy as np
@@ -22,28 +23,100 @@ from .configs import CameraConfig, ColorMode
class Camera(abc.ABC):
"""Base class for camera implementations.
Defines a standard interface for camera operations across different backends.
Subclasses must implement all abstract methods.
Manages basic camera properties (FPS, resolution) and core operations:
- Connection/disconnection
- Frame capture (sync/async)
Attributes:
fps (int | None): Configured frames per second
width (int | None): Frame width in pixels
height (int | None): Frame height in pixels
warmup_time (int | None): Time reading frames before returning from connect (in seconds)
Example:
class MyCamera(Camera):
def __init__(self, config): ...
@property
def is_connected(self) -> bool: ...
def connect(self, warmup=True): ...
# Plus other required methods
"""
def __init__(self, config: CameraConfig):
"""Initialize the camera with the given configuration.
Args:
config: Camera configuration containing FPS and resolution.
"""
self.fps: int | None = config.fps
self.width: int | None = config.width
self.height: int | None = config.height
self.warmup_time: int | None = config.warmup_time
@property
@abc.abstractmethod
def is_connected(self) -> bool:
"""Check if the camera is currently connected.
Returns:
bool: True if the camera is connected and ready to capture frames,
False otherwise.
"""
pass
@staticmethod
@abc.abstractmethod
def find_cameras() -> List[Dict[str, Any]]:
"""Detects available cameras connected to the system.
Returns:
List[Dict[str, Any]]: A list of dictionaries,
where each dictionary contains information about a detected camera.
"""
pass
@abc.abstractmethod
def connect(self, do_warmup_read: bool = True) -> None:
def connect(self, warmup: bool = True) -> None:
"""Establish connection to the camera.
Args:
warmup: If True (default), captures a warmup frame before returning. Useful
for cameras that require time to adjust capture settings.
If False, skips the warmup frame.
"""
pass
@abc.abstractmethod
def read(self, color_mode: ColorMode | None = None) -> np.ndarray:
"""Capture and return a single frame from the camera.
Args:
color_mode: Desired color mode for the output frame. If None,
uses the camera's default color mode.
Returns:
np.ndarray: Captured frame as a numpy array.
"""
pass
@abc.abstractmethod
def async_read(self, timeout_ms: float = 2000) -> np.ndarray:
def async_read(self, timeout_ms: float = ...) -> np.ndarray:
"""Asynchronously capture and return a single frame from the camera.
Args:
timeout_ms: Maximum time to wait for a frame in milliseconds.
Defaults to implementation-specific timeout.
Returns:
np.ndarray: Captured frame as a numpy array.
"""
pass
@abc.abstractmethod
def disconnect(self) -> None:
"""Disconnect from the camera and release resources."""
pass

5
lerobot/common/cameras/configs.py
View File

@@ -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:

278
lerobot/common/cameras/opencv/camera_opencv.py
View File

@@ -30,10 +30,9 @@ import cv2
import numpy as np
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
from lerobot.common.utils.utils import capture_timestamp_utc
from ..camera import Camera
from ..utils import IndexOrPath, get_cv2_backend, get_cv2_rotation
from ..utils import get_cv2_backend, get_cv2_rotation
from .configuration_opencv import ColorMode, OpenCVCameraConfig
# NOTE(Steven): The maximum opencv device index depends on your operating system. For instance,
@@ -59,20 +58,16 @@ class OpenCVCamera(Camera):
or port changes, especially on Linux. Use the provided utility script to find
available camera indices or paths:
```bash
python -m lerobot.find_cameras
python -m lerobot.find_cameras opencv
```
The camera's default settings (FPS, resolution, color mode) are used unless
overridden in the configuration.
Args:
config (OpenCVCameraConfig): Configuration object containing settings like
camera index/path, desired FPS, width, height, color mode, and rotation.
Example:
```python
from lerobot.common.cameras.opencv import OpenCVCamera
from lerobot.common.cameras.configuration_opencv import OpenCVCameraConfig, ColorMode
from lerobot.common.cameras.configuration_opencv import OpenCVCameraConfig, ColorMode, Cv2Rotation
# Basic usage with camera index 0
config = OpenCVCameraConfig(index_or_path=0)
@@ -97,7 +92,7 @@ class OpenCVCamera(Camera):
width=1280,
height=720,
color_mode=ColorMode.RGB,
rotation=90
rotation=Cv2Rotation.ROTATE_90
)
custom_camera = OpenCVCamera(custom_config)
# ... connect, read, disconnect ...
@@ -114,37 +109,79 @@ class OpenCVCamera(Camera):
super().__init__(config)
self.config = config
self.index_or_path: IndexOrPath = config.index_or_path
self.index_or_path = config.index_or_path
self.fps: int | None = config.fps
self.channels: int = config.channels
self.color_mode: ColorMode = config.color_mode
self.fps = config.fps
self.color_mode = config.color_mode
self.warmup_time = config.warmup_time
self.videocapture_camera: cv2.VideoCapture | None = None
self.videocapture: cv2.VideoCapture | None = None
self.thread: Thread | None = None
self.stop_event: Event | None = None
self.frame_queue: queue.Queue = queue.Queue(maxsize=1)
self.logs: dict = {} # NOTE(Steven): Might be removed in the future
self.rotation: int | None = get_cv2_rotation(config.rotation)
self.backend: int = get_cv2_backend() # NOTE(Steven): If we specify backend the opencv open fails
self.backend: int = get_cv2_backend()
if self.height and self.width:
if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
self.prerotated_width, self.prerotated_height = self.height, self.width
self.capture_width, self.capture_height = self.height, self.width
else:
self.prerotated_width, self.prerotated_height = self.width, self.height
self.capture_width, self.capture_height = self.width, self.height
def __str__(self) -> str:
"""Returns a string representation of the camera instance."""
return f"{self.__class__.__name__}({self.index_or_path})"
@property
def is_connected(self) -> bool:
"""Checks if the camera is currently connected and opened."""
return isinstance(self.videocapture_camera, cv2.VideoCapture) and self.videocapture_camera.isOpened()
return isinstance(self.videocapture, cv2.VideoCapture) and self.videocapture.isOpened()
def connect(self, warmup: bool = True):
"""
Connects to the OpenCV camera specified in the configuration.
Initializes the OpenCV VideoCapture object, sets desired camera properties
(FPS, width, height), and performs initial checks.
Raises:
DeviceAlreadyConnectedError: If the camera is already connected.
ValueError: If the specified camera index/path is not found or accessible.
ConnectionError: If the camera is found but fails to open.
RuntimeError: If the camera opens but fails to apply requested FPS/resolution settings.
"""
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} is already connected.")
# Use 1 thread for OpenCV operations to avoid potential conflicts or
# blocking in multi-threaded applications, especially during data collection.
cv2.setNumThreads(1)
self.videocapture = cv2.VideoCapture(self.index_or_path, self.backend)
if not self.videocapture.isOpened():
self.videocapture.release()
self.videocapture = None
raise ConnectionError(
f"Failed to open OpenCV camera {self.index_or_path}."
f"Run 'python -m lerobot.find_cameras opencv' for details about the available cameras in your system."
)
self._configure_capture_settings()
if warmup:
if self.warmup_time is None:
raise ValueError(
f"Warmup time is not set for {self}. Please set a warmup time in the configuration."
)
logger.debug(f"Reading a warm-up frames for {self} for {self.warmup_time} seconds...")
start_time = time.time()
while time.time() - start_time < self.warmup_time:
self.read()
time.sleep(0.1)
logger.debug(f"Camera {self.index_or_path} connected and configured successfully.")
def _configure_capture_settings(self) -> None:
"""
@@ -167,120 +204,65 @@ class OpenCVCamera(Camera):
if not self.is_connected:
raise DeviceNotConnectedError(f"Cannot configure settings for {self} as it is not connected.")
self._validate_fps()
self._validate_width_and_height()
if self.fps is None:
self.fps = self.videocapture.get(cv2.CAP_PROP_FPS)
logger.info(f"FPS set to camera default: {self.fps}.")
else:
self._validate_fps()
def connect(self, do_warmup_read: bool = True):
"""
Connects to the OpenCV camera specified in the configuration.
default_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH)))
default_height = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_HEIGHT)))
Initializes the OpenCV VideoCapture object, sets desired camera properties
(FPS, width, height), and performs initial checks.
Raises:
DeviceAlreadyConnectedError: If the camera is already connected.
ValueError: If the specified camera index/path is not found or accessible.
ConnectionError: If the camera is found but fails to open.
RuntimeError: If the camera opens but fails to apply requested FPS/resolution settings.
"""
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} is already connected.")
# Use 1 thread for OpenCV operations to avoid potential conflicts or
# blocking in multi-threaded applications, especially during data collection.
cv2.setNumThreads(1)
logger.debug(f"Attempting to connect to camera {self.index_or_path} using backend {self.backend}...")
self.videocapture_camera = cv2.VideoCapture(self.index_or_path)
if not self.videocapture_camera.isOpened():
self.videocapture_camera.release()
self.videocapture_camera = None
raise ConnectionError(
f"Failed to open OpenCV camera {self.index_or_path}."
f"Run 'python -m find_cameras list-cameras' for details."
)
logger.debug(f"Successfully opened camera {self.index_or_path}. Applying configuration...")
self._configure_capture_settings()
if do_warmup_read:
logger.debug(f"Reading a warm-up frame for {self.index_or_path}...")
self.read() # NOTE(Steven): For now we just read one frame, we could also loop for X frames/secs
logger.debug(f"Camera {self.index_or_path} connected and configured successfully.")
if self.width is None or self.height is None:
if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
self.width, self.height = default_height, default_width
self.capture_width, self.capture_height = default_width, default_height
else:
self.width, self.height = default_width, default_height
self.capture_width, self.capture_height = default_width, default_height
logger.info(f"Capture width set to camera default: {self.width}.")
logger.info(f"Capture height set to camera default: {self.height}.")
else:
self._validate_width_and_height()
def _validate_fps(self) -> None:
"""Validates and sets the camera's frames per second (FPS)."""
if self.fps is None:
self.fps = self.videocapture_camera.get(cv2.CAP_PROP_FPS)
logger.info(f"FPS set to camera default: {self.fps}.")
return
success = self.videocapture_camera.set(cv2.CAP_PROP_FPS, float(self.fps))
actual_fps = self.videocapture_camera.get(cv2.CAP_PROP_FPS)
success = self.videocapture.set(cv2.CAP_PROP_FPS, float(self.fps))
actual_fps = self.videocapture.get(cv2.CAP_PROP_FPS)
# Use math.isclose for robust float comparison
if not success or not math.isclose(self.fps, actual_fps, rel_tol=1e-3):
logger.warning(
f"Requested FPS {self.fps} for {self}, but camera reported {actual_fps} (set success: {success}). "
"This might be due to camera limitations."
)
raise RuntimeError(
f"Failed to set requested FPS {self.fps} for {self}. Actual value reported: {actual_fps}."
f"Failed to set requested FPS {self.fps} for {self}. Actual value reported: {actual_fps} set success: {success})."
)
logger.debug(f"FPS set to {actual_fps} for {self}.")
def _validate_width_and_height(self) -> None:
"""Validates and sets the camera's frame capture width and height."""
actual_width = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_WIDTH)))
actual_height = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_HEIGHT)))
if self.width is None or self.height is None:
if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
self.width, self.height = actual_height, actual_width
self.prerotated_width, self.prerotated_height = actual_width, actual_height
else:
self.width, self.height = actual_width, actual_height
self.prerotated_width, self.prerotated_height = actual_width, actual_height
logger.info(f"Capture width set to camera default: {self.width}.")
logger.info(f"Capture height set to camera default: {self.height}.")
return
success = self.videocapture_camera.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.prerotated_width))
if not success or self.prerotated_width != actual_width:
logger.warning(
f"Requested capture width {self.prerotated_width} for {self}, but camera reported {actual_width} (set success: {success})."
)
success = self.videocapture.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.capture_width))
actual_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH)))
if not success or self.capture_width != actual_width:
raise RuntimeError(
f"Failed to set requested capture width {self.prerotated_width} for {self}. Actual value: {actual_width}."
f"Failed to set requested capture width {self.capture_width} for {self}. Actual value: {actual_width} (set success: {success})."
)
logger.debug(f"Capture width set to {actual_width} for {self}.")
success = self.videocapture_camera.set(cv2.CAP_PROP_FRAME_HEIGHT, float(self.prerotated_height))
if not success or self.prerotated_height != actual_height:
logger.warning(
f"Requested capture height {self.prerotated_height} for {self}, but camera reported {actual_height} (set success: {success})."
)
success = self.videocapture.set(cv2.CAP_PROP_FRAME_HEIGHT, float(self.capture_height))
actual_height = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_HEIGHT)))
if not success or self.capture_height != actual_height:
raise RuntimeError(
f"Failed to set requested capture height {self.prerotated_height} for {self}. Actual value: {actual_height}."
f"Failed to set requested capture height {self.capture_height} for {self}. Actual value: {actual_height} (set success: {success})."
)
logger.debug(f"Capture height set to {actual_height} for {self}.")
@staticmethod
def find_cameras(
max_index_search_range=MAX_OPENCV_INDEX, raise_when_empty: bool = True
) -> List[Dict[str, Any]]:
def find_cameras() -> List[Dict[str, Any]]:
"""
Detects available OpenCV cameras connected to the system.
On Linux, it scans '/dev/video*' paths. On other systems (like macOS, Windows),
it checks indices from 0 up to `max_index_search_range`.
Args:
max_index_search_range (int): The maximum index to check on non-Linux systems.
raise_when_empty (bool): If True, raises an OSError if no cameras are found.
it checks indices from 0 up to `MAX_OPENCV_INDEX`.
Returns:
List[Dict[str, Any]]: A list of dictionaries,
@@ -290,16 +272,12 @@ class OpenCVCamera(Camera):
found_cameras_info = []
if platform.system() == "Linux":
logger.info("Linux detected. Scanning '/dev/video*' device paths...")
possible_paths = sorted(Path("/dev").glob("video*"), key=lambda p: p.name)
targets_to_scan = [str(p) for p in possible_paths]
logger.debug(f"Found potential paths: {targets_to_scan}")
else:
logger.info(
f"{platform.system()} system detected. Scanning indices from 0 to {max_index_search_range}..."
)
targets_to_scan = list(range(max_index_search_range))
targets_to_scan = list(range(MAX_OPENCV_INDEX))
logger.debug(f"Found potential paths: {targets_to_scan}")
for target in targets_to_scan:
camera = cv2.VideoCapture(target)
if camera.isOpened():
@@ -321,13 +299,10 @@ class OpenCVCamera(Camera):
}
found_cameras_info.append(camera_info)
logger.debug(f"Found OpenCV camera:: {camera_info}")
camera.release()
if not found_cameras_info:
logger.warning("No OpenCV devices detected.")
if raise_when_empty:
raise OSError("No OpenCV devices detected. Ensure cameras are connected.")
logger.info(f"Detected OpenCV cameras: {[cam['id'] for cam in found_cameras_info]}")
return found_cameras_info
@@ -361,7 +336,7 @@ class OpenCVCamera(Camera):
start_time = time.perf_counter()
# NOTE(Steven): Are we okay with this blocking an undefined amount of time?
ret, frame = self.videocapture_camera.read()
ret, frame = self.videocapture.read()
if not ret or frame is None:
raise RuntimeError(
@@ -374,7 +349,6 @@ class OpenCVCamera(Camera):
read_duration_ms = (time.perf_counter() - start_time) * 1e3
logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
self.logs["timestamp_utc"] = capture_timestamp_utc()
return processed_frame
def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray:
@@ -403,13 +377,9 @@ class OpenCVCamera(Camera):
h, w, c = image.shape
if h != self.prerotated_height or w != self.prerotated_width:
if h != self.capture_height or w != self.capture_width:
raise RuntimeError(
f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.prerotated_height}x{self.prerotated_width}) for {self}."
)
if c != self.channels:
logger.warning(
f"Captured frame channels ({c}) do not match configured channels ({self.channels}) for {self}."
f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.capture_height}x{self.capture_width}) for {self}."
)
processed_image = image
@@ -449,7 +419,7 @@ class OpenCVCamera(Camera):
logger.debug(f"Stopping read loop thread for {self}.")
def _ensure_read_thread_running(self):
def _start_read_thread(self) -> None:
"""Starts or restarts the background read thread if it's not running."""
if self.thread is not None and self.thread.is_alive():
self.thread.join(timeout=0.1)
@@ -464,6 +434,22 @@ class OpenCVCamera(Camera):
self.thread.start()
logger.debug(f"Read thread started for {self}.")
def _stop_read_thread(self) -> None:
"""Signals the background read thread to stop and waits for it to join."""
if self.stop_event is not None:
self.stop_event.set()
if self.thread is not None and self.thread.is_alive():
self.thread.join(timeout=2.0)
if self.thread.is_alive():
logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.")
else:
logger.debug(f"Read thread for {self} joined successfully.")
self.thread = None
self.stop_event = None
logger.debug(f"Read thread stopped for {self}.")
def async_read(self, timeout_ms: float = 2000) -> np.ndarray:
"""
Reads the latest available frame asynchronously.
@@ -490,41 +476,20 @@ class OpenCVCamera(Camera):
raise DeviceNotConnectedError(f"{self} is not connected.")
if self.thread is None or not self.thread.is_alive():
self._ensure_read_thread_running()
self._start_read_thread()
try:
return self.frame_queue.get(timeout=timeout_ms / 1000.0)
except queue.Empty as e:
thread_alive = self.thread is not None and self.thread.is_alive()
logger.error(
f"Timeout waiting for frame from {self} queue after {timeout_ms}ms. "
f"(Read thread alive: {thread_alive})"
)
raise TimeoutError(
f"Timed out waiting for frame from camera {self.index_or_path} after {timeout_ms} ms. "
f"Timed out waiting for frame from camera {self} after {timeout_ms} ms. "
f"Read thread alive: {thread_alive}."
) from e
except Exception as e:
logger.exception(f"Unexpected error getting frame from queue for {self}: {e}")
raise RuntimeError(f"Error getting frame from queue for camera {self.index_or_path}: {e}") from e
def _shutdown_read_thread(self):
"""Signals the background read thread to stop and waits for it to join."""
if self.stop_event is not None:
logger.debug(f"Signaling stop event for read thread of {self}.")
self.stop_event.set()
if self.thread is not None and self.thread.is_alive():
logger.debug(f"Waiting for read thread of {self} to join...")
self.thread.join(timeout=2.0)
if self.thread.is_alive():
logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.")
else:
logger.debug(f"Read thread for {self} joined successfully.")
self.thread = None
self.stop_event = None
def disconnect(self):
"""
Disconnects from the camera and cleans up resources.
@@ -536,18 +501,13 @@ class OpenCVCamera(Camera):
DeviceNotConnectedError: If the camera is already disconnected.
"""
if not self.is_connected and self.thread is None:
raise DeviceNotConnectedError(
f"Attempted to disconnect {self}, but it appears already disconnected."
)
logger.debug(f"Disconnecting from camera {self.index_or_path}...")
raise DeviceNotConnectedError(f"{self} not connected.")
if self.thread is not None:
self._shutdown_read_thread()
self._stop_read_thread()
if self.videocapture_camera is not None:
logger.debug(f"Releasing OpenCV VideoCapture object for {self}.")
self.videocapture_camera.release()
self.videocapture_camera = None
if self.videocapture is not None:
self.videocapture.release()
self.videocapture = None
logger.info(f"Camera {self.index_or_path} disconnected successfully.")
logger.info(f"{self} disconnected.")

5
lerobot/common/cameras/opencv/configuration_opencv.py
View File

@@ -44,7 +44,6 @@ class OpenCVCameraConfig(CameraConfig):
width: Requested frame width in pixels for the color stream.
height: Requested frame height in pixels for the color stream.
color_mode: Color mode for image output (RGB or BGR). Defaults to RGB.
channels: Number of color channels (currently only 3 is supported).
rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation.
Note:
@@ -53,7 +52,6 @@ class OpenCVCameraConfig(CameraConfig):
index_or_path: int | Path
color_mode: ColorMode = ColorMode.RGB
channels: int = 3 # NOTE(Steven): Why is this a config?
rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION
def __post_init__(self):
@@ -71,6 +69,3 @@ class OpenCVCameraConfig(CameraConfig):
raise ValueError(
f"`rotation` is expected to be in {(Cv2Rotation.NO_ROTATION, Cv2Rotation.ROTATE_90, Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270)}, but {self.rotation} is provided."
)
if self.channels != 3:
raise NotImplementedError(f"Unsupported number of channels: {self.channels}")

0
lerobot/common/cameras/intel/__init__.py → lerobot/common/cameras/realsense/__init__.py
View File

364
lerobot/common/cameras/intel/camera_realsense.py → lerobot/common/cameras/realsense/camera_realsense.py
View File

@@ -18,7 +18,6 @@ Provides the RealSenseCamera class for capturing frames from Intel RealSense cam
import contextlib
import logging
import math
import queue
import time
from threading import Event, Thread
@@ -26,10 +25,13 @@ from typing import Any, Dict, List
import cv2
import numpy as np
import pyrealsense2 as rs
try:
import pyrealsense2 as rs
except Exception as e:
logging.info(f"Could not import realsense: {e}")
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
from lerobot.common.utils.utils import capture_timestamp_utc
from ..camera import Camera
from ..configs import ColorMode
@@ -51,7 +53,7 @@ class RealSenseCamera(Camera):
Use the provided utility script to find available camera indices and default profiles:
```bash
python -m lerobot.find_cameras
python -m lerobot.find_cameras realsense
```
A `RealSenseCamera` instance requires a configuration object specifying the
@@ -61,53 +63,46 @@ class RealSenseCamera(Camera):
The camera's default settings (FPS, resolution, color mode) from the stream
profile are used unless overridden in the configuration.
Args:
config (RealSenseCameraConfig): Configuration object containing settings like
serial number or name, desired FPS, width, height, color mode, rotation,
and whether to capture depth.
Example:
```python
from lerobot.common.cameras.intel.camera_realsense import RealSenseCamera
from lerobot.common.cameras.intel.configuration_realsense import RealSenseCameraConfig
from lerobot.common.cameras.configs import ColorMode
from lerobot.common.cameras.realsense import RealSenseCamera, RealSenseCameraConfig
from lerobot.common.cameras import ColorMode, Cv2Rotation
# Basic usage with serial number
config = RealSenseCameraConfig(serial_number="1234567890") # Replace with actual SN
config = RealSenseCameraConfig(serial_number_or_name="1234567890") # Replace with actual SN
camera = RealSenseCamera(config)
try:
camera.connect()
print(f"Connected to {camera}")
color_image = camera.read() # Synchronous read (color only)
print(f"Read frame shape: {color_image.shape}")
async_image = camera.async_read() # Asynchronous read
print(f"Async read frame shape: {async_image.shape}")
except Exception as e:
print(f"An error occurred: {e}")
finally:
camera.disconnect()
print(f"Disconnected from {camera}")
camera.connect()
# Read 1 frame synchronously
color_image = camera.read()
print(color_image.shape)
# Read 1 frame asynchronously
async_image = camera.async_read()
# When done, properly disconnect the camera using
camera.disconnect()
# Example with depth capture and custom settings
custom_config = RealSenseCameraConfig(
serial_number="1234567890", # Replace with actual SN
serial_number_or_name="1234567890", # Replace with actual SN
fps=30,
width=1280,
height=720,
color_mode=ColorMode.BGR, # Request BGR output
rotation=0,
rotation=Cv2Rotation.NO_ROTATION,
use_depth=True
)
depth_camera = RealSenseCamera(custom_config)
try:
depth_camera.connect()
color_image, depth_map = depth_camera.read() # Returns tuple
print(f"Color shape: {color_image.shape}, Depth shape: {depth_map.shape}")
depth_map = depth_camera.read_depth()
print(f"Depth shape: {depth_map.shape}")
finally:
depth_camera.disconnect()
# Example using a unique camera name
name_config = RealSenseCameraConfig(name="Intel RealSense D435") # If unique
name_config = RealSenseCameraConfig(serial_number_or_name="Intel RealSense D435") # If unique
name_camera = RealSenseCamera(name_config)
# ... connect, read, disconnect ...
```
@@ -125,17 +120,15 @@ class RealSenseCamera(Camera):
self.config = config
if config.name is not None: # NOTE(Steven): Do we want to continue supporting this?
self.serial_number = self._find_serial_number_from_name(config.name)
elif config.serial_number is not None:
self.serial_number = str(config.serial_number)
if isinstance(config.serial_number_or_name, int):
self.serial_number = str(config.serial_number_or_name)
else:
raise ValueError("RealSenseCameraConfig must provide either 'serial_number' or 'name'.")
self.serial_number = self._find_serial_number_from_name(config.serial_number_or_name)
self.fps: int | None = config.fps
self.channels: int = config.channels
self.color_mode: ColorMode = config.color_mode
self.use_depth: bool = config.use_depth
self.fps = config.fps
self.color_mode = config.color_mode
self.use_depth = config.use_depth
self.warmup_time = config.warmup_time
self.rs_pipeline: rs.pipeline | None = None
self.rs_profile: rs.pipeline_profile | None = None
@@ -144,18 +137,15 @@ class RealSenseCamera(Camera):
self.stop_event: Event | None = None
self.frame_queue: queue.Queue = queue.Queue(maxsize=1)
self.logs: dict = {} # For timestamping or other metadata
self.rotation: int | None = get_cv2_rotation(config.rotation)
if self.height and self.width:
if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
self.prerotated_width, self.prerotated_height = self.height, self.width
self.capture_width, self.capture_height = self.height, self.width
else:
self.prerotated_width, self.prerotated_height = self.width, self.height
self.capture_width, self.capture_height = self.width, self.height
def __str__(self) -> str:
"""Returns a string representation of the camera instance."""
return f"{self.__class__.__name__}({self.serial_number})"
@property
@@ -163,36 +153,69 @@ class RealSenseCamera(Camera):
"""Checks if the camera pipeline is started and streams are active."""
return self.rs_pipeline is not None and self.rs_profile is not None
def connect(self, warmup: bool = True):
"""
Connects to the RealSense camera specified in the configuration.
Initializes the RealSense pipeline, configures the required streams (color
and optionally depth), starts the pipeline, and validates the actual stream settings.
Raises:
DeviceAlreadyConnectedError: If the camera is already connected.
ValueError: If the configuration is invalid (e.g., missing serial/name, name not unique).
ConnectionError: If the camera is found but fails to start the pipeline or no RealSense devices are detected at all.
RuntimeError: If the pipeline starts but fails to apply requested settings.
"""
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} is already connected.")
self.rs_pipeline = rs.pipeline()
rs_config = self._make_rs_pipeline_config()
try:
self.rs_profile = self.rs_pipeline.start(rs_config)
logger.debug(f"Successfully started pipeline for camera {self.serial_number}.")
except RuntimeError as e:
self.rs_profile = None
self.rs_pipeline = None
raise ConnectionError(
f"Failed to open {self} camera. Run 'python -m lerobot.find_cameras realsense' for details about the available cameras in your system."
) from e
logger.debug(f"Validating stream configuration for {self}...")
self._validate_capture_settings()
if warmup:
if self.warmup_time is None:
raise ValueError(
f"Warmup time is not set for {self}. Please set a warmup time in the configuration."
)
logger.debug(f"Reading a warm-up frames for {self} for {self.warmup_time} seconds...")
start_time = time.time()
while time.time() - start_time < self.warmup_time:
self.read()
time.sleep(0.1)
logger.info(f"{self} connected.")
@staticmethod
def find_cameras(raise_when_empty: bool = True) -> List[Dict[str, Any]]:
def find_cameras() -> List[Dict[str, Any]]:
"""
Detects available Intel RealSense cameras connected to the system.
Args:
raise_when_empty (bool): If True, raises an OSError if no cameras are found.
Returns:
List[Dict[str, Any]]: A list of dictionaries,
where each dictionary contains 'type', 'id' (serial number), 'name',
firmware version, USB type, and other available specs, and the default profile properties (width, height, fps, format).
Raises:
OSError: If `raise_when_empty` is True and no cameras are detected,
or if pyrealsense2 is not installed.
OSError: If pyrealsense2 is not installed.
ImportError: If pyrealsense2 is not installed.
"""
found_cameras_info = []
context = rs.context()
devices = context.query_devices()
if not devices:
logger.warning("No RealSense devices detected.")
if raise_when_empty:
raise OSError(
"No RealSense devices detected. Ensure cameras are connected, "
"library (`pyrealsense2`) is installed, and firmware is up-to-date."
)
for device in devices:
camera_info = {
"name": device.get_info(rs.camera_info.name),
@@ -223,14 +246,13 @@ class RealSenseCamera(Camera):
camera_info["default_stream_profile"] = stream_info
found_cameras_info.append(camera_info)
logger.debug(f"Found RealSense camera: {camera_info}")
logger.info(f"Detected RealSense cameras: {[cam['id'] for cam in found_cameras_info]}")
return found_cameras_info
def _find_serial_number_from_name(self, name: str) -> str:
"""Finds the serial number for a given unique camera name."""
camera_infos = self.find_cameras(raise_when_empty=True)
camera_infos = self.find_cameras()
found_devices = [cam for cam in camera_infos if str(cam["name"]) == name]
if not found_devices:
@@ -250,24 +272,24 @@ class RealSenseCamera(Camera):
logger.info(f"Found serial number '{serial_number}' for camera name '{name}'.")
return serial_number
def _configure_realsense_settings(self) -> rs.config:
def _make_rs_pipeline_config(self) -> rs.config:
"""Creates and configures the RealSense pipeline configuration object."""
rs_config = rs.config()
rs.config.enable_device(rs_config, self.serial_number)
if self.width and self.height and self.fps:
logger.debug(
f"Requesting Color Stream: {self.prerotated_width}x{self.prerotated_height} @ {self.fps} FPS, Format: {rs.format.rgb8}"
f"Requesting Color Stream: {self.capture_width}x{self.capture_height} @ {self.fps} FPS, Format: {rs.format.rgb8}"
)
rs_config.enable_stream(
rs.stream.color, self.prerotated_width, self.prerotated_height, rs.format.rgb8, self.fps
rs.stream.color, self.capture_width, self.capture_height, rs.format.rgb8, self.fps
)
if self.use_depth:
logger.debug(
f"Requesting Depth Stream: {self.prerotated_width}x{self.prerotated_height} @ {self.fps} FPS, Format: {rs.format.z16}"
f"Requesting Depth Stream: {self.capture_width}x{self.capture_height} @ {self.fps} FPS, Format: {rs.format.z16}"
)
rs_config.enable_stream(
rs.stream.depth, self.prerotated_width, self.prerotated_height, rs.format.z16, self.fps
rs.stream.depth, self.capture_width, self.capture_height, rs.format.z16, self.fps
)
else:
logger.debug(f"Requesting Color Stream: Default settings, Format: {rs.stream.color}")
@@ -295,111 +317,47 @@ class RealSenseCamera(Camera):
if not self.is_connected:
raise DeviceNotConnectedError(f"Cannot validate settings for {self} as it is not connected.")
self._validate_fps(self.rs_profile.get_stream(rs.stream.color).as_video_stream_profile())
self._validate_width_and_height(self.rs_profile.get_stream(rs.stream.color).as_video_stream_profile())
if self.use_depth:
self._validate_fps(self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile())
self._validate_width_and_height(
self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile()
)
def connect(self, do_warmup_read: bool = True):
"""
Connects to the RealSense camera specified in the configuration.
Initializes the RealSense pipeline, configures the required streams (color
and optionally depth), starts the pipeline, and validates the actual stream settings.
Raises:
DeviceAlreadyConnectedError: If the camera is already connected.
ValueError: If the configuration is invalid (e.g., missing serial/name, name not unique).
ConnectionError: If the camera is found but fails to start the pipeline.
RuntimeError: If the pipeline starts but fails to apply requested settings.
OSError: If no RealSense devices are detected at all.
"""
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} is already connected.")
logger.debug(f"Attempting to connect to camera {self.serial_number}...")
self.rs_pipeline = rs.pipeline()
rs_config = self._configure_realsense_settings()
try:
self.rs_profile = self.rs_pipeline.start(rs_config)
logger.debug(f"Successfully started pipeline for camera {self.serial_number}.")
except RuntimeError as e:
self.rs_profile = None
self.rs_pipeline = None
raise ConnectionError(
f"Failed to open RealSense camera {self.serial_number}. Error: {e}. "
f"Run 'python -m find_cameras list-cameras' for details."
) from e
logger.debug(f"Validating stream configuration for {self.serial_number}...")
self._validate_capture_settings()
if do_warmup_read:
logger.debug(f"Reading a warm-up frame for {self.serial_number}...")
self.read() # NOTE(Steven): For now we just read one frame, we could also loop for X frames/secs
logger.info(f"Camera {self.serial_number} connected and configured successfully.")
def _validate_fps(self, stream) -> None:
"""Validates and sets the internal FPS based on actual stream FPS."""
actual_fps = stream.fps()
stream = self.rs_profile.get_stream(rs.stream.color).as_video_stream_profile()
if self.fps is None:
self.fps = actual_fps
logger.info(f"FPS not specified, using camera default: {self.fps} FPS.")
return
self.fps = stream.fps()
# Use math.isclose for robust float comparison
if not math.isclose(self.fps, actual_fps, rel_tol=1e-3):
logger.warning(
f"Requested FPS {self.fps} for {self}, but camera reported {actual_fps}. "
"This might be due to camera limitations."
)
raise RuntimeError(
f"Failed to set requested FPS {self.fps} for {self}. Actual value reported: {actual_fps}."
)
logger.debug(f"FPS set to {actual_fps} for {self}.")
if self.width is None or self.height is None:
actual_width = int(round(stream.width()))
actual_height = int(round(stream.height()))
if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
self.width, self.height = actual_height, actual_width
self.capture_width, self.capture_height = actual_width, actual_height
else:
self.width, self.height = actual_width, actual_height
self.capture_width, self.capture_height = actual_width, actual_height
logger.info(f"Capture width set to camera default: {self.width}.")
logger.info(f"Capture height set to camera default: {self.height}.")
else:
self._validate_width_and_height(stream)
if self.use_depth:
stream = self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile()
self._validate_width_and_height(stream)
def _validate_width_and_height(self, stream) -> None:
"""Validates and sets the internal capture width and height based on actual stream width."""
actual_width = int(round(stream.width()))
actual_height = int(round(stream.height()))
if self.width is None or self.height is None:
if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
self.width, self.height = actual_height, actual_width
self.prerotated_width, self.prerotated_height = actual_width, actual_height
else:
self.width, self.height = actual_width, actual_height
self.prerotated_width, self.prerotated_height = actual_width, actual_height
logger.info(f"Capture width set to camera default: {self.width}.")
logger.info(f"Capture height set to camera default: {self.height}.")
return
if self.prerotated_width != actual_width:
logger.warning(
f"Requested capture width {self.prerotated_width} for {self}, but camera reported {actual_width}."
)
if self.capture_width != actual_width:
raise RuntimeError(
f"Failed to set requested capture width {self.prerotated_width} for {self}. Actual value: {actual_width}."
f"Failed to set requested capture width {self.capture_width} for {self}. Actual value: {actual_width}."
)
logger.debug(f"Capture width set to {actual_width} for {self}.")
if self.prerotated_height != actual_height:
logger.warning(
f"Requested capture height {self.prerotated_height} for {self}, but camera reported {actual_height}."
)
if self.capture_height != actual_height:
raise RuntimeError(
f"Failed to set requested capture height {self.prerotated_height} for {self}. Actual value: {actual_height}."
f"Failed to set requested capture height {self.capture_height} for {self}. Actual value: {actual_height}."
)
logger.debug(f"Capture height set to {actual_height} for {self}.")
def read_depth(self, timeout_ms: int = 5000) -> np.ndarray:
def read_depth(self, timeout_ms: int = 100) -> np.ndarray:
"""
Reads a single frame (depth) synchronously from the camera.
@@ -407,7 +365,7 @@ class RealSenseCamera(Camera):
from the camera hardware via the RealSense pipeline.
Args:
timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 5000ms.
timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 100ms.
Returns:
np.ndarray: The depth map as a NumPy array (height, width)
@@ -420,35 +378,29 @@ class RealSenseCamera(Camera):
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
if not self.use_depth:
raise RuntimeError(
f"Failed to capture depth frame from {self}. '.read_depth()'. Depth stream is not enabled."
f"Failed to capture depth frame '.read_depth()'. Depth stream is not enabled for {self}."
)
start_time = time.perf_counter()
ret, frame = self.rs_pipeline.try_wait_for_frames(
timeout_ms=timeout_ms
) # NOTE(Steven): This read has a timeout
ret, frame = self.rs_pipeline.try_wait_for_frames(timeout_ms=timeout_ms)
if not ret or frame is None:
raise RuntimeError(
f"Failed to capture frame from {self}. '.read_depth()' returned status={ret} and frame is None."
)
raise RuntimeError(f"{self} failed to capture frame. Returned status='{ret}'.")
depth_frame = frame.get_depth_frame()
depth_map = np.asanyarray(depth_frame.get_data())
depth_map_processed = self._postprocess_image(depth_map)
depth_map_processed = self._postprocess_image(depth_map, depth_frame=True)
read_duration_ms = (time.perf_counter() - start_time) * 1e3
logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
self.logs["timestamp_utc"] = capture_timestamp_utc()
return depth_map_processed
def read(self, color_mode: ColorMode | None = None, timeout_ms: int = 5000) -> np.ndarray:
def read(self, color_mode: ColorMode | None = None, timeout_ms: int = 100) -> np.ndarray:
"""
Reads a single frame (color) synchronously from the camera.
@@ -456,7 +408,7 @@ class RealSenseCamera(Camera):
from the camera hardware via the RealSense pipeline.
Args:
timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 5000ms.
timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 100ms.
Returns:
np.ndarray: The captured color frame as a NumPy array
@@ -490,10 +442,11 @@ class RealSenseCamera(Camera):
read_duration_ms = (time.perf_counter() - start_time) * 1e3
logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
self.logs["timestamp_utc"] = capture_timestamp_utc()
return color_image_processed
def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray:
def _postprocess_image(
self, image: np.ndarray, color_mode: ColorMode | None = None, depth_frame: bool = False
) -> np.ndarray:
"""
Applies color conversion, dimension validation, and rotation to a raw color frame.
@@ -516,15 +469,14 @@ class RealSenseCamera(Camera):
f"Invalid requested color mode '{color_mode}'. Expected {ColorMode.RGB} or {ColorMode.BGR}."
)
h, w, c = image.shape
if depth_frame:
h, w = image.shape
else:
h, w, _c = image.shape
if h != self.prerotated_height or w != self.prerotated_width:
if h != self.capture_height or w != self.capture_width:
raise RuntimeError(
f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.prerotated_height}x{self.prerotated_width}) for {self}."
)
if c != self.channels:
logger.warning(
f"Captured frame channels ({c}) do not match configured channels ({self.channels}) for {self}."
f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.capture_height}x{self.capture_width}) for {self}."
)
processed_image = image
@@ -564,7 +516,7 @@ class RealSenseCamera(Camera):
logger.debug(f"Stopping read loop thread for {self}.")
def _ensure_read_thread_running(self):
def _start_read_thread(self) -> None:
"""Starts or restarts the background read thread if it's not running."""
if self.thread is not None and self.thread.is_alive():
self.thread.join(timeout=0.1)
@@ -572,15 +524,29 @@ class RealSenseCamera(Camera):
self.stop_event.set()
self.stop_event = Event()
self.thread = Thread(
target=self._read_loop, args=(), name=f"RealSenseReadLoop-{self}-{self.serial_number}"
)
self.thread = Thread(target=self._read_loop, args=(), name=f"{self}_read_loop")
self.thread.daemon = True
self.thread.start()
logger.debug(f"Read thread started for {self}.")
def _stop_read_thread(self):
"""Signals the background read thread to stop and waits for it to join."""
if self.stop_event is not None:
self.stop_event.set()
if self.thread is not None and self.thread.is_alive():
self.thread.join(timeout=2.0)
if self.thread.is_alive():
logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.")
else:
logger.debug(f"Read thread for {self} joined successfully.")
self.thread = None
self.stop_event = None
logger.debug(f"Read thread stopped for {self}.")
# NOTE(Steven): Missing implementation for depth for now
def async_read(self, timeout_ms: float = 2000) -> np.ndarray:
def async_read(self, timeout_ms: float = 100) -> np.ndarray:
"""
Reads the latest available frame data (color or color+depth) asynchronously.
@@ -591,7 +557,7 @@ class RealSenseCamera(Camera):
Args:
timeout_ms (float): Maximum time in milliseconds to wait for a frame
to become available in the queue. Defaults to 2000ms (2 seconds).
to become available in the queue. Defaults to 100ms (0.1 seconds).
Returns:
np.ndarray:
@@ -606,42 +572,18 @@ class RealSenseCamera(Camera):
raise DeviceNotConnectedError(f"{self} is not connected.")
if self.thread is None or not self.thread.is_alive():
self._ensure_read_thread_running()
self._start_read_thread()
try:
return self.frame_queue.get(timeout=timeout_ms / 1000.0)
except queue.Empty as e:
thread_alive = self.thread is not None and self.thread.is_alive()
logger.error(
f"Timeout waiting for frame from {self} queue after {timeout_ms}ms. "
f"(Read thread alive: {thread_alive})"
)
raise TimeoutError(
f"Timed out waiting for frame from camera {self.serial_number} after {timeout_ms} ms. "
f"Timed out waiting for frame from camera {self} after {timeout_ms} ms. "
f"Read thread alive: {thread_alive}."
) from e
except Exception as e:
logger.exception(f"Unexpected error getting frame data from queue for {self}: {e}")
raise RuntimeError(
f"Error getting frame data from queue for camera {self.serial_number}: {e}"
) from e
def _shutdown_read_thread(self):
"""Signals the background read thread to stop and waits for it to join."""
if self.stop_event is not None:
logger.debug(f"Signaling stop event for read thread of {self}.")
self.stop_event.set()
if self.thread is not None and self.thread.is_alive():
logger.debug(f"Waiting for read thread of {self} to join...")
self.thread.join(timeout=2.0)
if self.thread.is_alive():
logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.")
else:
logger.debug(f"Read thread for {self} joined successfully.")
self.thread = None
self.stop_event = None
raise RuntimeError(f"Error getting frame data from queue for camera {self}: {e}") from e
def disconnect(self):
"""
@@ -658,10 +600,8 @@ class RealSenseCamera(Camera):
f"Attempted to disconnect {self}, but it appears already disconnected."
)
logger.debug(f"Disconnecting from camera {self.serial_number}...")
if self.thread is not None:
self._shutdown_read_thread()
self._stop_read_thread()
if self.rs_pipeline is not None:
logger.debug(f"Stopping RealSense pipeline object for {self}.")
@@ -669,4 +609,4 @@ class RealSenseCamera(Camera):
self.rs_pipeline = None
self.rs_profile = None
logger.info(f"Camera {self.serial_number} disconnected successfully.")
logger.info(f"{self} disconnected.")

21
lerobot/common/cameras/intel/configuration_realsense.py → lerobot/common/cameras/realsense/configuration_realsense.py
View File

@@ -40,25 +40,20 @@ class RealSenseCameraConfig(CameraConfig):
fps: Requested frames per second for the color stream.
width: Requested frame width in pixels for the color stream.
height: Requested frame height in pixels for the color stream.
name: Optional human-readable name to identify the camera.
serial_number: Optional unique serial number to identify the camera.
Either name or serial_number must be provided.
serial_number_or_name: Unique serial number or human-readable name to identify the camera.
color_mode: Color mode for image output (RGB or BGR). Defaults to RGB.
channels: Number of color channels (currently only 3 is supported).
use_depth: Whether to enable depth stream. Defaults to False.
rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation.
Note:
- Either name or serial_number must be specified, but not both.
- Either name or serial_number must be specified.
- Depth stream configuration (if enabled) will use the same FPS as the color stream.
- The actual resolution and FPS may be adjusted by the camera to the nearest supported mode.
- Only 3-channel color output (RGB/BGR) is currently supported.
- For `fps`, `width` and `height`, either all of them need to be set, or none of them.
"""
name: str | None = None
serial_number: int | None = None
serial_number_or_name: int | str
color_mode: ColorMode = ColorMode.RGB
channels: int | None = 3
use_depth: bool = False
rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION # NOTE(Steven): Check if draccus can parse to an enum
@@ -78,10 +73,8 @@ class RealSenseCameraConfig(CameraConfig):
f"`rotation` is expected to be in {(Cv2Rotation.NO_ROTATION, Cv2Rotation.ROTATE_90, Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270)}, but {self.rotation} is provided."
)
if self.channels != 3:
raise NotImplementedError(f"Unsupported number of channels: {self.channels}")
if bool(self.name) and bool(self.serial_number):
values = (self.fps, self.width, self.height)
if any(v is not None for v in values) and any(v is None for v in values):
raise ValueError(
f"One of them must be set: name or serial_number, but {self.name=} and {self.serial_number=} provided."
"For `fps`, `width` and `height`, either all of them need to be set, or none of them."
)

26
lerobot/common/cameras/utils.py
View File

@@ -37,7 +37,7 @@ def make_cameras_from_configs(camera_configs: dict[str, CameraConfig]) -> dict[s
cameras[key] = OpenCVCamera(cfg)
elif cfg.type == "intelrealsense":
from .intel.camera_realsense import RealSenseCamera
from .realsense.camera_realsense import RealSenseCamera
cameras[key] = RealSenseCamera(cfg)
else:
@@ -46,24 +46,26 @@ def make_cameras_from_configs(camera_configs: dict[str, CameraConfig]) -> dict[s
return cameras
def get_cv2_rotation(rotation: Cv2Rotation) -> int:
def get_cv2_rotation(rotation: Cv2Rotation) -> int | None:
import cv2
return {
Cv2Rotation.ROTATE_270: cv2.ROTATE_90_COUNTERCLOCKWISE,
Cv2Rotation.ROTATE_90: cv2.ROTATE_90_CLOCKWISE,
Cv2Rotation.ROTATE_180: cv2.ROTATE_180,
}.get(rotation)
if rotation == Cv2Rotation.ROTATE_90:
return cv2.ROTATE_90_CLOCKWISE
elif rotation == Cv2Rotation.ROTATE_180:
return cv2.ROTATE_180
elif rotation == Cv2Rotation.ROTATE_270:
return cv2.ROTATE_90_COUNTERCLOCKWISE
else:
return None
def get_cv2_backend() -> int:
import cv2
return {
"Linux": cv2.CAP_DSHOW,
"Windows": cv2.CAP_AVFOUNDATION,
"Darwin": cv2.CAP_ANY,
}.get(platform.system(), cv2.CAP_V4L2)
if platform.system() == "Windows":
return cv2.CAP_AVFOUNDATION
else:
return cv2.CAP_ANY
def save_image(img_array: np.ndarray, camera_index: int, frame_index: int, images_dir: Path):

3
lerobot/common/envs/configs.py
View File

@@ -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()

5
lerobot/common/motors/motors_bus.py
View File

@@ -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]:

16
lerobot/common/robots/config.py
View File

@@ -27,15 +27,13 @@ class RobotConfig(draccus.ChoiceRegistry, abc.ABC):
calibration_dir: Path | None = None
def __post_init__(self):
if hasattr(self, "cameras"):
cameras = self.cameras
if cameras:
for cam_name, cam_config in cameras.items():
for attr in ["width", "height", "fps"]:
if getattr(cam_config, attr) is None:
raise ValueError(
f"Camera config for '{cam_name}' has None value for required attribute '{attr}'"
)
if hasattr(self, "cameras") and self.cameras:
for _, config in self.cameras.items():
for attr in ["width", "height", "fps"]:
if getattr(config, attr) is None:
raise ValueError(
f"Specifying '{attr}' is required for the camera to be used in a robot"
)
@property
def type(self) -> str:

7
lerobot/common/robots/lekiwi/lekiwi.py
View File

@@ -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.")

12
lerobot/common/robots/robot.py
View File

@@ -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

2
lerobot/common/robots/stretch3/configuration_stretch3.py
View File

@@ -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

12
lerobot/common/teleoperators/teleoperator.py
View File

@@ -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

1
lerobot/common/utils/control_utils.py
View File

@@ -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:

9
lerobot/configs/policies.py
View File

@@ -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

63
lerobot/find_cameras.py
View File

@@ -14,6 +14,16 @@
# See the License for the specific language governing permissions and
# limitations under the License.
"""
Helper to find the camera devices available in your system.
Example:
```shell
python -m lerobot.find_cameras
```
"""
import argparse
import concurrent.futures
import logging
@@ -26,13 +36,12 @@ import numpy as np
from PIL import Image
from lerobot.common.cameras.configs import ColorMode
from lerobot.common.cameras.intel.camera_realsense import RealSenseCamera
from lerobot.common.cameras.intel.configuration_realsense import RealSenseCameraConfig
from lerobot.common.cameras.opencv.camera_opencv import OpenCVCamera
from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig
from lerobot.common.cameras.realsense.camera_realsense import RealSenseCamera
from lerobot.common.cameras.realsense.configuration_realsense import RealSenseCameraConfig
logger = logging.getLogger(__name__)
# logging.basicConfig(level=logging.INFO, format="%(asctime)s - %(levelname)s - %(module)s - %(message)s")
def find_all_opencv_cameras() -> List[Dict[str, Any]]:
@@ -45,7 +54,7 @@ def find_all_opencv_cameras() -> List[Dict[str, Any]]:
all_opencv_cameras_info: List[Dict[str, Any]] = []
logger.info("Searching for OpenCV cameras...")
try:
opencv_cameras = OpenCVCamera.find_cameras(raise_when_empty=False)
opencv_cameras = OpenCVCamera.find_cameras()
for cam_info in opencv_cameras:
all_opencv_cameras_info.append(cam_info)
logger.info(f"Found {len(opencv_cameras)} OpenCV cameras.")
@@ -65,7 +74,7 @@ def find_all_realsense_cameras() -> List[Dict[str, Any]]:
all_realsense_cameras_info: List[Dict[str, Any]] = []
logger.info("Searching for RealSense cameras...")
try:
realsense_cameras = RealSenseCamera.find_cameras(raise_when_empty=False)
realsense_cameras = RealSenseCamera.find_cameras()
for cam_info in realsense_cameras:
all_realsense_cameras_info.append(cam_info)
logger.info(f"Found {len(realsense_cameras)} RealSense cameras.")
@@ -170,7 +179,7 @@ def create_camera_instance(cam_meta: Dict[str, Any]) -> Dict[str, Any] | None:
instance = OpenCVCamera(cv_config)
elif cam_type == "RealSense":
rs_config = RealSenseCameraConfig(
serial_number=str(cam_id),
serial_number_or_name=int(cam_id),
color_mode=ColorMode.RGB,
)
instance = RealSenseCamera(rs_config)
@@ -180,7 +189,7 @@ def create_camera_instance(cam_meta: Dict[str, Any]) -> Dict[str, Any] | None:
if instance:
logger.info(f"Connecting to {cam_type} camera: {cam_id}...")
instance.connect(do_warmup_read=False)
instance.connect(warmup=False)
return {"instance": instance, "meta": cam_meta}
except Exception as e:
logger.error(f"Failed to connect or configure {cam_type} camera {cam_id}: {e}")
@@ -293,28 +302,8 @@ if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Unified camera utility script for listing cameras and capturing images."
)
subparsers = parser.add_subparsers(dest="command", help="Available commands")
# List cameras command
list_parser = subparsers.add_parser(
"list-cameras", help="Shows connected cameras. Optionally filter by type (realsense or opencv)."
)
list_parser.add_argument(
"camera_type",
type=str,
nargs="?",
default=None,
choices=["realsense", "opencv"],
help="Specify camera type to list (e.g., 'realsense', 'opencv'). Lists all if omitted.",
)
list_parser.set_defaults(func=lambda args: find_and_print_cameras(args.camera_type))
# Capture images command
capture_parser = subparsers.add_parser(
"capture-images",
help="Saves images from detected cameras (optionally filtered by type) using their default stream profiles.",
)
capture_parser.add_argument(
parser.add_argument(
"camera_type",
type=str,
nargs="?",
@@ -322,19 +311,19 @@ if __name__ == "__main__":
choices=["realsense", "opencv"],
help="Specify camera type to capture from (e.g., 'realsense', 'opencv'). Captures from all if omitted.",
)
capture_parser.add_argument(
parser.add_argument(
"--output-dir",
type=Path,
default="outputs/captured_images",
help="Directory to save images. Default: outputs/captured_images",
)
capture_parser.add_argument(
parser.add_argument(
"--record-time-s",
type=float,
default=6.0,
help="Time duration to attempt capturing frames. Default: 6 seconds.",
)
capture_parser.set_defaults(
parser.set_defaults(
func=lambda args: save_images_from_all_cameras(
output_dir=args.output_dir,
record_time_s=args.record_time_s,
@@ -344,14 +333,4 @@ if __name__ == "__main__":
args = parser.parse_args()
if args.command is None:
default_output_dir = capture_parser.get_default("output_dir")
default_record_time_s = capture_parser.get_default("record_time_s")
save_images_from_all_cameras(
output_dir=default_output_dir,
record_time_s=default_record_time_s,
camera_type_filter=None,
)
else:
args.func(args)
args.func(args)

2
lerobot/record.py
View File

@@ -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

2
lerobot/teleoperate.py
View File

@@ -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,

2
pyproject.toml
View File

@@ -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'"]

0
tests/artifacts/cameras/fakecam_square_128x128.png → tests/artifacts/cameras/image_128x128.png
View File

0
tests/artifacts/cameras/fakecam_sd_160x120.png → tests/artifacts/cameras/image_160x120.png
View File

0
tests/artifacts/cameras/fakecam_hd_320x180.png → tests/artifacts/cameras/image_320x180.png
View File

0
tests/artifacts/cameras/fakecam_fullhd_480x270.png → tests/artifacts/cameras/image_480x270.png
View File

52
tests/cameras/test_opencv.py
View File

@@ -19,7 +19,7 @@
# pytest tests/cameras/test_opencv.py::test_connect
# ```
import os
from pathlib import Path
import numpy as np
import pytest
@@ -29,17 +29,18 @@ from lerobot.common.cameras.opencv import OpenCVCamera, OpenCVCameraConfig
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
# NOTE(Steven): more tests + assertions?
TEST_ARTIFACTS_DIR = os.path.join(os.path.dirname(os.path.dirname(__file__)), "artifacts", "cameras")
DEFAULT_PNG_FILE_PATH = os.path.join(TEST_ARTIFACTS_DIR, "fakecam_sd_160x120.png")
TEST_ARTIFACTS_DIR = Path(__file__).parent.parent / "artifacts" / "cameras"
DEFAULT_PNG_FILE_PATH = TEST_ARTIFACTS_DIR / "image_160x120.png"
TEST_IMAGE_PATHS = [
os.path.join(TEST_ARTIFACTS_DIR, "fakecam_sd_160x120.png"),
os.path.join(TEST_ARTIFACTS_DIR, "fakecam_hd_320x180.png"),
os.path.join(TEST_ARTIFACTS_DIR, "fakecam_fullhd_480x270.png"),
os.path.join(TEST_ARTIFACTS_DIR, "fakecam_square_128x128.png"),
TEST_ARTIFACTS_DIR / "image_160x120.png",
TEST_ARTIFACTS_DIR / "image_320x180.png",
TEST_ARTIFACTS_DIR / "image_480x270.png",
TEST_ARTIFACTS_DIR / "image_128x128.png",
]
def test_base_class_implementation():
def test_abc_implementation():
"""Instantiation should raise an error if the class doesn't implement abstract methods/properties."""
config = OpenCVCameraConfig(index_or_path=0)
_ = OpenCVCamera(config)
@@ -49,7 +50,7 @@ def test_connect():
config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH)
camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
assert camera.is_connected
@@ -57,10 +58,10 @@ def test_connect():
def test_connect_already_connected():
config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH)
camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
with pytest.raises(DeviceAlreadyConnectedError):
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
def test_connect_invalid_camera_path():
@@ -68,7 +69,7 @@ def test_connect_invalid_camera_path():
camera = OpenCVCamera(config)
with pytest.raises(ConnectionError):
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
def test_invalid_width_connect():
@@ -80,14 +81,14 @@ def test_invalid_width_connect():
camera = OpenCVCamera(config)
with pytest.raises(RuntimeError):
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS)
@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS, ids=[str(path.name) for path in TEST_IMAGE_PATHS])
def test_read(index_or_path):
config = OpenCVCameraConfig(index_or_path=index_or_path)
camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
img = camera.read()
@@ -105,7 +106,7 @@ def test_read_before_connect():
def test_disconnect():
config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH)
camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
camera.disconnect()
@@ -120,11 +121,11 @@ def test_disconnect_before_connect():
_ = camera.disconnect()
@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS)
@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS, ids=[str(path.name) for path in TEST_IMAGE_PATHS])
def test_async_read(index_or_path):
config = OpenCVCameraConfig(index_or_path=index_or_path)
camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
try:
img = camera.async_read()
@@ -140,11 +141,13 @@ def test_async_read(index_or_path):
def test_async_read_timeout():
config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH)
camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
try:
with pytest.raises(TimeoutError):
camera.async_read(timeout_ms=0)
camera.async_read(
timeout_ms=0
) # NOTE(Steven): This is flaky as sdometimes we actually get a frame
finally:
if camera.is_connected:
camera.disconnect()
@@ -158,7 +161,7 @@ def test_async_read_before_connect():
_ = camera.async_read()
@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS)
@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS, ids=[str(path.name) for path in TEST_IMAGE_PATHS])
@pytest.mark.parametrize(
"rotation",
[
@@ -167,15 +170,16 @@ def test_async_read_before_connect():
Cv2Rotation.ROTATE_180,
Cv2Rotation.ROTATE_270,
],
ids=["no_rot", "rot90", "rot180", "rot270"],
)
def test_all_rotations(rotation, index_or_path):
filename = os.path.basename(index_or_path)
def test_rotation(rotation, index_or_path):
filename = Path(index_or_path).name
dimensions = filename.split("_")[-1].split(".")[0] # Assumes filenames format (_wxh.png)
original_width, original_height = map(int, dimensions.split("x"))
config = OpenCVCameraConfig(index_or_path=index_or_path, rotation=rotation)
camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
img = camera.read()
assert isinstance(img, np.ndarray)

124
tests/cameras/test_realsense.py
View File

@@ -19,7 +19,7 @@
# pytest tests/cameras/test_opencv.py::test_connect
# ```
import os
from pathlib import Path
from unittest.mock import patch
import numpy as np
@@ -29,95 +29,103 @@ from lerobot.common.cameras.configs import Cv2Rotation
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
try:
from lerobot.common.cameras.intel import RealSenseCamera, RealSenseCameraConfig
except (ImportError, ModuleNotFoundError):
from lerobot.common.cameras.realsense import RealSenseCamera, RealSenseCameraConfig
except (ImportError, ModuleNotFoundError, NameError):
pytest.skip("pyrealsense2 not available", allow_module_level=True)
TEST_ARTIFACTS_DIR = os.path.join(os.path.dirname(os.path.dirname(__file__)), "artifacts", "cameras")
BAG_FILE_PATH = os.path.join(TEST_ARTIFACTS_DIR, "test_rs.bag")
TEST_ARTIFACTS_DIR = Path(__file__).parent.parent / "artifacts" / "cameras"
BAG_FILE_PATH = TEST_ARTIFACTS_DIR / "test_rs.bag"
# NOTE(Steven): Missing tests for depth
# NOTE(Steven): Takes 20sec, the patch being the biggest bottleneck
# NOTE(Steven): more tests + assertions?
if not os.path.exists(BAG_FILE_PATH):
print(f"Warning: Bag file not found at {BAG_FILE_PATH}. Some tests might fail or be skipped.")
# NOTE(Steven): For some reason these tests take ~20sec in macOS but only ~2sec in Linux.
def mock_rs_config_enable_device_from_file(rs_config_instance, sn):
if not os.path.exists(BAG_FILE_PATH):
raise FileNotFoundError(f"Test bag file not found: {BAG_FILE_PATH}")
return rs_config_instance.enable_device_from_file(BAG_FILE_PATH, repeat_playback=True)
def mock_rs_config_enable_device_from_file(rs_config_instance, _sn):
return rs_config_instance.enable_device_from_file(str(BAG_FILE_PATH), repeat_playback=True)
def mock_rs_config_enable_device_bad_file(rs_config_instance, sn):
def mock_rs_config_enable_device_bad_file(rs_config_instance, _sn):
return rs_config_instance.enable_device_from_file("non_existent_file.bag", repeat_playback=True)
def test_base_class_implementation():
config = RealSenseCameraConfig(serial_number=42)
@pytest.fixture(name="patch_realsense", autouse=True)
def fixture_patch_realsense():
"""Automatically mock pyrealsense2.config.enable_device for all tests."""
with patch(
"pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file
) as mock:
yield mock
def test_abc_implementation():
"""Instantiation should raise an error if the class doesn't implement abstract methods/properties."""
config = RealSenseCameraConfig(serial_number_or_name=42)
_ = RealSenseCamera(config)
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file)
def test_connect(mock_enable_device):
config = RealSenseCameraConfig(serial_number=42)
def test_connect():
config = RealSenseCameraConfig(serial_number_or_name=42)
camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
assert camera.is_connected
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file)
def test_connect_already_connected(mock_enable_device):
config = RealSenseCameraConfig(serial_number=42)
def test_connect_already_connected():
config = RealSenseCameraConfig(serial_number_or_name=42)
camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
with pytest.raises(DeviceAlreadyConnectedError):
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_bad_file)
def test_connect_invalid_camera_path(mock_enable_device):
config = RealSenseCameraConfig(serial_number=42)
def test_connect_invalid_camera_path(patch_realsense):
patch_realsense.side_effect = mock_rs_config_enable_device_bad_file
config = RealSenseCameraConfig(serial_number_or_name=42)
camera = RealSenseCamera(config)
with pytest.raises(ConnectionError):
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file)
def test_invalid_width_connect(mock_enable_device):
config = RealSenseCameraConfig(serial_number=42, width=99999, height=480, fps=30)
def test_invalid_width_connect():
config = RealSenseCameraConfig(serial_number_or_name=42, width=99999, height=480, fps=30)
camera = RealSenseCamera(config)
with pytest.raises(ConnectionError):
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file)
def test_read(mock_enable_device):
config = RealSenseCameraConfig(serial_number=42, width=640, height=480, fps=30)
def test_read():
config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30)
camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
img = camera.read()
assert isinstance(img, np.ndarray)
def test_read_depth():
config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30, use_depth=True)
camera = RealSenseCamera(config)
camera.connect(warmup=False)
img = camera.read_depth(timeout_ms=1000) # NOTE(Steven): Reading depth takes longer
assert isinstance(img, np.ndarray)
def test_read_before_connect():
config = RealSenseCameraConfig(serial_number=42)
config = RealSenseCameraConfig(serial_number_or_name=42)
camera = RealSenseCamera(config)
with pytest.raises(DeviceNotConnectedError):
_ = camera.read()
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file)
def test_disconnect(mock_enable_device):
config = RealSenseCameraConfig(serial_number=42)
def test_disconnect():
config = RealSenseCameraConfig(serial_number_or_name=42)
camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
camera.disconnect()
@@ -125,18 +133,17 @@ def test_disconnect(mock_enable_device):
def test_disconnect_before_connect():
config = RealSenseCameraConfig(serial_number=42)
config = RealSenseCameraConfig(serial_number_or_name=42)
camera = RealSenseCamera(config)
with pytest.raises(DeviceNotConnectedError):
camera.disconnect()
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file)
def test_async_read(mock_enable_device):
config = RealSenseCameraConfig(serial_number=42, width=640, height=480, fps=30)
def test_async_read():
config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30)
camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
try:
img = camera.async_read()
@@ -149,22 +156,23 @@ def test_async_read(mock_enable_device):
camera.disconnect() # To stop/join the thread. Otherwise get warnings when the test ends
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file)
def test_async_read_timeout(mock_enable_device):
config = RealSenseCameraConfig(serial_number=42, width=640, height=480, fps=30)
def test_async_read_timeout():
config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30)
camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
try:
with pytest.raises(TimeoutError):
camera.async_read(timeout_ms=0)
camera.async_read(
timeout_ms=0
) # NOTE(Steven): This is flaky as sdometimes we actually get a frame
finally:
if camera.is_connected:
camera.disconnect()
def test_async_read_before_connect():
config = RealSenseCameraConfig(serial_number=42)
config = RealSenseCameraConfig(serial_number_or_name=42)
camera = RealSenseCamera(config)
with pytest.raises(DeviceNotConnectedError):
@@ -179,12 +187,12 @@ def test_async_read_before_connect():
Cv2Rotation.ROTATE_180,
Cv2Rotation.ROTATE_270,
],
ids=["no_rot", "rot90", "rot180", "rot270"],
)
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file)
def test_all_rotations(mock_enable_device, rotation):
config = RealSenseCameraConfig(serial_number=42, rotation=rotation)
def test_rotation(rotation):
config = RealSenseCameraConfig(serial_number_or_name=42, rotation=rotation)
camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False)
camera.connect(warmup=False)
img = camera.read()
assert isinstance(img, np.ndarray)

137
tests/mocks/mock_motors_bus.py Normal file
View File

@@ -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): ...

140
tests/motors/test_motors_bus.py
View File

@@ -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

95
tests/robots/test_so100_follower.py Normal file
View File

@@ -0,0 +1,95 @@
from contextlib import contextmanager
from unittest.mock import MagicMock, patch
import pytest
from lerobot.common.robots.so100_follower import (
SO100Follower,
SO100FollowerConfig,
)
def _make_bus_mock() -> MagicMock:
"""Return a bus mock with just the attributes used by the robot."""
bus = MagicMock(name="FeetechBusMock")
bus.is_connected = False
def _connect():
bus.is_connected = True
def _disconnect(_disable=True):
bus.is_connected = False
bus.connect.side_effect = _connect
bus.disconnect.side_effect = _disconnect
@contextmanager
def _dummy_cm():
yield
bus.torque_disabled.side_effect = _dummy_cm
return bus
@pytest.fixture
def follower():
bus_mock = _make_bus_mock()
def _bus_side_effect(*_args, **kwargs):
bus_mock.motors = kwargs["motors"]
motors_order: list[str] = list(bus_mock.motors)
bus_mock.sync_read.return_value = {motor: idx for idx, motor in enumerate(motors_order, 1)}
bus_mock.sync_write.return_value = None
bus_mock.write.return_value = None
bus_mock.disable_torque.return_value = None
bus_mock.enable_torque.return_value = None
bus_mock.is_calibrated = True
return bus_mock
with (
patch(
"lerobot.common.robots.so100_follower.so100_follower.FeetechMotorsBus",
side_effect=_bus_side_effect,
),
patch.object(SO100Follower, "configure", lambda self: None),
):
cfg = SO100FollowerConfig(port="/dev/null")
robot = SO100Follower(cfg)
yield robot
if robot.is_connected:
robot.disconnect()
def test_connect_disconnect(follower):
assert not follower.is_connected
follower.connect()
assert follower.is_connected
follower.disconnect()
assert not follower.is_connected
def test_get_observation(follower):
follower.connect()
obs = follower.get_observation()
expected_keys = {f"{m}.pos" for m in follower.bus.motors}
assert set(obs.keys()) == expected_keys
for idx, motor in enumerate(follower.bus.motors, 1):
assert obs[f"{motor}.pos"] == idx
def test_send_action(follower):
follower.connect()
action = {f"{m}.pos": i * 10 for i, m in enumerate(follower.bus.motors, 1)}
returned = follower.send_action(action)
assert returned == action
goal_pos = {m: (i + 1) * 10 for i, m in enumerate(follower.bus.motors)}
follower.bus.sync_write.assert_called_once_with("Goal_Position", goal_pos)

2
tests/utils.py
View File

@@ -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)