From 9a732ea0024ca893702d6fa1ec6cb0e6ed0a89be Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Mon, 18 Nov 2024 10:31:00 +0100 Subject: [PATCH 01/10] Adding pyrealsense --- node-hub/dora-pyrealsense/README.md | 70 +++++++++++++++ .../dora_pyrealsense/__init__.py | 11 +++ .../dora-pyrealsense/dora_pyrealsense/main.py | 89 +++++++++++++++++++ node-hub/dora-pyrealsense/pyproject.toml | 21 +++++ .../tests/test_dora_pyrealsense.py | 9 ++ 5 files changed, 200 insertions(+) create mode 100644 node-hub/dora-pyrealsense/README.md create mode 100644 node-hub/dora-pyrealsense/dora_pyrealsense/__init__.py create mode 100644 node-hub/dora-pyrealsense/dora_pyrealsense/main.py create mode 100644 node-hub/dora-pyrealsense/pyproject.toml create mode 100644 node-hub/dora-pyrealsense/tests/test_dora_pyrealsense.py diff --git a/node-hub/dora-pyrealsense/README.md b/node-hub/dora-pyrealsense/README.md new file mode 100644 index 000000000..56c5bb15a --- /dev/null +++ b/node-hub/dora-pyrealsense/README.md @@ -0,0 +1,70 @@ +# Dora Node for capturing video with OpenCV + +This node is used to capture video from a camera using OpenCV. + +# 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..d566bbf97 --- /dev/null +++ b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py @@ -0,0 +1,89 @@ +import argparse +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 + +FLIP = os.getenv("FLIP", "") +DEVICE_ID = os.getenv("DEVICE_ID", "") +pipeline = rs.pipeline() +config = rs.config() +config.enable_device(DEVICE_ID) +config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 15) +align_to = rs.stream.color +align = rs.align(align_to) +pipeline.start(config) + + +def main(): + 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() + # 使用线程锁来确保只有一个线程能访问数据流 + # 获取各种数据流 + color_frames = frames.get_color_frame() + frame = np.asanyarray(color_frames.get_data()) + + 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 == "rgb8": + frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + 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()) + + node.send_output("image", storage, 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..bed0ab799 --- /dev/null +++ b/node-hub/dora-pyrealsense/tests/test_dora_pyrealsense.py @@ -0,0 +1,9 @@ +import pytest + + +def test_import_main(): + from opencv_video_capture.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(RuntimeError): + main() From ea42eee632284b63a0374ebf9be6c6205de7641f Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Mon, 18 Nov 2024 11:37:06 +0100 Subject: [PATCH 02/10] Adding camera for franka --- examples/franka/camera.yaml | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 examples/franka/camera.yaml diff --git a/examples/franka/camera.yaml b/examples/franka/camera.yaml new file mode 100644 index 000000000..543d60002 --- /dev/null +++ b/examples/franka/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 + env: + DEVICE_ID: 0 + 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 From 66427eb6586743159b5c7f756a699124a2341769 Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Mon, 18 Nov 2024 11:45:11 +0100 Subject: [PATCH 03/10] Adding checks for pyrealsense --- .github/workflows/node_hub_test.sh | 2 +- node-hub/dora-pyrealsense/README.md | 21 +++++++++- .../dora-pyrealsense/dora_pyrealsense/main.py | 38 ++++++++++++------- .../tests/test_dora_pyrealsense.py | 5 ++- 4 files changed, 48 insertions(+), 18 deletions(-) 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/node-hub/dora-pyrealsense/README.md b/node-hub/dora-pyrealsense/README.md index 56c5bb15a..d5d2f3cd4 100644 --- a/node-hub/dora-pyrealsense/README.md +++ b/node-hub/dora-pyrealsense/README.md @@ -1,6 +1,23 @@ -# Dora Node for capturing video with OpenCV +# Dora Node for capturing video with PyRealSense -This node is used to capture video from a camera using OpenCV. +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 diff --git a/node-hub/dora-pyrealsense/dora_pyrealsense/main.py b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py index d566bbf97..c23056f61 100644 --- a/node-hub/dora-pyrealsense/dora_pyrealsense/main.py +++ b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py @@ -1,4 +1,3 @@ -import argparse import os import time @@ -11,18 +10,30 @@ RUNNER_CI = True if os.getenv("CI") == "true" else False import pyrealsense2 as rs -FLIP = os.getenv("FLIP", "") -DEVICE_ID = os.getenv("DEVICE_ID", "") -pipeline = rs.pipeline() -config = rs.config() -config.enable_device(DEVICE_ID) -config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 15) -align_to = rs.stream.color -align = rs.align(align_to) -pipeline.start(config) - 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", "jpeg") + 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.") + + 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) + pipeline.start() + node = Node() start_time = time.time() @@ -45,6 +56,7 @@ def main(): # 获取各种数据流 color_frames = frames.get_color_frame() frame = np.asanyarray(color_frames.get_data()) + ## Change rgb to bgr if FLIP == "VERTICAL": frame = cv2.flip(frame, 0) @@ -69,8 +81,8 @@ def main(): metadata["height"] = int(frame.shape[0]) # Get the right encoding - if encoding == "rgb8": - frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + 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: diff --git a/node-hub/dora-pyrealsense/tests/test_dora_pyrealsense.py b/node-hub/dora-pyrealsense/tests/test_dora_pyrealsense.py index bed0ab799..932ccddd9 100644 --- a/node-hub/dora-pyrealsense/tests/test_dora_pyrealsense.py +++ b/node-hub/dora-pyrealsense/tests/test_dora_pyrealsense.py @@ -2,8 +2,9 @@ def test_import_main(): - from opencv_video_capture.main 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(RuntimeError): + with pytest.raises(ConnectionError): main() From 7c968a84f55077e3cb30c1544f1778817e6c4557 Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Tue, 19 Nov 2024 09:20:23 +0100 Subject: [PATCH 04/10] Adding realasense example --- examples/realsense/README.md | 10 ++++++++++ examples/realsense/camera.yaml | 19 +++++++++++++++++++ 2 files changed, 29 insertions(+) create mode 100644 examples/realsense/README.md create mode 100644 examples/realsense/camera.yaml 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..98e1f0a69 --- /dev/null +++ b/examples/realsense/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 From 2f342c9101326174431256590d09b86ef206ff2b Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Sun, 1 Dec 2024 20:59:56 +0100 Subject: [PATCH 05/10] Small fix within pyrealsense and add depth frame --- examples/realsense/camera.yaml | 1 + .../dora-pyrealsense/dora_pyrealsense/main.py | 36 ++++++++++++++----- 2 files changed, 29 insertions(+), 8 deletions(-) diff --git a/examples/realsense/camera.yaml b/examples/realsense/camera.yaml index 98e1f0a69..b63a9eef6 100644 --- a/examples/realsense/camera.yaml +++ b/examples/realsense/camera.yaml @@ -6,6 +6,7 @@ nodes: tick: dora/timer/millis/20 outputs: - image + - depth_image env: IMAGE_WIDTH: 640 IMAGE_HEIGHT: 480 diff --git a/node-hub/dora-pyrealsense/dora_pyrealsense/main.py b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py index c23056f61..4d63cbe01 100644 --- a/node-hub/dora-pyrealsense/dora_pyrealsense/main.py +++ b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py @@ -16,7 +16,7 @@ def main(): 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", "jpeg") + encoding = os.getenv("ENCODING", "rgb8") ctx = rs.context() devices = ctx.query_devices() if devices.size() == 0: @@ -25,14 +25,24 @@ def main(): # 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.") + 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) - pipeline.start() + 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() node = Node() start_time = time.time() @@ -52,10 +62,17 @@ def main(): if event_id == "tick": frames = pipeline.wait_for_frames() - # 使用线程锁来确保只有一个线程能访问数据流 - # 获取各种数据流 - color_frames = frames.get_color_frame() - frame = np.asanyarray(color_frames.get_data()) + 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": @@ -92,6 +109,9 @@ def main(): storage = pa.array(frame.ravel()) node.send_output("image", storage, metadata) + node.send_output( + "depth_image", pa.array(scaled_depth_image.ravel()), metadata + ) elif event_type == "ERROR": raise RuntimeError(event["error"]) From f38a7d725b726aa9e51d6ecb7b9f7cbbf16d702c Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Tue, 3 Dec 2024 13:21:17 +0100 Subject: [PATCH 06/10] Adding depth camera data --- node-hub/dora-rerun/Cargo.toml | 1 + node-hub/dora-rerun/src/lib.rs | 46 +++++++++++++++++++++++++++++++++- 2 files changed, 46 insertions(+), 1 deletion(-) 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..bb51cec37 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( + "world/camera", + &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("world/camera/depth", &depth_image)?; } else if id.as_str().contains("text") { let buffer: StringArray = data.to_data().into(); buffer.iter().try_for_each(|string| -> Result<()> { From b8413cee2e518433fe0d9d85d4f8484b8c85e657 Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Tue, 3 Dec 2024 13:22:10 +0100 Subject: [PATCH 07/10] add code about resolution and field of view --- node-hub/dora-pyrealsense/dora_pyrealsense/main.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/node-hub/dora-pyrealsense/dora_pyrealsense/main.py b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py index 4d63cbe01..ed18b4b97 100644 --- a/node-hub/dora-pyrealsense/dora_pyrealsense/main.py +++ b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py @@ -43,7 +43,10 @@ def main(): 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() @@ -107,10 +110,12 @@ def main(): continue storage = pa.array(frame.ravel()) - + # metadata["resolution"] = [rgb_intr.width, rgb_intr.height] + # metadata["focal_length"] = [rgb_intr.fx, rgb_intr.fy] + # metadata["principal_point"] = [rgb_intr.ppx, rgb_intr.ppy] node.send_output("image", storage, metadata) node.send_output( - "depth_image", pa.array(scaled_depth_image.ravel()), metadata + "depth", pa.array(scaled_depth_image.ravel()), metadata ) elif event_type == "ERROR": From 34f78cf38ff8660e7a0ae1e1ecda9f33527d1af7 Mon Sep 17 00:00:00 2001 From: haixuantao Date: Fri, 6 Dec 2024 17:08:11 +0000 Subject: [PATCH 08/10] Add example for realsense camera --- examples/realsense/camera.yaml | 2 +- examples/realsense/camera_rerun.yaml | 20 +++++++++++++++++++ .../dora-pyrealsense/dora_pyrealsense/main.py | 13 ++++++------ node-hub/dora-rerun/src/lib.rs | 4 ++-- 4 files changed, 30 insertions(+), 9 deletions(-) create mode 100644 examples/realsense/camera_rerun.yaml diff --git a/examples/realsense/camera.yaml b/examples/realsense/camera.yaml index b63a9eef6..ecffab45f 100644 --- a/examples/realsense/camera.yaml +++ b/examples/realsense/camera.yaml @@ -6,7 +6,7 @@ nodes: tick: dora/timer/millis/20 outputs: - image - - depth_image + - depth env: IMAGE_WIDTH: 640 IMAGE_HEIGHT: 480 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/dora_pyrealsense/main.py b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py index ed18b4b97..142cd0f42 100644 --- a/node-hub/dora-pyrealsense/dora_pyrealsense/main.py +++ b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py @@ -43,9 +43,9 @@ def main(): 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_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() @@ -110,9 +110,10 @@ def main(): continue storage = pa.array(frame.ravel()) - # metadata["resolution"] = [rgb_intr.width, rgb_intr.height] - # metadata["focal_length"] = [rgb_intr.fx, rgb_intr.fy] - # metadata["principal_point"] = [rgb_intr.ppx, rgb_intr.ppy] + + # 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 diff --git a/node-hub/dora-rerun/src/lib.rs b/node-hub/dora-rerun/src/lib.rs index bb51cec37..e62ba3133 100644 --- a/node-hub/dora-rerun/src/lib.rs +++ b/node-hub/dora-rerun/src/lib.rs @@ -164,12 +164,12 @@ pub fn lib_main() -> Result<()> { .with_colormap(rerun::components::Colormap::Inferno); rec.log( - "world/camera", + 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("world/camera/depth", &depth_image)?; + 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<()> { From 34cdbc6fceb81ef862d20c8ba371b6cf247bb3af Mon Sep 17 00:00:00 2001 From: haixuantao Date: Fri, 6 Dec 2024 17:09:00 +0000 Subject: [PATCH 09/10] fix cargo lock --- Cargo.lock | 1 + 1 file changed, 1 insertion(+) 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", From 3f7f7839c253593c07fa80e7e7a5b3ef4667e6b4 Mon Sep 17 00:00:00 2001 From: haixuantao Date: Mon, 9 Dec 2024 13:11:08 +0000 Subject: [PATCH 10/10] Remove device id from franka --- examples/franka/camera.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/franka/camera.yaml b/examples/franka/camera.yaml index 543d60002..98e1f0a69 100644 --- a/examples/franka/camera.yaml +++ b/examples/franka/camera.yaml @@ -7,7 +7,6 @@ nodes: outputs: - image env: - DEVICE_ID: 0 IMAGE_WIDTH: 640 IMAGE_HEIGHT: 480