wip: add ros2 interface

Cargo.toml

@@ -14,4 +14,7 @@ rand = "0.9.1"
 rayon = "1.10.0"
 hdbscan = "0.10.0"
 nalgebra = "0.33.2"
-indicatif = "0.17.11"
+indicatif = "0.17.11"
+r2r = "0.9.5"
+futures-core = "0.3.31"
+futures = "0.3.31"

src/main.rs

@@ -13,6 +13,8 @@ use std::time::{Duration, Instant};
 use indicatif::{MultiProgress, ProgressBar, ProgressStyle};
 use nalgebra::{DMatrix};
 use ndarray::{Array1, Array2};
+use r2r::QosProfile;
+use futures::{executor::LocalPool, future, stream::StreamExt, task::LocalSpawnExt};
 
 fn create_extract_thread(image_rx: Receiver<Arc<Vec<u8>>>, width: u32, threshold: u8, downsample: f64) -> (Receiver<Arc<Vec<Vec<f64>>>>, Arc<Mutex<(Duration, Duration)>>) {
     let (tx, rx) = mpsc::sync_channel::<Arc<Vec<Vec<f64>>>>(1);
@@ -269,8 +271,28 @@ fn dummy_image_spawner_thread() -> Receiver<Arc<Vec<u8>>> {
 
 #[show_image::main]
 fn main() {
-    let image_rx = dummy_image_spawner_thread();
+    let ctx = r2r::Context::create().unwrap();
-    let (pointcloud_rx, usage_extract) = create_extract_thread(image_rx, 800, 196, 0.025);
+    let mut node = r2r::Node::create(ctx, "node", "").unwrap();
+    let subscriber = node.subscribe::<r2r::sensor_msgs::msg::Image>("/camera/front", QosProfile::default()).unwrap();
+    let mut timer = node.create_wall_timer(Duration::from_millis(1000)).unwrap();
+
+    let (img_tx, img_rx) = mpsc::sync_channel::<Arc<Vec<u8>>>(1);
+
+    // Set up a simple task executor.
+    let mut pool = LocalPool::new();
+    let spawner = pool.spawner();
+
+    // Run the subscriber in one task, printing the messages
+    spawner.spawn_local(async move {
+        subscriber
+            .for_each(|msg| {
+                img_tx.send(Arc::new(msg.data)).unwrap();
+                future::ready(())
+            })
+            .await
+    }).unwrap();
+
+    let (pointcloud_rx, usage_extract) = create_extract_thread(img_rx, 800, 196, 0.025);
     let (projection_rx, usage_transform) = create_transform_thread(pointcloud_rx);
     let (cluster_labels_rx, usage_cluster) = create_pointcloud_labels_thread(projection_rx);
     let (clusters_rx, usage_split) = create_cluster_separator_thread(cluster_labels_rx);
@@ -294,7 +316,9 @@ fn main() {
     let mut t: VecDeque<Duration> = VecDeque::with_capacity(25);
     let mut now: Instant = Instant::now();
 
-    for (z, lanes) in z_rx {
+    loop {
+        node.spin_once(Duration::from_millis(10));
+        pool.run_until_stalled();
 
         let u_extract = *usage_extract.lock().unwrap();
         let u_transform = *usage_transform.lock().unwrap();
@@ -322,42 +346,6 @@ fn main() {
         p_avg_ms.set_position(u_vec.iter().map(|(t, _)| t).sum::<Duration>().as_micros() as u64);
         p_framerate.set_position((1.0 / (t.iter().map(|d| d.as_secs_f64()).sum::<f64>() / t.len() as f64)) as u64);
 
-        let mut rgb_image = RgbImage::new(800, 800);
-
-        let colors = vec![Rgb([0, 255, 0]), Rgb([255, 0, 0])];
-
-        (0..rgb_image.height()).for_each(|x| {
-            let x_m = x as f64 * (45.0 / 800.0);
-            let const_part = - z[1] - x_m * z[2] + 0.5 * z[3] * x_m.powi(2);
-            let l = (0.5 * z[0]  + const_part) * (800.0/45.0);;
-            let r = (-0.5 * z[0] + const_part) * (800.0/45.0);;
-
-            if l.abs() < rgb_image.width() as f64 / 2.0 - 1.0 {
-                rgb_image.put_pixel((rgb_image.width() as f64 /2.0 + l) as u32, rgb_image.height() - 1 - x, colors[0]);
-            }
-            if r.abs() < rgb_image.width() as f64 / 2.0 - 1.0 {
-                rgb_image.put_pixel((rgb_image.width() as f64/2.0 + r) as u32, rgb_image.height() - 1 - x, colors[1]);
-            }
-        });
-
-        lanes[0].iter().for_each(|point| {
-            let x = point[0] * 800.0/45.0 + rgb_image.width() as f64 / 2.0;
-            let y = rgb_image.height() as f64 - 1.0 - point[1] * 800.0/45.0;
-            if x < rgb_image.width() as f64 && y < rgb_image.width() as f64 {
-                rgb_image.put_pixel(x as u32, y as u32, Rgb([0, 255, 255]));
-            }
-        });
-
-        lanes[1].iter().for_each(|point| {
-            let x = point[0] * 800.0/45.0 + rgb_image.width() as f64 / 2.0;
-            let y = rgb_image.height() as f64 - 1.0 - point[1] * 800.0/45.0;
-            if x < rgb_image.width() as f64 && y < rgb_image.width() as f64 {
-                rgb_image.put_pixel(x as u32, y as u32, Rgb([255, 255, 0]));
-            }
-        });
-
-        let image = ImageView::new(ImageInfo::new(PixelFormat::Rgb8, 800, 800), rgb_image.iter().as_slice());
-        window.set_image("camera", image).unwrap();
 
         if t.len() == 25 {
             t.pop_front();