From 6889c13fe5b8c18d460c62cef08e6763ebb3d161 Mon Sep 17 00:00:00 2001 From: zookiart Date: Wed, 24 Jun 2026 21:46:00 -0700 Subject: [PATCH] Force-release camera/serial resources when a device disconnect fails MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit When LeRobot's device.disconnect() throws (a flaky USB camera or serial port), the underlying handle can stay open — which keeps the camera busy and blocks the next teleop/record/calibrate run from acquiring it. Add safe_disconnect_device(): attempt the normal disconnect, and on failure best-effort force-close the serial/camera resources so the device is actually released. Wire it into teleoperation's teardown; other call sites (calibration, recording) can adopt the same helper in follow-ups. Tests in tests/test_devices.py cover the success path and the disconnect-fails-then-force-release path. Co-Authored-By: Claude Opus 4.8 --- lelab/teleoperate.py | 8 ++--- lelab/utils/devices.py | 51 ++++++++++++++++++++++++++++++++ tests/test_devices.py | 67 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 120 insertions(+), 6 deletions(-) create mode 100644 lelab/utils/devices.py create mode 100644 tests/test_devices.py diff --git a/lelab/teleoperate.py b/lelab/teleoperate.py index b666bde..6341680 100644 --- a/lelab/teleoperate.py +++ b/lelab/teleoperate.py @@ -24,6 +24,7 @@ from lerobot.teleoperators.so_leader import SO101Leader, SO101LeaderConfig from .utils.config import setup_calibration_files +from .utils.devices import safe_disconnect_device logger = logging.getLogger(__name__) @@ -128,12 +129,7 @@ def _safe_disconnect(device) -> None: Used on the connection-failure cleanup path so one device's failure can't leave the other holding its serial port open. """ - if device is None: - return - try: - device.disconnect() - except Exception as e: - logger.warning(f"Error disconnecting device during cleanup: {e}") + safe_disconnect_device(device, logger) def handle_start_teleoperation(request: TeleoperateRequest, websocket_manager=None) -> dict[str, Any]: diff --git a/lelab/utils/devices.py b/lelab/utils/devices.py new file mode 100644 index 0000000..3b94974 --- /dev/null +++ b/lelab/utils/devices.py @@ -0,0 +1,51 @@ +"""Device cleanup helpers for LeRobot hardware wrappers. + +Serial ports are a trust boundary on Windows: if a normal disconnect fails +while disabling torque, the COM handle can stay open until the Python process +exits. These helpers preserve LeRobot's normal disconnect behavior, then force +close the underlying port/cameras as a last resort. +""" + +from __future__ import annotations + +import logging +from contextlib import suppress +from typing import Any + + +def safe_disconnect_device(device: Any, logger: logging.Logger, context: str = "cleanup") -> None: + """Disconnect a LeRobot device and force-release resources on failure.""" + if device is None: + return + + try: + device.disconnect() + return + except Exception as exc: + logger.warning("Error disconnecting device during %s: %s", context, exc) + + _force_close_device_resources(device, logger) + + +def _force_close_device_resources(device: Any, logger: logging.Logger) -> None: + """Best-effort release for serial/camera resources after disconnect fails.""" + bus = getattr(device, "bus", None) + port_handler = getattr(bus, "port_handler", None) + if port_handler is not None: + with suppress(Exception): + port_handler.clearPort() + with suppress(Exception): + port_handler.is_using = False + try: + port_handler.closePort() + logger.info("Force-closed serial port after disconnect failure") + except Exception as exc: + logger.warning("Failed to force-close serial port after disconnect failure: %s", exc) + + cameras = getattr(device, "cameras", None) + if isinstance(cameras, dict): + for cam in cameras.values(): + try: + cam.disconnect() + except Exception as exc: + logger.warning("Failed to disconnect camera after device cleanup failure: %s", exc) diff --git a/tests/test_devices.py b/tests/test_devices.py new file mode 100644 index 0000000..eab3e20 --- /dev/null +++ b/tests/test_devices.py @@ -0,0 +1,67 @@ +from __future__ import annotations + +import logging + + +def test_safe_disconnect_force_closes_serial_port_after_disconnect_failure() -> None: + from lelab.utils.devices import safe_disconnect_device + + class PortHandler: + def __init__(self) -> None: + self.cleared = False + self.closed = False + self.is_using = True + + def clearPort(self) -> None: # noqa: N802 - mirrors LeRobot port handler API + self.cleared = True + + def closePort(self) -> None: # noqa: N802 - mirrors LeRobot port handler API + self.closed = True + + class Camera: + def __init__(self) -> None: + self.disconnected = False + + def disconnect(self) -> None: + self.disconnected = True + + class Device: + def __init__(self) -> None: + self.bus = type("Bus", (), {"port_handler": PortHandler()})() + self.cameras = {"cam": Camera()} + + def disconnect(self) -> None: + raise RuntimeError("Failed to write 'Torque_Enable' on id_=6") + + device = Device() + safe_disconnect_device(device, logging.getLogger(__name__)) + + assert device.bus.port_handler.cleared is True + assert device.bus.port_handler.is_using is False + assert device.bus.port_handler.closed is True + assert device.cameras["cam"].disconnected is True + + +def test_safe_disconnect_uses_normal_disconnect_when_it_succeeds() -> None: + from lelab.utils.devices import safe_disconnect_device + + class PortHandler: + def __init__(self) -> None: + self.closed = False + + def closePort(self) -> None: # noqa: N802 - mirrors LeRobot port handler API + self.closed = True + + class Device: + def __init__(self) -> None: + self.bus = type("Bus", (), {"port_handler": PortHandler()})() + self.disconnected = False + + def disconnect(self) -> None: + self.disconnected = True + + device = Device() + safe_disconnect_device(device, logging.getLogger(__name__)) + + assert device.disconnected is True + assert device.bus.port_handler.closed is False