cameras seems to work in Gazebo. Cleaner disconnect

lerobot/common/robot_devices/cameras/reachy2.py

@@ -0,0 +1,77 @@
+"""
+Wrapper for Reachy2 camera from sdk
+"""
+
+import argparse
+import concurrent.futures
+import math
+import platform
+import shutil
+import threading
+import time
+from dataclasses import dataclass, replace
+from pathlib import Path
+from threading import Thread
+
+import numpy as np
+from PIL import Image
+
+from lerobot.common.robot_devices.utils import (
+    RobotDeviceAlreadyConnectedError,
+    RobotDeviceNotConnectedError,
+    busy_wait,
+)
+from lerobot.common.utils.utils import capture_timestamp_utc
+
+from reachy2_sdk.media.camera import CameraView
+from reachy2_sdk.media.camera_manager import CameraManager
+
+
+@dataclass
+class ReachyCameraConfig:
+
+    fps: int | None = None
+    width: int | None = None
+    height: int | None = None
+    color_mode: str = "rgb"
+    rotation: int | None = None
+    mock: bool = False
+
+
+class ReachyCamera:
+    def __init__(self, host: str, port: int,name: str, image_type: str, config: ReachyCameraConfig | None = None, **kwargs):
+        self.host=host
+        self.port=port
+        self.image_type = image_type
+        self.name = name
+        self.config = config
+        self.cam_manager=None
+        self.is_connected = False
+        self.logs = {}
+
+    def connect(self):
+        if not self.is_connected:
+            self.cam_manager=CameraManager(host=self.host, port=self.port)
+            self.cam_manager.initialize_cameras()#FIXME: maybe we should not re-initialize
+            self.is_connected=True
+
+    def read(self) -> np.ndarray:
+
+        if not self.is_connected:
+            self.connect()
+
+        if self.name=='teleop' and hasattr(self.cam_manager,'teleop'):
+            if self.image_type=='left':
+                return self.cam_manager.teleop.get_frame(CameraView.LEFT)
+            elif self.image_type=='right':
+                return self.cam_manager.teleop.get_frame(CameraView.RIGHT)
+            else:
+                return None
+        elif self.name=='depth' and hasattr(self.cam_manager,'depth'):
+
+            if self.image_type=='depth':
+                return self.cam_manager.depth.get_depth_frame()
+            elif self.image_type=='rgb':
+                return self.cam_manager.depth.get_frame()
+            else:
+                return None

lerobot/common/robot_devices/robots/reachy2.py

@@ -45,16 +45,17 @@ class ReachyRobot():
 
         self.robot_type = self.config.robot_type
         self.cameras = self.config.cameras
-        self.is_connected = False #at init Reachy2 is in fact connected...
+        self.is_connected = False
         self.teleop = None
         self.logs = {}
         self.reachy=ReachySDK(host=config.ip_address)
-
+        self.is_connected=True  #at init Reachy2 is in fact connected...
 
         self.state_keys = None
         self.action_keys = None
 
     def connect(self) -> None:
+        print("Connecting to Reachy")
         self.reachy.is_connected = self.reachy.connect()
         if not self.is_connected:
             print(f'Cannot connect to Reachy at address {self.config.ip_address}. Maybe a connection already exists.')
@@ -81,6 +82,7 @@ class ReachyRobot():
 
     def get_state(self) -> dict:
 
+        if self.is_connected:
             return {
                 "neck_yaw.pos": np.radians(self.reachy.head.neck.yaw.present_position),
                 "neck_pitch.pos": np.radians(self.reachy.head.neck.pitch.present_position),
@@ -103,9 +105,13 @@ class ReachyRobot():
                 "l_gripper.pos": np.radians(self.reachy.l_arm.gripper.present_position),
                 #TODO mobile base
             }
+        else:
+            return {}
 
     def capture_observation(self) -> dict:
 
+        if self.is_connected:
+
             before_read_t = time.perf_counter()
             state = self.get_state()
             self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
@@ -120,7 +126,7 @@ class ReachyRobot():
             for name in self.cameras:
                 before_camread_t = time.perf_counter()
                 images[name] = self.cameras[name].read() #Reachy cameras read() is not blocking?
-            print(f'name: {name} img: {images[name]}')
+                # print(f'name: {name} img: {images[name]}')
                 if images[name] is not None:
                     images[name] = torch.from_numpy(copy(images[name][0])) #seems like I need to copy?
                     self.logs[f"read_camera_{name}_dt_s"] = images[name][1] #full timestamp, TODO dt
@@ -132,13 +138,14 @@ class ReachyRobot():
                 obs_dict[f"observation.images.{name}"] = images[name]
 
             return obs_dict
-
+        else:
+            return {}
     def send_action(self, action: torch.Tensor) -> torch.Tensor:
 
         if not self.is_connected:
             raise ConnectionError()
 
-        #TODO
+        #TODO: what shape is the action tensor?
         return action
 
     def print_logs(self) -> None:
@@ -146,10 +153,10 @@ class ReachyRobot():
 
 
     def disconnect(self) -> None:
+        print("Disconnecting")
         self.is_connected = False
+        print("Turn off")
         self.reachy.turn_off_smoothly()
         # self.reachy.turn_off()
-        time.sleep(6)
+        print("\t turn off done")
-
+        self.reachy.disconnect()
-    def __del__(self):
-        self.disconnect()

lerobot/scripts/test_reachy.py

@@ -34,6 +34,8 @@ if __name__ == '__main__':
     robot_cfg = init_hydra_config(robot_path, robot_overrides)
     robot = make_robot(robot_cfg)
 
-    print(robot.get_state())
+    print(robot.is_connected)
+    # print(robot.get_state())
     print(robot.capture_observation())
-    time.sleep(6)
+    time.sleep(5)
+    robot.disconnect()