From 18e7232f889751286326a0439f07c9f1f940c508 Mon Sep 17 00:00:00 2001
From: glannuzel <gaelle.lannuzel@gmail.com>
Date: Tue, 29 Jul 2025 14:07:26 +0200
Subject: [PATCH] Add teleop_left camera to observation

---
 .../robots/reachy2/configuration_reachy2.py   | 33 +++++++------
 src/lerobot/robots/reachy2/robot_reachy2.py   | 48 ++++++++++++-------
 src/lerobot/robots/reachy2/test_recording.py  |  4 +-
 3 files changed, 52 insertions(+), 33 deletions(-)

diff --git a/src/lerobot/robots/reachy2/configuration_reachy2.py b/src/lerobot/robots/reachy2/configuration_reachy2.py
index 7be8e89f9..fe948fd1d 100644
--- a/src/lerobot/robots/reachy2/configuration_reachy2.py
+++ b/src/lerobot/robots/reachy2/configuration_reachy2.py
@@ -15,7 +15,8 @@
 from dataclasses import dataclass, field
 
 from lerobot.cameras import CameraConfig
-from lerobot.cameras.opencv import OpenCVCameraConfig
+# from lerobot.cameras.opencv import OpenCVCameraConfig
+from lerobot.cameras.reachy2_camera import Reachy2CameraConfig
 from lerobot.cameras.configs import ColorMode, Cv2Rotation
 
 from ..config import RobotConfig
@@ -33,19 +34,23 @@ class Reachy2RobotConfig(RobotConfig):
     # cameras
     cameras: dict[str, CameraConfig] = field(
         default_factory=lambda: {
-            # "head": RealSenseCameraConfig(
-            #     name="Intel RealSense D435I",
-            #     fps=30,
-            #     width=640,
-            #     height=480,
-            #     rotation=90,
-            # ),
-            # "wrist": RealSenseCameraConfig(
-            #     name="Intel RealSense D405",
-            #     fps=30,
-            #     width=640,
-            #     height=480,
-            # ),
+        #     "webcam": OpenCVCameraConfig(
+        #         index_or_path="/dev/video0",
+        #         fps=30,
+        #         width=640,
+        #         height=480,
+        #         color_mode=ColorMode.RGB,
+        #         rotation=Cv2Rotation.NO_ROTATION
+        #     ),
+        "teleop_left": Reachy2CameraConfig(
+                name="teleop",
+                image_type="left",
+                fps=30,
+                width=640,
+                height=480,
+                color_mode=ColorMode.RGB,
+                rotation=Cv2Rotation.NO_ROTATION
+            ),
         }
     )
 
diff --git a/src/lerobot/robots/reachy2/robot_reachy2.py b/src/lerobot/robots/reachy2/robot_reachy2.py
index 5f75bb841..932bacc9b 100644
--- a/src/lerobot/robots/reachy2/robot_reachy2.py
+++ b/src/lerobot/robots/reachy2/robot_reachy2.py
@@ -80,10 +80,11 @@ class Reachy2Robot(Robot):
 
     @property
     def observation_features(self) -> dict:
-        return dict.fromkeys(
-            REACHY2_MOTORS.keys(),
-            float,
-        )
+        return {**self.motors_features, **self.camera_features}
+        # return dict.fromkeys(
+        #     REACHY2_MOTORS.keys(),
+        #     float,
+        # )
         # return {
         #     "dtype": "float32",
         #     "shape": len(REACHY2_MOTORS),
@@ -92,18 +93,28 @@ class Reachy2Robot(Robot):
 
     @property
     def action_features(self) -> dict:
-        return self.observation_features
+        return self.motors_features
 
     @property
     def camera_features(self) -> dict[str, dict]:
-        cam_ft = {}
-        for cam_key, cam in self.cameras.items():
-            cam_ft[cam_key] = {
-                "shape": (cam.height, cam.width, cam.channels),
-                "names": ["height", "width", "channels"],
-                "info": None,
-            }
-        return cam_ft
+        return {
+            cam: (self.cameras[cam].height, self.cameras[cam].width, 3) for cam in self.cameras
+        }
+        # cam_ft = {}
+        # for cam_key, cam in self.cameras.items():
+        #     cam_ft[cam_key] = {
+        #         "shape": (cam.height, cam.width, cam.channels),
+        #         "names": ["height", "width", "channels"],
+        #         "info": None,
+        #     }
+        # return cam_ft
+
+    @property
+    def motors_features(self) -> dict:
+        return dict.fromkeys(
+            REACHY2_MOTORS.keys(),
+            float,
+        )
 
     @property
     def is_connected(self) -> bool:
@@ -115,8 +126,8 @@ class Reachy2Robot(Robot):
             print("Error connecting to Reachy 2.")
             raise ConnectionError()
 
-        # for cam in self.cameras.values():
-        #     cam.connect()
+        for cam in self.cameras.values():
+            cam.connect()
 
         # if not self.is_connected:
         #     print("Could not connect to the cameras, check that all cameras are plugged-in.")
@@ -149,6 +160,9 @@ class Reachy2Robot(Robot):
         # state = np.asarray(list(state.values()))
         # obs_dict[OBS_STATE] = state
 
+        for cam_key, cam in self.cameras.items():
+            obs_dict[cam_key] = cam.async_read()
+
         # Capture images from cameras
         # for cam_key, cam in self.cameras.items():
             # before_camread_t = time.perf_counter()
@@ -178,5 +192,5 @@ class Reachy2Robot(Robot):
     def disconnect(self) -> None:
         self.reachy.turn_off_smoothly()
         self.reachy.disconnect()
-        # for cam in self.cameras.values():
-        #     cam.disconnect()
+        for cam in self.cameras.values():
+            cam.disconnect()
diff --git a/src/lerobot/robots/reachy2/test_recording.py b/src/lerobot/robots/reachy2/test_recording.py
index c4bd3b053..1da9e5f93 100644
--- a/src/lerobot/robots/reachy2/test_recording.py
+++ b/src/lerobot/robots/reachy2/test_recording.py
@@ -34,8 +34,8 @@ dataset = LeRobotDataset.create(
     fps=FPS,
     features=dataset_features,
     robot_type=robot.name,
-    # use_videos=True,
-    # image_writer_threads=4,
+    use_videos=True,
+    image_writer_threads=4,
 )
 
 # Initialize the keyboard listener and rerun visualization