Reintroduce vision cycler (HULKs#2293)

* Reintroduce vision cycler

* Fix map panel lines overlay

* fmt

* Fix required inputs breaking in replay

* use width instead of step

* fix nv12 to YCbCr422 conversion

* Rename `RawOrJpeg` -> `ImageBuffer`

Author: rmburg

Cargo.lock

@@ -47,6 +47,7 @@ members = [
   "crates/simulation_message",
   "crates/source_analyzer",
   "crates/types",
+  "crates/vision",
   "crates/walking_inference",
   "crates/world_state",
   "crates/zed",
@@ -195,6 +196,7 @@ quote = "1.0.38"
 rand = "0.9.0"
 rand_chacha = { version = "0.9.0", features = ["serde"] }
 rand_distr = "0.5.0"
+ransac = { path = "crates/ransac" }
 regex = "1.11.1"
 repository = { path = "crates/repository" }
 reqwest = { version = "0.12.12", features = ["blocking"] }
@@ -229,6 +231,7 @@ tracing = "0.1.41"
 tracing-subscriber = "0.3.19"
 types = { path = "crates/types" }
 uuid = { version = "1.12.1", features = ["v4"] }
+vision = { path = "crates/vision" }
 walking_inference = { path = "crates/walking_inference" }
 wgpu = "27.0.0"
 world_state = { path = "crates/world_state" }

Cargo.toml

@@ -1370,7 +1370,7 @@ fn generate_context_initializers(node: &Node, cycler: &Cycler, mode: CyclerMode)
                                 CyclerMode::Replay => {
                                     let name = path_to_extraction_variable_name(cycler_instance, path, "required_input");
                                     quote! {
-                                        &#name .unwrap()
+                                        #name .as_ref().unwrap()
                                     }
                                 },
                             }

crates/code_generation/src/cyclers.rs

@@ -59,6 +59,7 @@ tokio = { workspace = true }
 tokio-tungstenite = { workspace = true }
 tokio-util = { workspace = true }
 types = { workspace = true }
+vision = { workspace = true }
 walking_inference = { workspace = true }
 world_state = { workspace = true }
 zenoh = { workspace = true }

crates/hulk_booster/Cargo.toml

@@ -42,6 +42,7 @@ serde = { workspace = true }
 serde_json = { workspace = true }
 tokio = { workspace = true }
 types = { workspace = true }
+vision = { workspace = true }
 walking_inference = { workspace = true }
 world_state = { workspace = true }
 zed = { workspace = true }

crates/hulk_imagine/Cargo.toml

@@ -9,6 +9,19 @@ use source_analyzer::{
 pub fn collect_hulk_cyclers(root: impl AsRef<Path>) -> Result<Cyclers, Error> {
     let manifest = FrameworkManifest {
         cyclers: vec![
+            CyclerManifest {
+                name: "Vision",
+                kind: CyclerKind::Perception,
+                instances: vec![""],
+                setup_nodes: vec!["vision::image_receiver"],
+                nodes: vec![
+                    "vision::field_border_detection",
+                    "vision::image_segmenter",
+                    "vision::line_detection",
+                    "vision::segment_filter",
+                ],
+                execution_time_warning_threshold: Some(Duration::from_secs_f32(1.0 / 30.0)),
+            },
             CyclerManifest {
                 name: "ObjectDetection",
                 kind: CyclerKind::Perception,

crates/hulk_manifest/src/lib.rs

@@ -53,6 +53,7 @@ tokio = { workspace = true }
 tokio-tungstenite = { workspace = true }
 tokio-util = { workspace = true }
 types = { workspace = true }
+vision = { workspace = true }
 walking_inference = { workspace = true }
 world_state = { workspace = true }
 

crates/hulk_mujoco/Cargo.toml

@@ -49,6 +49,7 @@ serde_json = { workspace = true }
 tokio = { workspace = true }
 tokio-util = { workspace = true }
 types = { workspace = true }
+vision = { workspace = true }
 walking_inference = { workspace = true }
 world_state = { workspace = true }
 

crates/hulk_replayer/Cargo.toml

@@ -7,7 +7,8 @@ use std::{
 
 use color_eyre::eyre::{self, WrapErr};
 use geometry::circle::Circle;
-use image::{ImageReader, RgbImage};
+use image::{ImageError, ImageReader, RgbImage, error::DecodingError};
+use num_traits::Euclid;
 use serde::{Deserialize, Serialize};
 
 use coordinate_systems::Pixel;
@@ -33,12 +34,12 @@ pub struct YCbCr422Image {
     buffer: Arc<Vec<YCbCr422>>,
 }
 
-impl From<RgbImage> for YCbCr422Image {
-    fn from(rgb_image: RgbImage) -> Self {
+impl From<&RgbImage> for YCbCr422Image {
+    fn from(rgb_image: &RgbImage) -> Self {
         let width_422 = rgb_image.width() / 2;
         let height = rgb_image.height();
         let data = rgb_image
-            .into_vec()
+            .to_vec()
             .chunks(6)
             .map(|pixel| {
                 let left_color: YCbCr444 = Rgb {
@@ -110,15 +111,17 @@ impl From<YCbCr422Image> for RgbImage {
     }
 }
 
-impl From<&Ros2Image> for YCbCr422Image {
-    fn from(ros2_image: &Ros2Image) -> Self {
+impl TryFrom<&Ros2Image> for YCbCr422Image {
+    type Error = ImageError;
+
+    fn try_from(ros2_image: &Ros2Image) -> Result<Self, ImageError> {
         let width_422 = ros2_image.width / 2;
         let height = ros2_image.height;
 
-        let data = match ros2_image.encoding.as_str() {
+        let data: Vec<YCbCr422> = match ros2_image.encoding.as_str() {
             "rgb8" => ros2_image
                 .data
-                .chunks(6)
+                .chunks_exact(6)
                 .map(|pixel| {
                     let left_color: YCbCr444 = Rgb {
                         red: pixel[0],
@@ -135,14 +138,61 @@ impl From<&Ros2Image> for YCbCr422Image {
                     [left_color, right_color].into()
                 })
                 .collect(),
-            _ => unimplemented!("image encoding not supported"),
+            "nv12" => {
+                let y_plane_size = (ros2_image.width * ros2_image.height) as usize;
+                let uv_plane_size = (ros2_image.width * ros2_image.height / 2) as usize;
+
+                if ros2_image.data.len() < y_plane_size + uv_plane_size {
+                    return Err(ImageError::Decoding(DecodingError::from_format_hint(
+                        image::error::ImageFormatHint::Name(
+                            "NV12: Source buffer is too small for the given dimensions".to_string(),
+                        ),
+                    )));
+                }
+
+                let y_stride = ros2_image.width;
+                let chunked_y_stride = y_stride / 2;
+
+                let (y_plane, uv_plane) = ros2_image.data.split_at(y_plane_size);
+
+                assert_eq!(uv_plane.len(), uv_plane_size);
+
+                y_plane
+                    .chunks_exact(2)
+                    .enumerate()
+                    .map(|(i, y_chunk)| {
+                        let (y_row, column) = (i as u32).div_rem_euclid(&chunked_y_stride);
+                        let uv_row = y_row / 2;
+
+                        assert!(uv_row <= ros2_image.height / 2);
+                        assert!(column <= ros2_image.width / 2);
+
+                        // don't forget sunscreen
+                        let uv_index = uv_row * chunked_y_stride + column;
+                        let uv_byte_index = uv_index as usize * 2;
+
+                        assert!(uv_byte_index < uv_plane_size);
+
+                        let cb = uv_plane[uv_byte_index];
+                        let cr = uv_plane[uv_byte_index + 1];
+
+                        YCbCr422 {
+                            y1: y_chunk[0],
+                            cb,
+                            y2: y_chunk[1],
+                            cr,
+                        }
+                    })
+                    .collect()
+            }
+            encoding => unimplemented!(r#"image encoding "{encoding}" not supported"#),
         };
 
-        Self {
+        Ok(Self {
             width_422,
             height,
             buffer: Arc::new(data),
-        }
+        })
     }
 }
 
@@ -225,7 +275,7 @@ impl YCbCr422Image {
 
     pub fn load_from_rgb_file(path: impl AsRef<Path>) -> eyre::Result<Self> {
         let rgb_image = ImageReader::open(path)?.decode()?.into_rgb8();
-        Ok(Self::from(rgb_image))
+        Ok(Self::from(&rgb_image))
     }
 
     pub fn save_to_rgb_file(&self, file: impl AsRef<Path> + Debug) -> eyre::Result<()> {

crates/types/src/ycbcr422_image.rs

@@ -0,0 +1,29 @@
+[package]
+name = "vision"
+version.workspace = true
+edition.workspace = true
+license.workspace = true
+homepage.workspace = true
+
+[dependencies]
+approx = { workspace = true }
+calibration = { workspace = true }
+color-eyre = { workspace = true }
+context_attribute = { workspace = true }
+coordinate_systems = { workspace = true }
+filtering = { workspace = true }
+framework = { workspace = true }
+geometry = { workspace = true }
+hardware = { workspace = true }
+image = { workspace = true }
+itertools = { workspace = true }
+linear_algebra = { workspace = true }
+nalgebra = { workspace = true }
+ordered-float = { workspace = true }
+projection = { workspace = true }
+rand = { workspace = true }
+rand_chacha = { workspace = true }
+ransac = { workspace = true }
+ros2 = { workspace = true }
+serde = { workspace = true }
+types = { workspace = true }