Skip to content

Commit 3736334

Browse files
Add realsense support
1 parent e824a57 commit 3736334

22 files changed

+4036
-40
lines changed

.gitignore

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -177,3 +177,4 @@ outputs/
177177
*.urdf
178178
*.xml
179179
*.part
180+
src/lerobot/cameras/tests/

pyproject.toml

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@ dependencies = [
6868
"setuptools>=71.0.0,<81.0.0",
6969
"cmake>=3.29.0.1,<4.2.0",
7070
"einops>=0.8.0,<0.9.0",
71-
"opencv-python-headless>=4.9.0,<4.13.0",
71+
"opencv-python>=4.9.0,<4.13.0",
7272
"av>=15.0.0,<16.0.0",
7373
"jsonlines>=4.0.0,<5.0.0",
7474
"packaging>=24.2,<26.0",
@@ -113,7 +113,7 @@ unitree_g1 = [
113113
]
114114
reachy2 = ["reachy2_sdk>=1.0.14,<1.1.0"]
115115
kinematics = ["lerobot[placo-dep]"]
116-
intelrealsense = [
116+
realsense = [
117117
"pyrealsense2>=2.55.1.6486,<2.57.0 ; sys_platform != 'darwin'",
118118
"pyrealsense2-macosx>=2.54,<2.55.0 ; sys_platform == 'darwin'",
119119
]
@@ -157,7 +157,7 @@ all = [
157157
"lerobot[lekiwi]",
158158
"lerobot[reachy2]",
159159
"lerobot[kinematics]",
160-
"lerobot[intelrealsense]",
160+
"lerobot[realsense]",
161161
"lerobot[pi]",
162162
"lerobot[smolvla]",
163163
# "lerobot[groot]", TODO(Steven): Gr00t requires specific installation instructions for flash-attn

requirements-macos.txt

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -400,7 +400,7 @@ numpy==2.2.6
400400
# mujoco
401401
# numba
402402
# opencv-python
403-
# opencv-python-headless
403+
# opencv-python
404404
# pandas
405405
# peft
406406
# pyquaternion
@@ -423,11 +423,10 @@ omegaconf==2.3.0
423423
opencv-python==4.12.0.88
424424
# via
425425
# gym-pusht
426+
# lerobot
426427
# libero
427428
# reachy2-sdk
428429
# robosuite
429-
opencv-python-headless==4.12.0.88
430-
# via lerobot
431430
orderly-set==5.5.0
432431
# via deepdiff
433432
packaging==25.0

requirements-ubuntu.txt

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -408,7 +408,7 @@ numpy==2.2.6
408408
# mujoco
409409
# numba
410410
# opencv-python
411-
# opencv-python-headless
411+
# opencv-python
412412
# pandas
413413
# peft
414414
# pyquaternion
@@ -468,11 +468,10 @@ omegaconf==2.3.0
468468
opencv-python==4.12.0.88
469469
# via
470470
# gym-pusht
471+
# lerobot
471472
# libero
472473
# reachy2-sdk
473474
# robosuite
474-
opencv-python-headless==4.12.0.88
475-
# via lerobot
476475
orderly-set==5.5.0
477476
# via deepdiff
478477
packaging==25.0

src/lerobot/async_inference/policy_server.py

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -226,6 +226,8 @@ def GetActions(self, request, context): # noqa: N802
226226
self.logger.info(
227227
f"Running inference for observation #{obs.get_timestep()} (must_go: {obs.must_go})"
228228
)
229+
self.logger.info(f"Policy image feature keys: {sorted(self.policy_image_features.keys())}")
230+
self.logger.info(f"Incoming raw observation keys: {sorted(obs.get_observation().keys())}")
229231

230232
with self._predicted_timesteps_lock:
231233
self._predicted_timesteps.add(obs.get_timestep())

src/lerobot/async_inference/robot_client.py

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,14 @@
4848

4949
from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig # noqa: F401
5050
from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401
51+
from lerobot.cameras.realsense.configuration_rs_d435i import ( # noqa: F401
52+
RealSenseD435iColorCameraConfig,
53+
RealSenseD435iDepthCameraConfig,
54+
)
55+
from lerobot.cameras.realsense.configuration_rs_d405 import ( # noqa: F401
56+
RealSenseD405ColorCameraConfig,
57+
RealSenseD405DepthCameraConfig,
58+
)
5159
from lerobot.robots import ( # noqa: F401
5260
Robot,
5361
RobotConfig,
@@ -396,6 +404,7 @@ def control_loop_observation(self, task: str, verbose: bool = False) -> RawObser
396404

397405
raw_observation: RawObservation = self.robot.get_observation()
398406
raw_observation["task"] = task
407+
self.logger.info(f"Raw observation keys: {sorted(raw_observation.keys())}")
399408

400409
with self.latest_action_lock:
401410
latest_action = self.latest_action

src/lerobot/cameras/opencv/camera_opencv.py

Lines changed: 72 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -101,7 +101,12 @@ class OpenCVCamera(Camera):
101101
```
102102
"""
103103

104-
def __init__(self, config: OpenCVCameraConfig):
104+
def __init__(
105+
self,
106+
config: OpenCVCameraConfig,
107+
source_camera: "OpenCVCamera | None" = None,
108+
source_key: str | None = None,
109+
):
105110
"""
106111
Initializes the OpenCVCamera instance.
107112
@@ -112,6 +117,10 @@ def __init__(self, config: OpenCVCameraConfig):
112117

113118
self.config = config
114119
self.index_or_path = config.index_or_path
120+
self.source_camera = source_camera
121+
self.source_key = source_key
122+
self.copy = config.copy
123+
self._copy_connected = False
115124

116125
self.fps = config.fps
117126
self.color_mode = config.color_mode
@@ -133,12 +142,40 @@ def __init__(self, config: OpenCVCameraConfig):
133142
if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
134143
self.capture_width, self.capture_height = self.height, self.width
135144

145+
def _validate_copy_configuration(self) -> None:
146+
if self.source_camera is None:
147+
raise ValueError(f"{self} copy source is not configured.")
148+
149+
config_mismatches = []
150+
if self.width != self.source_camera.width:
151+
config_mismatches.append(f"width={self.width} != {self.source_camera.width}")
152+
if self.height != self.source_camera.height:
153+
config_mismatches.append(f"height={self.height} != {self.source_camera.height}")
154+
if self.color_mode != self.source_camera.color_mode:
155+
config_mismatches.append(f"color_mode={self.color_mode} != {self.source_camera.color_mode}")
156+
if self.rotation != self.source_camera.rotation:
157+
config_mismatches.append(f"rotation={self.rotation} != {self.source_camera.rotation}")
158+
159+
if config_mismatches:
160+
raise ValueError(
161+
f"{self} copy configuration must match source camera {self.source_key}: "
162+
+ ", ".join(config_mismatches)
163+
)
164+
136165
def __str__(self) -> str:
166+
if self.is_copy_camera and self.source_key is not None:
167+
return f"{self.__class__.__name__}(copy:{self.source_key})"
137168
return f"{self.__class__.__name__}({self.index_or_path})"
138169

170+
@property
171+
def is_copy_camera(self) -> bool:
172+
return self.source_camera is not None
173+
139174
@property
140175
def is_connected(self) -> bool:
141176
"""Checks if the camera is currently connected and opened."""
177+
if self.is_copy_camera:
178+
return self._copy_connected
142179
return isinstance(self.videocapture, cv2.VideoCapture) and self.videocapture.isOpened()
143180

144181
def connect(self, warmup: bool = True) -> None:
@@ -156,6 +193,14 @@ def connect(self, warmup: bool = True) -> None:
156193
if self.is_connected:
157194
raise DeviceAlreadyConnectedError(f"{self} is already connected.")
158195

196+
if self.is_copy_camera:
197+
self._validate_copy_configuration()
198+
if self.source_camera is None or not self.source_camera.is_connected:
199+
raise ConnectionError(f"{self} requires source camera {self.source_key} to be connected first.")
200+
self._copy_connected = True
201+
logger.info(f"{self} connected.")
202+
return
203+
159204
# Use 1 thread for OpenCV operations to avoid potential conflicts or
160205
# blocking in multi-threaded applications, especially during data collection.
161206
cv2.setNumThreads(1)
@@ -362,6 +407,9 @@ def read(self, color_mode: ColorMode | None = None) -> NDArray[Any]:
362407
received frame dimensions don't match expectations before rotation.
363408
ValueError: If an invalid `color_mode` is requested.
364409
"""
410+
if self.is_copy_camera:
411+
return self._read_from_source(timeout_ms=max(self.warmup_s * 1000, 200), color_mode=color_mode)
412+
365413
if not self.is_connected:
366414
raise DeviceNotConnectedError(f"{self} is not connected.")
367415

@@ -382,6 +430,19 @@ def read(self, color_mode: ColorMode | None = None) -> NDArray[Any]:
382430

383431
return processed_frame
384432

433+
def _read_from_source(self, timeout_ms: float, color_mode: ColorMode | None = None) -> NDArray[Any]:
434+
if not self.is_connected or self.source_camera is None or not self.source_camera.is_connected:
435+
raise DeviceNotConnectedError(f"{self} is not connected.")
436+
437+
if color_mode is not None and color_mode != self.color_mode:
438+
raise ValueError(
439+
f"{self} cannot override color_mode while copying from {self.source_key}. "
440+
"Match the copy camera config to the source camera config instead."
441+
)
442+
443+
frame = self.source_camera.async_read(timeout_ms=timeout_ms)
444+
return frame.copy()
445+
385446
def _postprocess_image(self, image: NDArray[Any], color_mode: ColorMode | None = None) -> NDArray[Any]:
386447
"""
387448
Applies color conversion, dimension validation, and rotation to a raw frame.
@@ -496,6 +557,9 @@ def async_read(self, timeout_ms: float = 200) -> NDArray[Any]:
496557
TimeoutError: If no frame becomes available within the specified timeout.
497558
RuntimeError: If an unexpected error occurs.
498559
"""
560+
if self.is_copy_camera:
561+
return self._read_from_source(timeout_ms=timeout_ms)
562+
499563
if not self.is_connected:
500564
raise DeviceNotConnectedError(f"{self} is not connected.")
501565

@@ -528,6 +592,13 @@ def disconnect(self) -> None:
528592
Raises:
529593
DeviceNotConnectedError: If the camera is already disconnected.
530594
"""
595+
if self.is_copy_camera:
596+
if not self._copy_connected:
597+
raise DeviceNotConnectedError(f"{self} not connected.")
598+
self._copy_connected = False
599+
logger.info(f"{self} disconnected.")
600+
return
601+
531602
if not self.is_connected and self.thread is None:
532603
raise DeviceNotConnectedError(f"{self} not connected.")
533604

src/lerobot/cameras/opencv/configuration_opencv.py

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,9 @@ class OpenCVCameraConfig(CameraConfig):
4343
Attributes:
4444
index_or_path: Either an integer representing the camera device index,
4545
or a Path object pointing to a video file.
46+
copy: Copy frames from another declared OpenCV camera instead of opening
47+
a second physical device. `False` disables copying, an integer `n`
48+
maps to `camera{n}`, and a string can directly name the source key.
4649
fps: Requested frames per second for the color stream.
4750
width: Requested frame width in pixels for the color stream.
4851
height: Requested frame height in pixels for the color stream.
@@ -58,12 +61,25 @@ class OpenCVCameraConfig(CameraConfig):
5861
"""
5962

6063
index_or_path: int | Path
64+
copy: bool | int | str = False
6165
color_mode: ColorMode = ColorMode.RGB
6266
rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION
6367
warmup_s: int = 1
6468
fourcc: str | None = None
6569

6670
def __post_init__(self) -> None:
71+
if isinstance(self.copy, bool):
72+
if self.copy:
73+
raise ValueError("`copy=True` is ambiguous. Use `copy: 2` or `copy: camera2` instead.")
74+
elif isinstance(self.copy, int):
75+
if self.copy < 1:
76+
raise ValueError(f"`copy` must be >= 1 when provided as an integer, but {self.copy} is provided.")
77+
elif isinstance(self.copy, str):
78+
if not self.copy.strip():
79+
raise ValueError("`copy` must not be empty when provided as a string.")
80+
else:
81+
raise ValueError(f"`copy` must be False, an integer, or a string, but {self.copy!r} is provided.")
82+
6783
if self.color_mode not in (ColorMode.RGB, ColorMode.BGR):
6884
raise ValueError(
6985
f"`color_mode` is expected to be {ColorMode.RGB.value} or {ColorMode.BGR.value}, but {self.color_mode} is provided."

src/lerobot/cameras/realsense/__init__.py

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,4 +13,11 @@
1313
# limitations under the License.
1414

1515
from .camera_realsense import RealSenseCamera
16+
from .camera_rs_d405 import RealSenseD405ColorCamera, RealSenseD405DepthCamera
17+
from .camera_rs_d435i import RealSenseD435iColorCamera, RealSenseD435iDepthCamera
1618
from .configuration_realsense import RealSenseCameraConfig
19+
from .configuration_rs_d405 import RealSenseD405ColorCameraConfig, RealSenseD405DepthCameraConfig
20+
from .configuration_rs_d435i import (
21+
RealSenseD435iColorCameraConfig,
22+
RealSenseD435iDepthCameraConfig,
23+
)

0 commit comments

Comments
 (0)