--wip--

Author: schmidma

Cargo.lock

@@ -265,6 +265,7 @@ watch = "0.2.3"
 webots = { version = "0.8.0" }
 wgpu = "24.0.5"
 xdg = "2.5.2"
+yuv = "0.8.9"
 zbus = "5.5.0"
 zed = { path = "crates/zed" }
 zenoh = "1.7.2"

Cargo.toml

@@ -6,7 +6,15 @@ use std::{
 
 use serde::Deserialize;
 
-use crate::{Cache, Timestamped, TopicStream};
+use crate::{stream::StreamError, Cache, Timestamped, TopicStream};
+
+#[derive(Debug, thiserror::Error)]
+pub enum BufferError {
+    #[error("Stream error: {0}")]
+    StreamError(StreamError),
+}
+
+pub type Result<T, E = BufferError> = std::result::Result<T, E>;
 
 #[derive(Clone)]
 pub struct TopicBuffer<T> {
@@ -17,15 +25,25 @@ impl<T> TopicBuffer<T>
 where
     for<'de> T: Deserialize<'de> + Timestamped + Clone + Send + 'static,
 {
-    pub fn new(mut stream: TopicStream<T>, capacity: usize) -> (Self, impl Future<Output = ()>) {
+    pub fn new(
+        mut stream: TopicStream<T>,
+        capacity: usize,
+    ) -> (Self, impl Future<Output = Result<()>>) {
         let cache = Arc::new(RwLock::new(Cache::new(capacity)));
 
         let driver = {
             let cache = cache.clone();
             async move {
-                while let Ok(msg) = stream.recv_async().await {
-                    let mut cache = cache.write().expect("lock is poisoned");
-                    cache.add(msg);
+                loop {
+                    match stream.recv_async().await {
+                        Ok(msg) => {
+                            let mut cache = cache.write().expect("lock is poisoned");
+                            cache.add(msg);
+                        }
+                        Err(err) => {
+                            return Err(BufferError::StreamError(err));
+                        }
+                    }
                 }
             }
         };

crates/hulkz/src/buffer.rs

@@ -11,7 +11,7 @@ pub enum PublisherError {
     Zenoh(#[from] zenoh::Error),
 }
 
-pub type Result<T> = std::result::Result<T, PublisherError>;
+pub type Result<T, E = PublisherError> = std::result::Result<T, E>;
 
 pub struct Publisher<'a, T>
 where

crates/hulkz/src/publisher.rs

@@ -3,15 +3,15 @@ use std::{future::Future, sync::Arc};
 use serde::{Deserialize, Serialize};
 use zenoh::handlers::RingChannel;
 
-use crate::{Publisher, Timestamped, TopicBuffer, TopicStream};
+use crate::{buffer::BufferError, Publisher, Timestamped, TopicBuffer, TopicStream};
 
 #[derive(Debug, thiserror::Error)]
 pub enum SessionError {
     #[error("Zenoh session error: {0}")]
     Zenoh(#[from] zenoh::Error),
 }
 
-pub type Result<T> = std::result::Result<T, SessionError>;
+pub type Result<T, E = SessionError> = std::result::Result<T, E>;
 
 pub struct Session {
     session: Arc<zenoh::Session>,
@@ -54,7 +54,10 @@ impl Session {
         &self,
         key_exp: &str,
         capacity: usize,
-    ) -> Result<(TopicBuffer<T>, impl Future<Output = ()>)>
+    ) -> Result<(
+        TopicBuffer<T>,
+        impl Future<Output = Result<(), BufferError>>,
+    )>
     where
         for<'de> T: Deserialize<'de> + Timestamped + Clone + Send + 'static,
     {

crates/hulkz/src/session.rs

@@ -10,7 +10,7 @@ pub enum StreamError {
     Zenoh(#[from] zenoh::Error),
 }
 
-pub type Result<T> = std::result::Result<T, StreamError>;
+pub type Result<T, E = StreamError> = std::result::Result<T, E>;
 
 pub struct TopicStream<T> {
     inner: ZenohSubscriber<RingChannelHandler<Sample>>,

crates/hulkz/src/stream.rs

@@ -9,7 +9,9 @@ homepage.workspace = true
 booster = { workspace = true }
 color-eyre = { workspace = true }
 hulkz = { workspace = true }
+image = { workspace = true }
 rerun = { workspace = true }
 ros2 = { workspace = true }
 tokio = { workspace = true }
 tracing-subscriber = { workspace = true }
+yuv = { workspace = true }

crates/simple-viewer/Cargo.toml

@@ -1,6 +1,8 @@
-use booster::LowState;
-use color_eyre::Result;
+use color_eyre::{eyre::bail, Result};
 use hulkz::Session;
+use image::{error::DecodingError, ImageError, RgbImage};
+use ros2::sensor_msgs::image::Image;
+use yuv::{yuv_nv12_to_rgb, YuvBiPlanarImage, YuvConversionMode, YuvRange, YuvStandardMatrix};
 
 #[tokio::main]
 async fn main() -> Result<()> {
@@ -15,24 +17,65 @@ async fn main() -> Result<()> {
 
     let session = Session::new().await?;
 
-    let mut low_state = session
-        .stream::<LowState>("HULK10/booster/low_state")
-        .await?;
+    let mut stream = session.stream::<Image>("booster/rectified_image").await?;
+
+    while let Ok(image) = stream.recv_async().await {
+        let y_plane_size = (image.step * image.height) as usize;
+        // UV plane is half height, but same stride as Y in NV12 (usually)
+        let uv_plane_size = (image.step * image.height / 2) as usize;
+
+        if image.data.len() < y_plane_size + uv_plane_size {
+            bail!("NV12: Source buffer is too small for the given dimensions");
+        }
+
+        // 2. Prepare Output Buffer
+        // RgbImage is a flattened Vec<u8> (R, G, B, R, G, B...)
+        let mut rgb_image = RgbImage::new(image.width, image.height);
+
+        // 3. Define Strides
+        // ROS 'step' is the stride for the Y plane.
+        let y_stride = image.step;
+        // NV12 UV plane usually has the same stride as Y
+        let uv_stride = image.step;
+        // RGB output stride (3 bytes per pixel * width)
+        let rgb_stride = image.width * 3;
+
+        // 4. Split Input Data into Planes
+        let (y_plane, remaining) = image.data.split_at(y_plane_size);
+        let uv_plane = &remaining[..uv_plane_size];
+
+        let yuv_bi_planar_image = YuvBiPlanarImage {
+            y_plane,
+            y_stride,
+            uv_plane,
+            uv_stride,
+            width: image.width,
+            height: image.height,
+        };
+
+        yuv_nv12_to_rgb(
+            &yuv_bi_planar_image,
+            rgb_image.as_flat_samples_mut().as_mut_slice(),
+            rgb_stride,
+            YuvRange::Limited, // Standard for video (16-235). Use 'Full' for JPEGs.
+            YuvStandardMatrix::Bt709, // Standard for HD Video. Use Bt601 for SD/Webcams.
+            YuvConversionMode::Balanced,
+        )
+        .map_err(|e| {
+            ImageError::Decoding(DecodingError::from_format_hint(
+                image::error::ImageFormatHint::Name(format!("NV12: {e}")),
+            ))
+        })?;
 
-    while let Ok(low_state) = low_state.recv_async().await {
-        let accelerometer = low_state.imu_state.linear_acceleration;
-        rec.log(
-            "booster/imu/accelerometer",
-            &rerun::archetypes::Scalars::new([
-                accelerometer.x(),
-                accelerometer.y(),
-                accelerometer.z(),
-            ]),
-        )?;
-        let rpy = low_state.imu_state.roll_pitch_yaw;
         rec.log(
-            "booster/imu/roll_pitch_yaw",
-            &rerun::archetypes::Scalars::new([rpy.x(), rpy.y(), rpy.z()]),
+            "booster/rectified_image",
+            &rerun::archetypes::Image::from_image(rgb_image)?,
+            // &rerun::archetypes::Image::from_color_model_and_bytes(
+            //     rgb_image.as_raw(),
+            //     [image.width, image.height],
+            //     rerun::ColorModel::RGB,
+            //     rerun::ChannelDatatype::U8,
+            // ),
         )?;
     }