fix(unitree_g1): make camera server robust and release devices cleanly

Add open retries and a longer warmup in ImageServer, and expose a stop()
that releases the V4L2 devices. run_g1_server now stops the camera server
on shutdown so cameras don't stay wedged after Ctrl-C.

Co-authored-by: Cursor <cursoragent@cursor.com>

src/lerobot/cameras/zmq/image_server.py

@@ -92,9 +92,21 @@ class ImageServer:
     def __init__(self, config: dict, port: int = 5555):
         # fps controls the publish loop rate (how often frames are sent over ZMQ), not the camera capture rate
         self.fps = config.get("fps", 30)
+        # First-frame warmup: UVC cameras (Arducam/RealSense) can take >1s to deliver
+        # their first frame, especially with several sharing a USB bus. A tight timeout
+        # aborts the launch and leaves the device wedged (no STREAMOFF), so give it room.
+        self.warmup_s = config.get("warmup_s", 5)
+        # Flaky USB cameras (RealSense especially) intermittently fail the first
+        # open or first-frame read; retry a few times before giving up.
+        self.open_attempts = config.get("open_attempts", 5)
+        self.open_retry_delay_s = config.get("open_retry_delay_s", 2.0)
         self.cameras: dict[str, OpenCVCamera] = {}
         self.capture_threads: dict[str, CameraCaptureThread] = {}
+        self._stop = threading.Event()
 
+        # If any camera fails to open, release the ones we already opened so the V4L2
+        # devices get a clean STREAMOFF instead of staying busy until the next reboot.
+        try:
             for name, cfg in config.get("cameras", {}).items():
                 shape = cfg.get("shape", [480, 640])
                 cam_kwargs = {
@@ -103,6 +115,7 @@ class ImageServer:
                     "width": shape[1],
                     "height": shape[0],
                     "color_mode": ColorMode.RGB,
+                    "warmup_s": self.warmup_s,
                     # Force V4L2 (Linux): the default FFMPEG backend is read-only for capture
                     # props, so it can't set FOURCC/resolution (e.g. RealSense color nodes).
                     "backend": Cv2Backends.V4L2,
@@ -113,13 +126,38 @@ class ImageServer:
                     cam_kwargs["fourcc"] = cfg["fourcc"]
                 cam_config = OpenCVCameraConfig(**cam_kwargs)
                 camera = OpenCVCamera(cam_config)
+
+                last_err: Exception | None = None
+                for attempt in range(1, self.open_attempts + 1):
+                    try:
                         camera.connect()
+                        last_err = None
+                        break
+                    except Exception as e:  # noqa: BLE001
+                        last_err = e
+                        logger.warning(
+                            "Camera %s open attempt %d/%d failed: %s",
+                            name, attempt, self.open_attempts, e,
+                        )
+                        with contextlib.suppress(Exception):
+                            camera.disconnect()
+                        if attempt < self.open_attempts:
+                            time.sleep(self.open_retry_delay_s)
+                if last_err is not None:
+                    raise RuntimeError(
+                        f"Camera {name} failed to open after {self.open_attempts} attempts"
+                    ) from last_err
+
                 self.cameras[name] = camera
                 logger.info(f"Camera {name}: {shape[1]}x{shape[0]}")
 
                 # Create capture thread for this camera
                 capture_thread = CameraCaptureThread(camera, name)
                 self.capture_threads[name] = capture_thread
+        except Exception:
+            logger.exception("Failed to open cameras; releasing any already-opened devices.")
+            self._release_cameras()
+            raise
 
         # ZMQ PUB socket
         self.context = zmq.Context()
@@ -130,6 +168,28 @@ class ImageServer:
 
         logger.info(f"ImageServer running on port {port}")
 
+    def stop(self) -> None:
+        """Signal the publish loop to exit so ``run()`` reaches its cleanup.
+
+        Call this from the owning process on shutdown (e.g. Ctrl-C) — otherwise a
+        daemon thread running ``run()`` is killed abruptly and the cameras never get
+        released, leaving the V4L2 devices wedged until reboot.
+        """
+        self._stop.set()
+
+    def _release_cameras(self) -> None:
+        """Stop capture threads and disconnect all cameras (safe to call twice).
+
+        Ensures each V4L2 device gets a clean STREAMOFF/release so a failed or
+        interrupted run doesn't leave devices busy until the next reboot.
+        """
+        for capture_thread in self.capture_threads.values():
+            with contextlib.suppress(Exception):
+                capture_thread.stop()
+        for cam in self.cameras.values():
+            with contextlib.suppress(Exception):
+                cam.disconnect()
+
     def run(self):
         frame_count = 0
         frame_times = deque(maxlen=60)
@@ -141,12 +201,12 @@ class ImageServer:
         # Wait for first frames to be captured and encoded
         logger.info("Waiting for cameras to start capturing...")
         for name, capture_thread in self.capture_threads.items():
-            while capture_thread.get_latest()[0] is None:
+            while capture_thread.get_latest()[0] is None and not self._stop.is_set():
                 time.sleep(0.01)
             logger.info(f"Camera {name} ready (capture + encode in background)")
 
         try:
-            while True:
+            while not self._stop.is_set():
                 t0 = time.time()
 
                 # Build message. Always include EVERY camera's latest frame so each message
@@ -177,10 +237,7 @@ class ImageServer:
         except KeyboardInterrupt:
             pass
         finally:
-            for capture_thread in self.capture_threads.values():
-                capture_thread.stop()
-            for cam in self.cameras.values():
-                cam.disconnect()
+            self._release_cameras()
             self.socket.close()
             self.context.term()
 

src/lerobot/robots/unitree_g1/run_g1_server.py

@@ -321,6 +321,7 @@ def main() -> None:
 
     # Optionally start camera server in background thread
     camera_thread = None
+    camera_server = None
     if args.camera or args.cameras:
         if args.cameras:
             cameras = parse_camera_specs(args.cameras, args.camera_width, args.camera_height)
@@ -437,12 +438,16 @@ def main() -> None:
         print("shutting down bridge...")
     finally:
         shutdown_event.set()
+        # Stop the camera server first so it releases the V4L2 devices cleanly;
+        # otherwise the daemon thread is killed on exit and the cameras stay wedged.
+        if camera_server is not None:
+            camera_server.stop()
         ctx.term()  # terminates blocking zmq.recv() calls
         t_state.join(timeout=2.0)
         if t_gripper is not None:
             t_gripper.join(timeout=2.0)
         if camera_thread is not None:
-            camera_thread.join(timeout=2.0)
+            camera_thread.join(timeout=3.0)
         for g in grippers.values():
             try:
                 g.bus.disconnect(disable_torque=True)