diff --git a/.github/workflows/node_hub_test.sh b/.github/workflows/node_hub_test.sh index 13328dfa7..91eaa8f04 100755 --- a/.github/workflows/node_hub_test.sh +++ b/.github/workflows/node_hub_test.sh @@ -44,7 +44,7 @@ else echo "Running linting and tests for Python project in $dir..." pip install . poetry run black --check . - poetry run pylint --disable=C,R --ignored-modules=cv2 **/*.py + poetry run pylint --disable=C,R --ignored-modules=cv2,pyrealsense2 **/*.py poetry run pytest fi fi diff --git a/Cargo.lock b/Cargo.lock index e907e789e..d9d4e0151 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2689,6 +2689,7 @@ dependencies = [ name = "dora-rerun" version = "0.3.7" dependencies = [ + "bytemuck", "dora-node-api", "eyre", "k", diff --git a/examples/franka/camera.yaml b/examples/franka/camera.yaml new file mode 100644 index 000000000..98e1f0a69 --- /dev/null +++ b/examples/franka/camera.yaml @@ -0,0 +1,19 @@ +nodes: + - id: camera + build: pip install -e ../../node-hub/dora-pyrealsense + path: dora-pyrealsense + inputs: + tick: dora/timer/millis/20 + outputs: + - image + env: + IMAGE_WIDTH: 640 + IMAGE_HEIGHT: 480 + + - id: plot + build: pip install ../../node-hub/opencv-plot + path: opencv-plot + inputs: + image: + source: camera/image + queue_size: 1 diff --git a/examples/realsense/README.md b/examples/realsense/README.md new file mode 100644 index 000000000..b9dcec59e --- /dev/null +++ b/examples/realsense/README.md @@ -0,0 +1,10 @@ +# Quick example of using Pyrealsense + +Make sure to follow the installation at: [`node-hub/dora-pyrealsense/README.md`](node-hub/dora-pyrealsense/README.md) + +Then: + +```bash +dora build dataflow.yaml +dora run dataflow.yaml +``` diff --git a/examples/realsense/camera.yaml b/examples/realsense/camera.yaml new file mode 100644 index 000000000..ecffab45f --- /dev/null +++ b/examples/realsense/camera.yaml @@ -0,0 +1,20 @@ +nodes: + - id: camera + build: pip install -e ../../node-hub/dora-pyrealsense + path: dora-pyrealsense + inputs: + tick: dora/timer/millis/20 + outputs: + - image + - depth + env: + IMAGE_WIDTH: 640 + IMAGE_HEIGHT: 480 + + - id: plot + build: pip install ../../node-hub/opencv-plot + path: opencv-plot + inputs: + image: + source: camera/image + queue_size: 1 diff --git a/examples/realsense/camera_rerun.yaml b/examples/realsense/camera_rerun.yaml new file mode 100644 index 000000000..6212ba2da --- /dev/null +++ b/examples/realsense/camera_rerun.yaml @@ -0,0 +1,20 @@ +nodes: + - id: camera + build: pip install -e ../../node-hub/dora-pyrealsense + path: dora-pyrealsense + inputs: + tick: dora/timer/millis/20 + outputs: + - image + - depth + env: + IMAGE_WIDTH: 640 + IMAGE_HEIGHT: 480 + + - id: plot + build: pip install -e ../../node-hub/dora-rerun + path: dora-rerun + inputs: + image: camera/image + world/camera/depth: camera/depth + diff --git a/node-hub/dora-pyrealsense/README.md b/node-hub/dora-pyrealsense/README.md new file mode 100644 index 000000000..d5d2f3cd4 --- /dev/null +++ b/node-hub/dora-pyrealsense/README.md @@ -0,0 +1,87 @@ +# Dora Node for capturing video with PyRealSense + +This node is used to capture video from a camera using PyRealsense. + +# Installation + +Make sure to use realsense udev config at https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md + +You can try, the following: + +```bash +wget https://raw.githubusercontent.com/IntelRealSense/librealsense/refs/heads/master/scripts/setup_udev_rules.sh + +mkdir config +cd config +wget https://raw.githubusercontent.com/IntelRealSense/librealsense/refs/heads/master/scripts/config/99-realsense-libusb.rules +cd .. + +chmod +x setup_udev_rules.sh +``` + +# YAML + +```yaml +- id: opencv-video-capture + build: pip install ../../node-hub/opencv-video-capture + path: opencv-video-capture + inputs: + tick: dora/timer/millis/16 # try to capture at 60fps + outputs: + - image: # the captured image + + env: + PATH: 0 # optional, default is 0 + + IMAGE_WIDTH: 640 # optional, default is video capture width + IMAGE_HEIGHT: 480 # optional, default is video capture height +``` + +# Inputs + +- `tick`: empty Arrow array to trigger the capture + +# Outputs + +- `image`: an arrow array containing the captured image + +```Python +## Image data +image_data: UInt8Array # Example: pa.array(img.ravel()) +metadata = { + "width": 640, + "height": 480, + "encoding": str, # bgr8, rgb8 +} + +## Example +node.send_output( + image_data, {"width": 640, "height": 480, "encoding": "bgr8"} + ) + +## Decoding +storage = event["value"] + +metadata = event["metadata"] +encoding = metadata["encoding"] +width = metadata["width"] +height = metadata["height"] + +if encoding == "bgr8": + channels = 3 + storage_type = np.uint8 + +frame = ( + storage.to_numpy() + .astype(storage_type) + .reshape((height, width, channels)) +) +``` + +## Examples + +Check example at [examples/python-dataflow](examples/python-dataflow) + +## License + +This project is licensed under Apache-2.0. Check out [NOTICE.md](../../NOTICE.md) for more information. diff --git a/node-hub/dora-pyrealsense/dora_pyrealsense/__init__.py b/node-hub/dora-pyrealsense/dora_pyrealsense/__init__.py new file mode 100644 index 000000000..ac3cbef9f --- /dev/null +++ b/node-hub/dora-pyrealsense/dora_pyrealsense/__init__.py @@ -0,0 +1,11 @@ +import os + +# Define the path to the README file relative to the package directory +readme_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "README.md") + +# Read the content of the README file +try: + with open(readme_path, "r", encoding="utf-8") as f: + __doc__ = f.read() +except FileNotFoundError: + __doc__ = "README file not found." diff --git a/node-hub/dora-pyrealsense/dora_pyrealsense/main.py b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py new file mode 100644 index 000000000..142cd0f42 --- /dev/null +++ b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py @@ -0,0 +1,127 @@ +import os +import time + +import cv2 +import numpy as np +import pyarrow as pa + +from dora import Node + +RUNNER_CI = True if os.getenv("CI") == "true" else False +import pyrealsense2 as rs + + +def main(): + FLIP = os.getenv("FLIP", "") + DEVICE_SERIAL = os.getenv("DEVICE_SERIAL", "") + image_height = int(os.getenv("IMAGE_HEIGHT", "480")) + image_width = int(os.getenv("IMAGE_WIDTH", "640")) + encoding = os.getenv("ENCODING", "rgb8") + ctx = rs.context() + devices = ctx.query_devices() + if devices.size() == 0: + raise ConnectionError("No realsense camera connected.") + + # Serial list + serials = [device.get_info(rs.camera_info.serial_number) for device in devices] + if DEVICE_SERIAL and (DEVICE_SERIAL in serials): + raise ConnectionError( + f"Device with serial {DEVICE_SERIAL} not found within: {serials}." + ) + + pipeline = rs.pipeline() + + config = rs.config() + config.enable_device(DEVICE_SERIAL) + config.enable_stream(rs.stream.color, image_width, image_height, rs.format.rgb8, 30) + config.enable_stream(rs.stream.depth, image_width, image_height) + + align_to = rs.stream.color + align = rs.align(align_to) + + profile = pipeline.start(config) + + depth_sensor = profile.get_device().first_depth_sensor() + depth_scale = depth_sensor.get_depth_scale() + # rgb_profile = profile.get_stream(rs.stream.color) + # depth_profile = profile.get_stream(rs.stream.depth) + # rgb_intr = rgb_profile.as_video_stream_profile().get_intrinsics() + # rgb_intr = depth_profile.get_extrinsics_to(rgb_profile) + node = Node() + start_time = time.time() + + pa.array([]) # initialize pyarrow array + + for event in node: + + # Run this example in the CI for 10 seconds only. + if RUNNER_CI and time.time() - start_time > 10: + break + + event_type = event["type"] + + if event_type == "INPUT": + event_id = event["id"] + + if event_id == "tick": + frames = pipeline.wait_for_frames() + aligned_frames = align.process(frames) + + aligned_depth_frame = aligned_frames.get_depth_frame() + color_frame = aligned_frames.get_color_frame() + if not aligned_depth_frame or not color_frame: + continue + + depth_image = np.asanyarray(aligned_depth_frame.get_data()) + scaled_depth_image = depth_image * depth_scale + frame = np.asanyarray(color_frame.get_data()) + + ## Change rgb to bgr + + if FLIP == "VERTICAL": + frame = cv2.flip(frame, 0) + elif FLIP == "HORIZONTAL": + frame = cv2.flip(frame, 1) + elif FLIP == "BOTH": + frame = cv2.flip(frame, -1) + + # resize the frame + if ( + image_width is not None + and image_height is not None + and ( + frame.shape[1] != image_width or frame.shape[0] != image_height + ) + ): + frame = cv2.resize(frame, (image_width, image_height)) + + metadata = event["metadata"] + metadata["encoding"] = encoding + metadata["width"] = int(frame.shape[1]) + metadata["height"] = int(frame.shape[0]) + + # Get the right encoding + if encoding == "bgr8": + frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) + elif encoding in ["jpeg", "jpg", "jpe", "bmp", "webp", "png"]: + ret, frame = cv2.imencode("." + encoding, frame) + if not ret: + print("Error encoding image...") + continue + + storage = pa.array(frame.ravel()) + + # metadata["resolution"] = [int(rgb_intr.width), int(rgb_intr.height)] + # metadata["focal_length"] = [int(rgb_intr.fx), int(rgb_intr.fy)] + # metadata["principal_point"] = [int(rgb_intr.ppx), int(rgb_intr.ppy)] + node.send_output("image", storage, metadata) + node.send_output( + "depth", pa.array(scaled_depth_image.ravel()), metadata + ) + + elif event_type == "ERROR": + raise RuntimeError(event["error"]) + + +if __name__ == "__main__": + main() diff --git a/node-hub/dora-pyrealsense/pyproject.toml b/node-hub/dora-pyrealsense/pyproject.toml new file mode 100644 index 000000000..383f846c0 --- /dev/null +++ b/node-hub/dora-pyrealsense/pyproject.toml @@ -0,0 +1,21 @@ +[tool.poetry] +name = "dora-pyrealsense" +version = "0.0.1" +authors = ["Haixuan Xavier Tao "] +description = "Dora Node for capturing video with Pyrealsense" +readme = "README.md" + +packages = [{ include = "dora_pyrealsense" }] + +[tool.poetry.dependencies] +dora-rs = "^0.3.6" +numpy = "< 2.0.0" +opencv-python = ">= 4.1.1" +pyrealsense2 = "2.54.1.5216" + +[tool.poetry.scripts] +dora-pyrealsense = "dora_pyrealsense.main:main" + +[build-system] +requires = ["poetry-core>=1.8.0"] +build-backend = "poetry.core.masonry.api" diff --git a/node-hub/dora-pyrealsense/tests/test_dora_pyrealsense.py b/node-hub/dora-pyrealsense/tests/test_dora_pyrealsense.py new file mode 100644 index 000000000..932ccddd9 --- /dev/null +++ b/node-hub/dora-pyrealsense/tests/test_dora_pyrealsense.py @@ -0,0 +1,10 @@ +import pytest + + +def test_import_main(): + + from dora_pyrealsense.main import main + + # Check that everything is working, and catch dora Runtime Exception as we're not running in a dora dataflow. + with pytest.raises(ConnectionError): + main() diff --git a/node-hub/dora-rerun/Cargo.toml b/node-hub/dora-rerun/Cargo.toml index 9dbf5638a..1849c52c5 100644 --- a/node-hub/dora-rerun/Cargo.toml +++ b/node-hub/dora-rerun/Cargo.toml @@ -25,6 +25,7 @@ pyo3 = { workspace = true, features = [ "abi3", "eyre" ], optional = true } +bytemuck = "1.20.0" [lib] diff --git a/node-hub/dora-rerun/src/lib.rs b/node-hub/dora-rerun/src/lib.rs index 7357bd639..e62ba3133 100644 --- a/node-hub/dora-rerun/src/lib.rs +++ b/node-hub/dora-rerun/src/lib.rs @@ -4,7 +4,7 @@ use std::env::VarError; use dora_node_api::{ arrow::{ - array::{AsArray, Float32Array, StringArray, StructArray, UInt8Array}, + array::{AsArray, Float32Array, Float64Array, StringArray, StructArray, UInt8Array}, datatypes::Float32Type, }, DoraNode, Event, Parameter, @@ -126,6 +126,50 @@ pub fn lib_main() -> Result<()> { rec.log(id.as_str(), &image) .context("could not log image")?; }; + } else if id.as_str().contains("depth") { + let height = + if let Some(Parameter::Integer(height)) = metadata.parameters.get("height") { + height + } else { + &480 + }; + let width = + if let Some(Parameter::Integer(width)) = metadata.parameters.get("width") { + width + } else { + &640 + }; + // let focal_length = if let Some(Parameter::ListInt(focals)) = + // metadata.parameters.get("focal_length") + // { + // focals + // } else { + // &vec![605, 605] + // }; + // let resolutions = if let Some(Parameter::ListInt(resolutions)) = + // metadata.parameters.get("resolutions") + // { + // resolutions + // } else { + // &vec![640, 480] + // }; + let buffer: &Float64Array = data.as_any().downcast_ref().unwrap(); + let buffer: &[u8] = bytemuck::cast_slice(buffer.values()); + let image_buffer = ImageBuffer::try_from(buffer.to_vec()) + .context("Could not convert buffer to image buffer")?; + let image_format = + ImageFormat::depth([*width as _, *height as _], rerun::ChannelDatatype::F64); + + let depth_image = rerun::DepthImage::new(image_buffer, image_format) + .with_colormap(rerun::components::Colormap::Inferno); + + rec.log( + id.as_str().replace("/depth", ""), + &rerun::Pinhole::from_focal_length_and_resolution(&[605., 605.], &[640., 480.]), + )?; + + // If we log a pinhole camera model, the depth gets automatically back-projected to 3D + rec.log(id.as_str(), &depth_image)?; } else if id.as_str().contains("text") { let buffer: StringArray = data.to_data().into(); buffer.iter().try_for_each(|string| -> Result<()> {