diff --git a/docker/Dockerfile b/docker/Dockerfile index f38fa759..2b03ad71 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,146 +1,60 @@ -# Base image with Python 3.10 and slim Debian system -FROM python:3.10-slim +FROM stereolabs/zed:5.2-gl-devel-cuda12.8-ubuntu24.04 +# FROM stereolabs/zed:5.0-gl-devel-cuda12.1-ubuntu22.04 -# System configuration ENV DEBIAN_FRONTEND=noninteractive \ + PIP_DISABLE_PIP_VERSION_CHECK=1 \ PYTHONDONTWRITEBYTECODE=1 \ PYTHONUNBUFFERED=1 \ - CMAKE_BUILD_PARALLEL_LEVEL=2 \ - SKBUILD_BUILD_OPTIONS="-j2" \ - MAKEFLAGS="-j2" -############################################################################# -# Explanation of environment variables: -# DEBIAN_FRONTEND=noninteractive: Disables interactive prompts during apt-get install. -# PYTHONDONTWRITEBYTECODE=1: Prevents .pyc files, keeping the image clean. -# PYTHONUNBUFFERED=1: Ensures logs are written directly (no buffering). -# Parallel build settings for CMake, scikit-build, and make to improve build speed. -############################################################################# + UV_LINK_MODE=copy -# create a user to avoid running as root -RUN useradd -ms /bin/bash devuser -WORKDIR /home/devuser -USER root +WORKDIR /opt/rcs-src -# Install system dependencies (from debian_deps.txt manually inlined here) RUN apt-get update && apt-get install -y --no-install-recommends \ build-essential \ + ca-certificates \ cmake \ + curl \ git \ - libpoco-dev \ - libeigen3-dev \ - libxslt-dev \ - libcoin-dev \ - libccd-dev \ + libgl1 \ + libglib2.0-0 \ libglfw3-dev \ - libboost-all-dev \ - liblzma-dev \ - libxml2-dev \ - libxslt1-dev \ + libpoco-dev \ ninja-build \ - clang \ - clang-format \ - clang-tidy \ - pkg-config \ - curl \ - unzip \ - wget \ - libgl1 \ - patchelf \ - python3-venv \ - libegl-dev \ - libegl1-mesa-dev \ - libglib2.0-dev \ - mesa-utils \ && rm -rf /var/lib/apt/lists/* -# Remove root password so `su -` works from devuser -RUN passwd -d root - - -# Switch to non-root user -USER devuser +RUN curl -LsSf https://astral.sh/uv/install.sh | sh -# Set up virtual environment (Python) -RUN python3 -m venv /home/devuser/.venv -# prepend /home/devuser/.venv/bin to the existing PATH -# This ensures that the virtual environment's Python and pip are used by default. -ENV PATH="/home/devuser/.venv/bin:$PATH" +ENV PATH="/opt/venv/bin:/root/.local/bin:${PATH}" \ + VIRTUAL_ENV=/opt/venv -# Copy project files into container -COPY --chown=devuser . /home/devuser/project -WORKDIR /home/devuser/project +RUN uv python install 3.11 \ + && uv venv --python 3.11 /opt/venv \ + && python -m ensurepip --upgrade -# Upgrade pip and install project build tools -RUN pip install --upgrade pip setuptools +# Install the ZED Python bindings into the same virtualenv used by the project. +RUN uv pip install requests \ + && python /usr/local/zed/get_python_api.py -# Install development dependencies -RUN pip install --group build_deps +COPY . /opt/rcs-src +COPY docker/link-editable-source.sh /usr/local/bin/link-editable-source -# Install the package in editable mode (CMake + pybind11 + scikit-build-core triggered) -RUN pip install -e . --no-cache-dir --verbose - -# Default command that runs when you start a container without specifying a command explicitly. -CMD ["python3"] - -###################################################################### -# Build the Docker image with specified memory limits -# To build the Docker image, run the following command in the terminal: -# docker build --memory=4g --memory-swap=6g . -t rcs-dev -###################################################################### -# --memory=4g Limit the build process to 4 GB of RAM -# --memory-swap=6g Limit total memory (RAM + swap) to 6 GB -# . Use current directory as the Docker context -# -t rcs-dev Tag the built image as "rcs-dev" -###################################################################### - -###################################################################### -# Run the Docker container interactively (without GUI) -# docker run -it --rm rcs-dev bash -###################################################################### -# -it Interactive mode with TTY -# --rm Automatically remove container after exit -# rcs-dev Name of the Docker image to run -# bash Start an interactive bash shell inside the container -###################################################################### - -###################################################################### -# Optional: Run GUI applications from inside the container -# First, allow X11 connections from Docker containers: -# Run this command on the host machine: -# xhost +local:docker -###################################################################### -# xhost A utility to manage X11 display access control -# +local:docker Grant X11 access to Docker containers running locally -###################################################################### - -###################################################################### -# Run container with GUI support (no GPU) -# docker run -it --rm -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix --shm-size=1g rcs-dev bash -###################################################################### -# -e DISPLAY=$DISPLAY Pass display info to container -# -v /tmp/.X11-unix:/tmp/.X11-unix Mount X11 socket for GUI apps -# --shm-size=1g Increase shared memory for rendering (useful for tools like MuJoCo) -###################################################################### - -###################################################################### -# Run container with NVIDIA GPU support -# Make sure NVIDIA Container Toolkit is installed and configured -# For more info: https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html -# docker run -it --rm --gpus all --runtime=nvidia -e NVIDIA_VISIBLE_DEVICES=all -e NVIDIA_DRIVER_CAPABILITIES=all -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix --shm-size=1g rcs-dev bash -###################################################################### -# --gpus all Enable all available GPUs -# --runtime=nvidia Use NVIDIA runtime for GPU access -# -e NVIDIA_VISIBLE_DEVICES=all Expose all GPUs inside container -# -e NVIDIA_DRIVER_CAPABILITIES=all Enable all GPU features (e.g., graphics, compute) -# Other flags same as GUI setup above -###################################################################### -# Run the container with NVIDIA GPU support and hardware access: -# docker run -it --rm --gpus all --runtime=nvidia -e NVIDIA_VISIBLE_DEVICES=all -e NVIDIA_DRIVER_CAPABILITIES=all -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix --shm-size=2g --network host --privileged --cap-add=SYS_NICE --ulimit rtprio=99 --ulimit rttime=-1 --ulimit memlock=8428281856 -v /dev:/dev rcs-dev bash -# Optional flags for running the container with hardware access: -# --network host \ # Use the host's network stack (needed for low-latency ROS comms) -# --privileged \ # Grant full device and kernel access (required for hardware control) -# --cap-add=SYS_NICE \ # Allow processes to raise their scheduling priority -# --ulimit rtprio=99 \ # Enable real-time priority up to 99 -# --ulimit rttime=-1 \ # Disable CPU time limit for real-time threads -# --ulimit memlock=8428281856 \ # Lock ~8GB of RAM to prevent memory swapping -# -v /dev:/dev \ # Mount all host devices for hardware access (e.g., Franka arm) +RUN chmod +x /usr/local/bin/link-editable-source \ + && uv pip install \ + build \ + wheel \ + "setuptools>=45" \ + "scikit-build-core>=0.3.3" \ + pybind11 \ + cmake \ + ninja \ + "mujoco==3.2.6" \ + "pin==3.7.0" \ + && uv pip install /opt/rcs-src \ + && uv pip install --no-build-isolation /opt/rcs-src/extensions/rcs_fr3 \ + && uv pip install /opt/rcs-src/extensions/rcs_realsense \ + && uv pip install /opt/rcs-src/extensions/rcs_robotiq2f85 \ + && uv pip install /opt/rcs-src/extensions/rcs_zed + +WORKDIR /workspace/robot-control-stack + +CMD ["sh"] diff --git a/docker/README.md b/docker/README.md index fd1d77a7..78bd0da4 100644 --- a/docker/README.md +++ b/docker/README.md @@ -1,24 +1,28 @@ -# Docker (GUI + GPU + HW add-ons) +# Docker -**Prereqs:** Docker + docker-compose, X11 on host, NVIDIA driver + NVIDIA Container Toolkit (legacy `runtime: nvidia`). -**Layout:** `docker/Dockerfile`, overrides in `docker/compose/` (`base.yml`, `gui.yml`, `gpu.yml`, `hw.yml`). +Build the image from the repository root: -## Build the image -`docker-compose -f docker/compose/base.yml build dev` +```sh +docker build -f docker/Dockerfile -t rcs-dev . +``` -## (GUI) allow X access (host) -`export XAUTHORITY=${XAUTHORITY:-$HOME/.Xauthority}` -`xhost +local:docker` +Run the development container with Docker Compose: -## Run container with GUI + GPU + HW and open a shell -`docker-compose -f docker/compose/base.yml -f docker/compose/gui.yml -f docker/compose/gpu.yml -f docker/compose/hw.yml run --rm run bash` -*(Use fewer `-f` files for lighter setups, e.g., GUI+GPU without HW.)* +```sh +xhost +si:localuser:root +docker compose -f docker/compose/dev.yml run --rm rcs +``` -## Inside the container -`pip install -ve extensions/rcs_fr3` -`cd examples` -`python fr3_env_cartesian_control.py` +Notes: -## Troubleshooting -- **`nvidia-smi` missing in container:** ensure it exists on host at `/usr/bin/nvidia-smi` (GPU override bind-mounts it). -- **GUI can’t open:** re-run the `xhost` command and confirm `$DISPLAY` is set on the host. \ No newline at end of file +- The compose setup bind-mounts the repository into `/workspace/robot-control-stack`. +- The compose service is tagged as `rcs-dev`, so the manual Docker build tag and the Compose service refer to the same image name. +- The Docker image installs the ZED Python API (`pyzed`) during build by running `/usr/local/zed/get_python_api.py` inside the project virtualenv. +- The compose setup requests GPU access using a device reservation, which is more widely supported than the newer service-level `gpus:` key. +- The host should grant local X11 access before starting the container: `xhost +si:localuser:root`. +- `~/zed_models` is mounted into `/usr/local/zed/resources` to match the direct `docker run` setup. +- `/dev/dri` is masked inside the container so host Mesa/AMD render nodes do not override the NVIDIA runtime devices. +- NVIDIA PRIME/GLX environment variables are exported to bias OpenGL/EGL selection toward the NVIDIA stack when using X11 forwarding. +- Python source changes are picked up from the mounted repo, including `extensions/rcs_zed`. +- If you change C++ code in `rcs` or `rcs_fr3`, rebuild the image. +- For non-GPU hosts, comment out the GPU-related lines in `docker/compose/dev.yml`. diff --git a/docker/compose/base.yml b/docker/compose/base.yml deleted file mode 100644 index a5c8a80b..00000000 --- a/docker/compose/base.yml +++ /dev/null @@ -1,38 +0,0 @@ -services: - # Build / dev: the ONLY service that mounts your source - dev: - build: - context: ${PWD} # run commands from the repo root - dockerfile: docker/Dockerfile - image: rcs-dev - user: root - tty: true - stdin_open: true - working_dir: /home/devuser/project - volumes: - - ${PWD}:/home/devuser/project - environment: - PYTHONUNBUFFERED: "1" - shm_size: "2g" - - # Runtime base: NO source mount here - run: - image: rcs-dev - user: root - tty: true - stdin_open: true - working_dir: /home/devuser/project - environment: - PYTHONUNBUFFERED: "1" - shm_size: "2g" - - -# Build the dev image -# docker-compose -f compose/base.yml build dev -# Run the dev container -# docker-compose \ -# -f compose/base.yml \ -# -f compose/gui.yml \ -# -f compose/gpu.yml \ -# -f compose/hw.yml \ -# run --rm run bash \ No newline at end of file diff --git a/docker/compose/dev.yml b/docker/compose/dev.yml new file mode 100644 index 00000000..0c470d34 --- /dev/null +++ b/docker/compose/dev.yml @@ -0,0 +1,38 @@ +services: + rcs: + image: rcs-dev + build: + context: ../.. + dockerfile: docker/Dockerfile + working_dir: /workspace/robot-control-stack + command: + - /bin/sh + - -lc + - /usr/local/bin/link-editable-source && exec /bin/sh + environment: + DISPLAY: ${DISPLAY} + RCS_PREFIX: /workspace/robot-control-stack + NVIDIA_DISABLE_REQUIRE: 1 + NVIDIA_VISIBLE_DEVICES: all + NVIDIA_DRIVER_CAPABILITIES: all + __NV_PRIME_RENDER_OFFLOAD: 1 + __GLX_VENDOR_LIBRARY_NAME: nvidia + __VK_LAYER_NV_optimus: NVIDIA_only + volumes: + - ../..:/workspace/robot-control-stack + - /tmp/.X11-unix:/tmp/.X11-unix + - /dev:/dev + - ${HOME}/zed_models:/usr/local/zed/resources + tmpfs: + - /dev/dri + network_mode: host + privileged: true + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: all + capabilities: [gpu] + stdin_open: true + tty: true diff --git a/docker/compose/gpu.yml b/docker/compose/gpu.yml deleted file mode 100644 index 5e727ce8..00000000 --- a/docker/compose/gpu.yml +++ /dev/null @@ -1,12 +0,0 @@ -version: "3.8" - -services: - run: - # Old-compose compatible GPU enablement - runtime: nvidia - privileged: true - network_mode: host - environment: - NVIDIA_VISIBLE_DEVICES: "all" - NVIDIA_DRIVER_CAPABILITIES: "all" - diff --git a/docker/compose/gui.yml b/docker/compose/gui.yml deleted file mode 100644 index bda9305f..00000000 --- a/docker/compose/gui.yml +++ /dev/null @@ -1,11 +0,0 @@ -version: "3.8" - -services: - run: - environment: - DISPLAY: ${DISPLAY} - XAUTHORITY: /tmp/.docker.xauth - QT_X11_NO_MITSHM: "1" - volumes: - - /tmp/.X11-unix:/tmp/.X11-unix:rw - - ${XAUTHORITY:-$HOME/.Xauthority}:/tmp/.docker.xauth:ro diff --git a/docker/compose/hw.yml b/docker/compose/hw.yml deleted file mode 100644 index 7c1de5b4..00000000 --- a/docker/compose/hw.yml +++ /dev/null @@ -1,20 +0,0 @@ -version: "3.8" - -services: - run: - privileged: true - volumes: - - /dev:/dev - cap_add: - - SYS_NICE - ulimits: - rtprio: 99 - rttime: -1 - memlock: - soft: 8428281856 - hard: 8428281856 - # If you prefer least-privilege instead of /dev:/dev, swap in specific devices: - # devices: - # - /dev/video0:/dev/video0 - # - /dev/ttyUSB0:/dev/ttyUSB0 - # - /dev/dri:/dev/dri diff --git a/docker/link-editable-source.sh b/docker/link-editable-source.sh new file mode 100644 index 00000000..0b1b967a --- /dev/null +++ b/docker/link-editable-source.sh @@ -0,0 +1,51 @@ +#!/bin/sh +set -eu + +REPO_ROOT="${1:-/workspace/robot-control-stack}" + +if [ ! -d "$REPO_ROOT" ]; then + echo "Mounted repo not found at $REPO_ROOT; leaving installed packages unchanged." + exit 0 +fi + +SITE_PACKAGES="$(python -c 'import sysconfig; print(sysconfig.get_paths()["purelib"])')" + +link_mixed_package() { + src_dir="$1" + dst_dir="$2" + keep_dir_name="${3:-}" + + if [ ! -d "$src_dir" ] || [ ! -d "$dst_dir" ]; then + return + fi + + # Replace only the Python sources from the mounted repo and keep compiled + # artifacts that were installed into site-packages during image build. + for path in "$src_dir"/* "$src_dir"/.[!.]* "$src_dir"/..?*; do + [ -e "$path" ] || continue + name="$(basename "$path")" + if [ -n "$keep_dir_name" ] && [ "$name" = "$keep_dir_name" ]; then + continue + fi + rm -rf "$dst_dir/$name" + cp -as "$path" "$dst_dir/$name" + done +} + +link_pure_python_package() { + src_dir="$1" + dst_dir="$2" + + if [ ! -d "$src_dir" ]; then + return + fi + + rm -rf "$dst_dir" + ln -s "$src_dir" "$dst_dir" +} + +link_mixed_package "$REPO_ROOT/python/rcs" "$SITE_PACKAGES/rcs" "_core" +link_mixed_package "$REPO_ROOT/extensions/rcs_fr3/src/rcs_fr3" "$SITE_PACKAGES/rcs_fr3" "_core" +link_pure_python_package "$REPO_ROOT/extensions/rcs_realsense/src/rcs_realsense" "$SITE_PACKAGES/rcs_realsense" +link_pure_python_package "$REPO_ROOT/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85" "$SITE_PACKAGES/rcs_robotiq2f85" +link_pure_python_package "$REPO_ROOT/extensions/rcs_zed/src/rcs_zed" "$SITE_PACKAGES/rcs_zed" diff --git a/examples/teleop/franka.py b/examples/teleop/franka.py index 334fed08..979b55fe 100644 --- a/examples/teleop/franka.py +++ b/examples/teleop/franka.py @@ -41,6 +41,9 @@ # "bird_eye": "243522070364", # } CAMERA_DICT = None +ZED_CAMERA_DICT = { + "zed": "19928076", +} MQ3_ADDR = "10.42.0.1" # DIGIT_DICT = { @@ -93,6 +96,23 @@ def get_env(): for name, identifier in CAMERA_DICT.items() }, ) + if ZED_CAMERA_DICT is not None: + camera_cfgs["zed"] = HardwareCameraCreatorConfig( + camera_type_id="zed", + camera_cfgs={ + name: BaseCameraConfig( + identifier=identifier, + resolution_width=1280, + resolution_height=720, + frame_rate=30, + ) + for name, identifier in ZED_CAMERA_DICT.items() + }, + kwargs={ + "enable_depth": False, + "enable_imu": False, + }, + ) if DIGIT_DICT is not None: camera_cfgs["digit"] = HardwareCameraCreatorConfig( camera_type_id="digit", diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 83e670d8..2225a577 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -78,6 +78,23 @@ def _create_realsense_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera ) +def _create_zed_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: + try: + from rcs.camera.hw import CalibrationStrategy + from rcs_zed.camera import ZEDCameraSet + except ImportError as e: + msg = "ZED camera support requires the `rcs_zed` extension to be installed." + raise ImportError(msg) from e + + calibration_strategy = { + name: typing.cast(CalibrationStrategy, DummyCalibrationStrategy()) for name in cfg.camera_cfgs + } + return typing.cast( + HardwareCamera, + ZEDCameraSet(cameras=cfg.camera_cfgs, calibration_strategy=calibration_strategy, **cfg.kwargs), + ) + + def _create_digit_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: try: from rcs.camera.digit_cam import DigitCam @@ -90,6 +107,7 @@ def _create_digit_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: HARDWARE_CAMERA_CREATORS: dict[str, typing.Callable[[HardwareCameraCreatorConfig], HardwareCamera]] = { "realsense": _create_realsense_camera, + "zed": _create_zed_camera, "digit": _create_digit_camera, } diff --git a/extensions/rcs_realsense/README.md b/extensions/rcs_realsense/README.md new file mode 100644 index 00000000..f0ad1dd4 --- /dev/null +++ b/extensions/rcs_realsense/README.md @@ -0,0 +1,10 @@ +# RCS Realsense Cameras +Package installation: +```shell +pip install -ve . +``` +Usage: +```shell +python -m rcs_realsense serials +python -m rcs_realsense rgb-view +``` \ No newline at end of file diff --git a/extensions/rcs_realsense/src/rcs_realsense/__main__.py b/extensions/rcs_realsense/src/rcs_realsense/__main__.py index ff342679..93ebd43f 100644 --- a/extensions/rcs_realsense/src/rcs_realsense/__main__.py +++ b/extensions/rcs_realsense/src/rcs_realsense/__main__.py @@ -1,24 +1,89 @@ import logging +import cv2 import pyrealsense2 as rs import typer from rcs_realsense.camera import RealSenseCameraSet +from rcs import common + logger = logging.getLogger(__name__) realsense_app = typer.Typer(help="CLI tool for the intel realsense module of rcs.") +def _display_frame(window_name: str, frame, *, is_rgb: bool): + image = frame.camera.color.data + if is_rgb: + image = image[:, :, ::-1] + cv2.imshow(window_name, image) + + @realsense_app.command() def serials(): """Reads out the serial numbers of the connected realsense devices.""" - context = rs.context() - devices = RealSenseCameraSet.enumerate_connected_devices(context) + try: + context = rs.context() + devices = RealSenseCameraSet.enumerate_connected_devices(context) + except Exception as exc: + typer.secho(f"Could not enumerate RealSense devices: {exc}", fg=typer.colors.RED, err=True) + return + if len(devices) == 0: - logger.warning("No realsense devices connected.") + typer.secho("No RealSense devices connected.", fg=typer.colors.YELLOW, err=True) return - logger.info("Connected devices:") + + typer.echo("Connected devices:") for device in devices.values(): - logger.info(" %s: %s", device.product_line, device.serial) + typer.echo(f" {device.product_line}: {device.serial}") + + +@realsense_app.command("rgb-view") +def rgb_view( + serial: str | None = typer.Argument( + None, help="Optional RealSense serial number. Uses the first device if omitted." + ), + width: int = typer.Option(1280, help="Requested capture width."), + height: int = typer.Option(720, help="Requested capture height."), + fps: int = typer.Option(30, help="Requested capture frame rate."), + window_name: str = typer.Option("RealSense RGB", help="OpenCV window title."), +): + """Open a live RGB window using the RCS RealSense camera interface.""" + if serial is None: + devices = RealSenseCameraSet.enumerate_connected_devices(rs.context()) + if len(devices) == 0: + msg = "No RealSense devices connected." + raise typer.BadParameter(msg) + serial = next(iter(devices)) + + camera = RealSenseCameraSet( + cameras={ + "viewer": common.BaseCameraConfig( + identifier=serial, + resolution_width=width, + resolution_height=height, + frame_rate=fps, + ) + }, + enable_ir=False, + enable_imu=False, + ) + + try: + camera.open() + except Exception as exc: + msg = f"Could not start RealSense camera {serial}: {exc}" + raise typer.BadParameter(msg) from exc + + logger.info("Streaming RGB from RealSense %s. Press 'q' to quit.", serial) + try: + while True: + frame = camera.poll_frame("viewer") + _display_frame(window_name, frame, is_rgb=True) + if cv2.waitKey(1) & 0xFF == ord("q"): + break + finally: + camera.close() + cv2.destroyAllWindows() if __name__ == "__main__": diff --git a/extensions/rcs_realsense/src/rcs_realsense/calibration.py b/extensions/rcs_realsense/src/rcs_realsense/calibration.py index c853f45a..fe32c0bc 100644 --- a/extensions/rcs_realsense/src/rcs_realsense/calibration.py +++ b/extensions/rcs_realsense/src/rcs_realsense/calibration.py @@ -40,8 +40,8 @@ def calibrate( input() tries = 3 while len(samples) < 10 and tries > 0: - logger.info("not enough frames in recorded, waiting 2 seconds...") - tries = -1 + logger.info("Not enough frames recorded, waiting 2 seconds...") + tries -= 1 sleep(2) if tries == 0: logger.warning("Calibration failed, not enough frames arrived.") @@ -51,7 +51,6 @@ def calibrate( with lock: for sample in samples: frames.append(sample.camera.color.data.copy()) - # print(frames) # Removed print for cleaner logs, optional _, tag_to_cam = get_average_marker_pose(frames, intrinsics=intrinsics, calib_tag_id=9, show_live_window=False) @@ -120,7 +119,7 @@ def get_average_marker_pose( def get_marker_pose(calib_tag_id, detector, intrinsics, frame): - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY) # CHANGE 3: Pose estimation happens INSIDE .detect() # We must extract camera params first to pass them here diff --git a/extensions/rcs_usb_cam/README.md b/extensions/rcs_usb_cam/README.md new file mode 100644 index 00000000..efa94a5b --- /dev/null +++ b/extensions/rcs_usb_cam/README.md @@ -0,0 +1,9 @@ +# RCS USC Cameras +Package installation: +```shell +pip install -ve . +``` +Usage: +```shell +python -m rcs_usb_cam rgb-view +``` \ No newline at end of file diff --git a/extensions/rcs_usb_cam/src/rcs_usb_cam/__main__.py b/extensions/rcs_usb_cam/src/rcs_usb_cam/__main__.py new file mode 100644 index 00000000..be90086e --- /dev/null +++ b/extensions/rcs_usb_cam/src/rcs_usb_cam/__main__.py @@ -0,0 +1,63 @@ +import logging + +import cv2 +import typer +from rcs_usb_cam.camera import USBCameraConfig, USBCameraSet + +logger = logging.getLogger(__name__) +usb_cam_app = typer.Typer(help="CLI tools for the generic USB camera module of rcs.") + + +def _capture_identifier(identifier: str) -> str | int: + return int(identifier) if identifier.isdigit() else identifier + + +def _display_frame(window_name: str, frame): + cv2.imshow(window_name, frame.camera.color.data) + + +@usb_cam_app.command("rgb-view") +def rgb_view( + identifier: str = typer.Argument("/dev/video0", help="Video device path or numeric camera id."), + width: int = typer.Option(640, help="Requested capture width."), + height: int = typer.Option(480, help="Requested capture height."), + fps: int = typer.Option(30, help="Requested capture frame rate."), + window_name: str = typer.Option("USB Camera RGB", help="OpenCV window title."), +): + """Open a live RGB window using the RCS USB camera interface.""" + camera_identifier = _capture_identifier(identifier) + camera = USBCameraSet( + cameras={ + "viewer": USBCameraConfig( + identifier=camera_identifier, # type: ignore[arg-type] + resolution_width=width, + resolution_height=height, + frame_rate=fps, + ) + } + ) + + try: + camera.open() + except Exception as exc: + msg = f"Could not open USB camera {identifier}: {exc}" + raise typer.BadParameter(msg) from exc + + logger.info("Streaming RGB from %s. Press 'q' to quit.", identifier) + try: + while True: + frame = camera.poll_frame("viewer") + _display_frame(window_name, frame) + if cv2.waitKey(1) & 0xFF == ord("q"): + break + finally: + camera.close() + cv2.destroyAllWindows() + + +def main(): + usb_cam_app() + + +if __name__ == "__main__": + main() diff --git a/extensions/rcs_zed/Makefile b/extensions/rcs_zed/Makefile new file mode 100644 index 00000000..940b17db --- /dev/null +++ b/extensions/rcs_zed/Makefile @@ -0,0 +1,23 @@ +PYSRC = src/rcs_zed +TESTSRC = tests +TESTFILE = tests/test_zed_extension.py +PYTHONPATH_LOCAL = ../../python:src:../rcs_realsense/src + +pycheckformat: + isort --check-only ${PYSRC} ${TESTSRC} + black --check ${PYSRC} ${TESTSRC} + +pyformat: + isort ${PYSRC} ${TESTSRC} + black ${PYSRC} ${TESTSRC} + +ruff: + ruff check ${PYSRC} ${TESTSRC} + +pytest: + PYTHONPATH=${PYTHONPATH_LOCAL} python -m pytest ${TESTFILE} + +test-compile: + python -m compileall ${PYSRC} ${TESTFILE} + +.PHONY: pycheckformat pyformat ruff pytest test-compile diff --git a/extensions/rcs_zed/README.md b/extensions/rcs_zed/README.md new file mode 100644 index 00000000..6352d1e6 --- /dev/null +++ b/extensions/rcs_zed/README.md @@ -0,0 +1,43 @@ +# Zed Camera Extension + +## Installation +- You need a PC with an Nvidia GPU and CUDA 12.8 installed (12.x is probably fine) +- Download and install the Zed SDK from [here](https://www.stereolabs.com/en-fr/developers/release) +- Install the python bindings with the manual [here](https://github.com/stereolabs/zed-python-api) + +Or just use the docker shipped with RCS. Checkout [this readme](../../docker/README.md) to build and start the docker. The tools shown below are available inside the docer container. + +Package installation: +```shell +pip install -ve . +``` + + +## Usage + + +```shell +python -m rcs_zed serials +python -m rcs_zed rgb-view +``` + +### Permissions +```shell +# if in docker, run this on your host system +sudo usermod -a -G video,plugdev $USER +``` + +### Tools to check your Zed +Most relevant is the `ZED_Diagnostic`, use this to check what exactly is not working. + +| Command / Tool | What it does | When to use it | +| :--- | :--- | :--- | +| **`ZED_Explorer`** | Live video feed & recording | To check image quality, adjust exposure, and record **.SVO** files. | +| **`ZED_Depth_Viewer`** | 3D Depth & Point Cloud | To see the "Neural" depth in action and check 3D accuracy. | +| **`ZED_Diagnostic`** | Hardware/Software Health | Use this first if something feels "broken." | +| **`ZED_Sensor_Viewer`** | IMU & Magnetometer | To see real-time data from the accelerometer and gyroscope. | +| **`ZED_Studio`** | Multi-Camera Management | If you have more than one ZED connected at once. | +| **`ZEDfu`** | Spatial Mapping | To "scan" a room and create a 3D mesh in real-time. | + +### Issues with the connection +- see [this article](https://support.stereolabs.com/hc/en-us/articles/207635225-How-to-fix-USB-3-0-bandwidth-and-connection-issues) \ No newline at end of file diff --git a/extensions/rcs_zed/pyproject.toml b/extensions/rcs_zed/pyproject.toml new file mode 100644 index 00000000..51694e69 --- /dev/null +++ b/extensions/rcs_zed/pyproject.toml @@ -0,0 +1,25 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" + +[project] +name = "rcs_zed" +version = "0.6.3" +description = "RCS ZED camera module" +dependencies = [ + "rcs>=0.6.3", + "opencv-python~=4.10.0", + "pupil_apriltags", + "diskcache", + "typer~=0.9", +] +maintainers = [{ name = "Tobias Jülg", email = "tobias.juelg@utn.de" }] +authors = [{ name = "Tobias Jülg", email = "tobias.juelg@utn.de" }] +requires-python = ">=3.11" + +[tool.black] +line-length = 120 +target-version = ["py310"] + +[tool.isort] +profile = "black" diff --git a/extensions/rcs_zed/src/rcs_zed/__init__.py b/extensions/rcs_zed/src/rcs_zed/__init__.py new file mode 100644 index 00000000..63af8876 --- /dev/null +++ b/extensions/rcs_zed/src/rcs_zed/__init__.py @@ -0,0 +1 @@ +__version__ = "0.6.3" diff --git a/extensions/rcs_zed/src/rcs_zed/__main__.py b/extensions/rcs_zed/src/rcs_zed/__main__.py new file mode 100644 index 00000000..cfe51fa5 --- /dev/null +++ b/extensions/rcs_zed/src/rcs_zed/__main__.py @@ -0,0 +1,89 @@ +import logging + +import cv2 +import typer +from rcs_zed.camera import ZEDCameraSet + +from rcs import common + +logger = logging.getLogger(__name__) +zed_app = typer.Typer(help="CLI tools for the ZED camera module of rcs.") + + +def _display_frame(window_name: str, frame): + cv2.imshow(window_name, frame.camera.color.data[:, :, ::-1]) + + +@zed_app.command() +def serials(): + """Reads out the serial numbers and models of connected ZED devices via the SDK.""" + try: + devices = ZEDCameraSet.enumerate_connected_devices() + except RuntimeError as exc: + typer.secho(str(exc), fg=typer.colors.YELLOW, err=True) + return + if len(devices) == 0: + typer.secho("No ZED devices connected or the ZED SDK is not available.", fg=typer.colors.YELLOW, err=True) + return + typer.echo("Connected devices:") + for device in devices.values(): + typer.echo(f" {device.model}: {device.serial} (imu={device.has_imu})") + + +@zed_app.command("rgb-view") +def rgb_view( + serial: str | None = typer.Argument(None, help="Optional ZED serial number. Uses the first device if omitted."), + width: int = typer.Option(1280, help="Requested capture width."), + height: int = typer.Option(720, help="Requested capture height."), + fps: int = typer.Option(30, help="Requested capture frame rate."), + window_name: str = typer.Option("ZED RGB", help="OpenCV window title."), +): + """Open a live RGB window using the RCS ZED camera interface.""" + if serial is None: + try: + devices = ZEDCameraSet.enumerate_connected_devices() + except RuntimeError as exc: + msg = str(exc) + raise typer.BadParameter(msg) from exc + if len(devices) == 0: + msg = "No ZED devices connected." + raise typer.BadParameter(msg) + serial = next(iter(devices)) + + camera = ZEDCameraSet( + cameras={ + "viewer": common.BaseCameraConfig( + identifier=serial, + resolution_width=width, + resolution_height=height, + frame_rate=fps, + ) + }, + enable_depth=False, + enable_imu=False, + ) + + try: + camera.open() + except Exception as exc: + msg = f"Could not start ZED camera {serial}: {exc}" + raise typer.BadParameter(msg) from exc + + logger.info("Streaming RGB from ZED %s. Press 'q' to quit.", serial) + try: + while True: + frame = camera.poll_frame("viewer") + _display_frame(window_name, frame) + if cv2.waitKey(1) & 0xFF == ord("q"): + break + finally: + camera.close() + cv2.destroyAllWindows() + + +def main(): + zed_app() + + +if __name__ == "__main__": + main() diff --git a/extensions/rcs_zed/src/rcs_zed/camera.py b/extensions/rcs_zed/src/rcs_zed/camera.py new file mode 100644 index 00000000..e9fd61dd --- /dev/null +++ b/extensions/rcs_zed/src/rcs_zed/camera.py @@ -0,0 +1,330 @@ +import copy +import logging +import threading +import typing +from dataclasses import dataclass +from time import time + +import numpy as np +from rcs.camera.hw import CalibrationStrategy, DummyCalibrationStrategy, HardwareCamera +from rcs.camera.interface import BaseCameraSet, CameraFrame, DataFrame, Frame, IMUFrame + +from rcs import common + +try: + from pyzed import sl +except ImportError: # pragma: no cover - exercised via fake backend tests + sl = None # type: ignore[assignment] + + +@dataclass +class ZEDDeviceInfo: + serial: str + model: str + has_depth: bool = True + has_imu: bool = False + + +@dataclass +class ZEDFrameBundle: + color: np.ndarray + timestamp: float + color_intrinsics: np.ndarray[tuple[typing.Literal[3], typing.Literal[4]], np.dtype[np.float64]] + depth: np.ndarray | None = None + accel: np.ndarray | None = None + gyro: np.ndarray | None = None + + +def _intrinsics_matrix(fx: float, fy: float, cx: float, cy: float) -> np.ndarray: + return np.array( + [ + [fx, 0, cx, 0], + [0, fy, cy, 0], + [0, 0, 1, 0], + ], + dtype=np.float64, + ) + + +class PyZEDCameraHandle: + def __init__(self, camera: typing.Any, device_info: ZEDDeviceInfo, color_intrinsics: np.ndarray): + self.camera = camera + self.device_info = device_info + self.color_intrinsics = color_intrinsics + self.runtime_parameters = sl.RuntimeParameters() # type: ignore[union-attr] + self.image_mat = sl.Mat() # type: ignore[union-attr] + self.depth_mat = sl.Mat() # type: ignore[union-attr] + self.sensors_data = sl.SensorsData() # type: ignore[union-attr] + + def _timestamp_seconds(self) -> float: + try: + timestamp = self.camera.get_timestamp(sl.TIME_REFERENCE.IMAGE) # type: ignore[union-attr] + if hasattr(timestamp, "get_nanoseconds"): + return float(timestamp.get_nanoseconds()) * 1e-9 + if hasattr(timestamp, "get_microseconds"): + return float(timestamp.get_microseconds()) * 1e-6 + if hasattr(timestamp, "get_milliseconds"): + return float(timestamp.get_milliseconds()) * 1e-3 + except Exception: + pass + return time() + + def grab_frame(self) -> ZEDFrameBundle: + err = self.camera.grab(self.runtime_parameters) + if err != sl.ERROR_CODE.SUCCESS: # type: ignore[union-attr] + msg = f"Failed to grab ZED frame: {err}" + raise RuntimeError(msg) + + self.camera.retrieve_image(self.image_mat, sl.VIEW.LEFT) # type: ignore[union-attr] + color_raw = np.array(self.image_mat.get_data(), copy=True) + if color_raw.ndim != 3: + msg = f"Unexpected ZED image shape {color_raw.shape}" + raise RuntimeError(msg) + color_rgb = color_raw[:, :, :3][:, :, ::-1] if color_raw.shape[2] == 4 else color_raw[:, :, ::-1] + + depth = None + if self.device_info.has_depth: + self.camera.retrieve_measure(self.depth_mat, sl.MEASURE.DEPTH) # type: ignore[union-attr] + depth_raw = np.array(self.depth_mat.get_data(), copy=True) + if depth_raw.ndim > 2: + depth_raw = depth_raw[:, :, 0] + depth_m = np.nan_to_num(depth_raw, nan=0.0, posinf=0.0, neginf=0.0) + depth_m = np.clip(depth_m, a_min=0.0, a_max=np.iinfo(np.uint16).max / BaseCameraSet.DEPTH_SCALE) + depth = (depth_m * BaseCameraSet.DEPTH_SCALE).astype(np.uint16) + + accel = None + gyro = None + if self.device_info.has_imu: + sensor_err = self.camera.get_sensors_data(self.sensors_data, sl.TIME_REFERENCE.IMAGE) # type: ignore[union-attr] + if sensor_err == sl.ERROR_CODE.SUCCESS: # type: ignore[union-attr] + imu_data = self.sensors_data.get_imu_data() + if hasattr(imu_data, "get_linear_acceleration"): + accel = np.array(imu_data.get_linear_acceleration(), dtype=np.float64) + if hasattr(imu_data, "get_angular_velocity"): + gyro = np.array(imu_data.get_angular_velocity(), dtype=np.float64) + + return ZEDFrameBundle( + color=color_rgb, + depth=depth, + accel=accel, + gyro=gyro, + timestamp=self._timestamp_seconds(), + color_intrinsics=self.color_intrinsics, + ) + + def close(self): + self.camera.close() + + +class ZEDCameraSet(HardwareCamera): + CALIBRATION_FRAME_SIZE = 30 + + @staticmethod + def _require_sdk(): + if sl is None: + msg = ( + "The ZED SDK Python bindings are not available. Install the ZED SDK and ensure " + "`import pyzed.sl as sl` works on this machine." + ) + raise RuntimeError(msg) + + @staticmethod + def _device_has_imu(device: typing.Any) -> bool: + for attr in ("sensors_configuration", "sensors_conf"): + sensors_conf = getattr(device, attr, None) + if sensors_conf is None: + continue + camera_imu = getattr(sensors_conf, "camera_imu", None) + if camera_imu is None: + continue + if hasattr(camera_imu, "available"): + return bool(camera_imu.available) + return True + return False + + @staticmethod + def _model_to_string(model: typing.Any) -> str: + if hasattr(model, "name"): + return str(model.name) + return str(model) + + @staticmethod + def _map_resolution(width: int, height: int): + ZEDCameraSet._require_sdk() + assert sl is not None + mapping = { + (2208, 1242): sl.RESOLUTION.HD2K, + (1920, 1080): sl.RESOLUTION.HD1080, + (1280, 720): sl.RESOLUTION.HD720, + (672, 376): sl.RESOLUTION.VGA, + } + if (width, height) not in mapping: + msg = f"Unsupported ZED resolution {width}x{height}. Use one of: {sorted(mapping)}" + raise ValueError(msg) + return mapping[(width, height)] + + @classmethod + def enumerate_connected_devices(cls) -> dict[str, ZEDDeviceInfo]: + cls._require_sdk() + assert sl is not None + devices: dict[str, ZEDDeviceInfo] = {} + for device in sl.Camera.get_device_list(): + serial = str(device.serial_number) + devices[serial] = ZEDDeviceInfo( + serial=serial, + model=cls._model_to_string(device.camera_model), + has_depth=True, + has_imu=cls._device_has_imu(device), + ) + return devices + + @classmethod + def open_camera( + cls, + config: common.BaseCameraConfig, + *, + enable_depth: bool, + enable_imu: bool, + ) -> PyZEDCameraHandle: + cls._require_sdk() + assert sl is not None + + init = sl.InitParameters() + init.camera_resolution = cls._map_resolution(config.resolution_width, config.resolution_height) + init.camera_fps = config.frame_rate + init.coordinate_units = sl.UNIT.METER + init.depth_mode = sl.DEPTH_MODE.NONE if not enable_depth else sl.DEPTH_MODE.QUALITY + init.sdk_verbose = False + init.set_from_serial_number(int(config.identifier)) + + camera = sl.Camera() + err = camera.open(init) + if err != sl.ERROR_CODE.SUCCESS: + msg = f"Could not open ZED camera {config.identifier}: {err}" + raise RuntimeError(msg) + + information = camera.get_camera_information() + calibration = information.camera_configuration.calibration_parameters + left_cam = calibration.left_cam + info = ZEDDeviceInfo( + serial=str(config.identifier), + model=cls._model_to_string(information.camera_model), + has_depth=enable_depth, + has_imu=enable_imu and cls._device_has_imu(information), + ) + intrinsics = _intrinsics_matrix(left_cam.fx, left_cam.fy, left_cam.cx, left_cam.cy) + return PyZEDCameraHandle(camera=camera, device_info=info, color_intrinsics=intrinsics) + + def __init__( + self, + cameras: dict[str, common.BaseCameraConfig], + calibration_strategy: dict[str, CalibrationStrategy] | None = None, + enable_depth: bool = True, + enable_imu: bool = True, + ) -> None: + self.cameras = cameras + if calibration_strategy is None: + calibration_strategy = {camera_name: DummyCalibrationStrategy() for camera_name in cameras} + self.calibration_strategy = calibration_strategy + self.enable_depth = enable_depth + self.enable_imu = enable_imu + self._logger = logging.getLogger(__name__) + self._camera_names = list(self.cameras.keys()) + self._available_devices: dict[str, ZEDDeviceInfo] = {} + self._enabled_devices: dict[str, PyZEDCameraHandle] = {} + self._frame_buffer_lock: dict[str, threading.Lock] = {} + self._frame_buffer: dict[str, list[Frame]] = {} + + assert ( + len({camera.resolution_width for camera in self.cameras.values()}) == 1 + and len({camera.resolution_height for camera in self.cameras.values()}) == 1 + and len({camera.frame_rate for camera in self.cameras.values()}) == 1 + ), "All cameras must have the same resolution and frame rate." + + @property + def camera_names(self) -> list[str]: + return self._camera_names + + def config(self, camera_name) -> common.BaseCameraConfig: + return self.cameras[camera_name] + + def update_available_devices(self): + self._available_devices = self.enumerate_connected_devices() + + def open(self): + self.update_available_devices() + self._enabled_devices = {} + self._frame_buffer = {} + self._frame_buffer_lock = {} + + for camera_name, camera_cfg in self.cameras.items(): + if camera_cfg.identifier not in self._available_devices: + msg = f"ZED device {camera_name} with serial {camera_cfg.identifier} not found." + raise RuntimeError(msg) + + device_info = self._available_devices[camera_cfg.identifier] + opened = self.open_camera( + camera_cfg, + enable_depth=self.enable_depth and device_info.has_depth, + enable_imu=self.enable_imu and device_info.has_imu, + ) + self._enabled_devices[camera_name] = opened + self._frame_buffer[camera_name] = [] + self._frame_buffer_lock[camera_name] = threading.Lock() + + def poll_frame(self, camera_name: str) -> Frame: + assert camera_name in self.camera_names, f"Camera {camera_name} not found in the enabled devices" + device = self._enabled_devices[camera_name] + bundle = device.grab_frame() + extrinsics = self.calibration_strategy[camera_name].get_extrinsics() + + color = DataFrame( + data=bundle.color, + timestamp=bundle.timestamp, + intrinsics=bundle.color_intrinsics, + extrinsics=extrinsics, + ) + depth = None + if bundle.depth is not None: + depth = DataFrame( + data=bundle.depth, + timestamp=bundle.timestamp, + intrinsics=bundle.color_intrinsics, + extrinsics=extrinsics, + ) + + accel = DataFrame(data=bundle.accel, timestamp=bundle.timestamp) if bundle.accel is not None else None + gyro = DataFrame(data=bundle.gyro, timestamp=bundle.timestamp) if bundle.gyro is not None else None + + frame = Frame( + camera=CameraFrame(color=color, depth=depth), + imu=IMUFrame(accel=accel, gyro=gyro) if (accel is not None or gyro is not None) else None, + avg_timestamp=bundle.timestamp, + ) + + with self._frame_buffer_lock[camera_name]: + if len(self._frame_buffer[camera_name]) >= self.CALIBRATION_FRAME_SIZE: + self._frame_buffer[camera_name].pop(0) + self._frame_buffer[camera_name].append(copy.deepcopy(frame)) + return frame + + def close(self): + for device in self._enabled_devices.values(): + device.close() + self._enabled_devices = {} + + def calibrate(self) -> bool: + for camera_name in self.cameras: + device = self._enabled_devices.get(camera_name) + if device is None: + msg = f"Camera {camera_name} must be opened before calibration." + raise RuntimeError(msg) + if not self.calibration_strategy[camera_name].calibrate( + intrinsics=device.color_intrinsics, + samples=self._frame_buffer[camera_name], + lock=self._frame_buffer_lock[camera_name], + ): + self._logger.warning("Calibration of camera %s failed.", camera_name) + return False + self._logger.info("Calibration successful.") + return True diff --git a/extensions/rcs_zed/src/rcs_zed/utils.py b/extensions/rcs_zed/src/rcs_zed/utils.py new file mode 100644 index 00000000..0573076a --- /dev/null +++ b/extensions/rcs_zed/src/rcs_zed/utils.py @@ -0,0 +1,28 @@ +import typing + +from rcs.camera.hw import CalibrationStrategy +from rcs_realsense.calibration import FR3BaseArucoCalibration +from rcs_zed.camera import ZEDCameraSet + +from rcs import common + + +def default_zed(name2id: dict[str, str] | None) -> ZEDCameraSet | None: + if name2id is None: + return None + cameras = { + name: common.BaseCameraConfig(identifier=id, resolution_width=1280, resolution_height=720, frame_rate=30) + for name, id in name2id.items() + } + calibration_strategy = {name: typing.cast(CalibrationStrategy, FR3BaseArucoCalibration(name)) for name in name2id} + return ZEDCameraSet(cameras=cameras, calibration_strategy=calibration_strategy) + + +def default_zed_dummy_calibration(name2id: dict[str, str] | None) -> ZEDCameraSet | None: + if name2id is None: + return None + cameras = { + name: common.BaseCameraConfig(identifier=id, resolution_width=1280, resolution_height=720, frame_rate=30) + for name, id in name2id.items() + } + return ZEDCameraSet(cameras=cameras) diff --git a/extensions/rcs_zed/tests/test_zed_extension.py b/extensions/rcs_zed/tests/test_zed_extension.py new file mode 100644 index 00000000..32e8294f --- /dev/null +++ b/extensions/rcs_zed/tests/test_zed_extension.py @@ -0,0 +1,119 @@ +import sys +from pathlib import Path +from typing import TypedDict + +import numpy as np +import pytest + +REPO_ROOT = Path(__file__).resolve().parents[3] +sys.path.insert(0, str(REPO_ROOT / "python")) +sys.path.insert(0, str(REPO_ROOT / "extensions/rcs_zed/src")) + +from rcs_zed.camera import ZEDCameraSet, ZEDDeviceInfo, ZEDFrameBundle # noqa: E402 + +from rcs import common # noqa: E402 + + +class FakeOpenedZEDCamera: + def __init__(self, device_info: ZEDDeviceInfo, color_intrinsics: np.ndarray, frame_bundle: ZEDFrameBundle): + self.device_info = device_info + self.color_intrinsics = color_intrinsics + self._frame_bundle = frame_bundle + self.closed = False + + def grab_frame(self) -> ZEDFrameBundle: + return self._frame_bundle + + def close(self): + self.closed = True + + +class PatchZedState(TypedDict): + devices: dict[str, ZEDDeviceInfo] + opened: dict[str, FakeOpenedZEDCamera] + open_calls: list[tuple[str, bool, bool]] + + +@pytest.fixture() +def patch_zed(monkeypatch) -> PatchZedState: + state: PatchZedState = {"devices": {}, "opened": {}, "open_calls": []} + + def fake_enumerate(_cls): + return state["devices"] + + def fake_open(_cls, config: common.BaseCameraConfig, *, enable_depth: bool, enable_imu: bool): + state["open_calls"].append((config.identifier, enable_depth, enable_imu)) + return state["opened"][config.identifier] + + monkeypatch.setattr(ZEDCameraSet, "enumerate_connected_devices", classmethod(fake_enumerate)) + monkeypatch.setattr(ZEDCameraSet, "open_camera", classmethod(fake_open)) + return state + + +def test_zed_frame_mapping_depth_scaling_and_imu_downgrade(patch_zed): + intrinsics = np.array([[100.0, 0.0, 10.0, 0.0], [0.0, 110.0, 20.0, 0.0], [0.0, 0.0, 1.0, 0.0]]) + color = np.arange(27, dtype=np.uint8).reshape(3, 3, 3) + depth = np.full((3, 3), 1234, dtype=np.uint16) + frame_bundle = ZEDFrameBundle( + color=color, + depth=depth, + accel=None, + gyro=None, + timestamp=12.5, + color_intrinsics=intrinsics, + ) + device_info = ZEDDeviceInfo(serial="123", model="ZED Mini", has_depth=True, has_imu=False) + opened = FakeOpenedZEDCamera(device_info=device_info, color_intrinsics=intrinsics, frame_bundle=frame_bundle) + patch_zed["devices"] = {"123": device_info} + patch_zed["opened"] = {"123": opened} + + camera_set = ZEDCameraSet( + cameras={ + "wrist": common.BaseCameraConfig( + identifier="123", resolution_width=1280, resolution_height=720, frame_rate=30 + ) + }, + ) + camera_set.open() + frame = camera_set.poll_frame("wrist") + assert frame.camera.color.intrinsics is not None + + assert patch_zed["open_calls"] == [("123", True, False)] + assert np.array_equal(frame.camera.color.data, color) + assert np.array_equal(frame.camera.depth.data, depth) # type: ignore[union-attr] + assert np.array_equal(frame.camera.color.intrinsics, intrinsics) + assert frame.imu is None + assert frame.avg_timestamp == 12.5 + + +def test_zed_enumeration_and_multi_camera_open(patch_zed): + intrinsics = np.eye(3, 4) + bundle = ZEDFrameBundle( + color=np.zeros((2, 2, 3), dtype=np.uint8), depth=None, timestamp=1.0, color_intrinsics=intrinsics + ) + devices = { + "111": ZEDDeviceInfo(serial="111", model="ZED Mini", has_depth=True, has_imu=False), + "222": ZEDDeviceInfo(serial="222", model="ZED 2", has_depth=True, has_imu=True), + } + opened = {serial: FakeOpenedZEDCamera(info, intrinsics, bundle) for serial, info in devices.items()} + patch_zed["devices"] = devices + patch_zed["opened"] = opened + + enumerated = ZEDCameraSet.enumerate_connected_devices() + assert enumerated == devices + + camera_set = ZEDCameraSet( + cameras={ + "left": common.BaseCameraConfig( + identifier="111", resolution_width=1280, resolution_height=720, frame_rate=30 + ), + "right": common.BaseCameraConfig( + identifier="222", resolution_width=1280, resolution_height=720, frame_rate=30 + ), + }, + ) + camera_set.open() + assert patch_zed["open_calls"] == [("111", True, False), ("222", True, True)] + camera_set.close() + assert opened["111"].closed + assert opened["222"].closed diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 8482e18b..390df41b 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -9,7 +9,15 @@ from rcs import camera, envs, hand, sim -RCS_PREFIX = os.path.join(os.path.dirname(__file__), "../../") + +def _rcs_prefix() -> str: + env_prefix = os.environ.get("RCS_PREFIX") + if env_prefix: + return os.path.abspath(env_prefix) + return os.path.abspath(os.path.join(os.path.dirname(__file__), "../../")) + + +RCS_PREFIX = _rcs_prefix() # TODO: assets must be "downloaded" first time this is imported