added ros2 visualization

src/main.rs

@@ -9,10 +9,8 @@ use std::time::{Duration, Instant};
 use indicatif::{MultiProgress, ProgressBar, ProgressStyle};
 use r2r::QosProfile;
 use futures::{executor::LocalPool, future, stream::StreamExt, task::LocalSpawnExt};
-use image::{ImageReader, Rgb, RgbImage};
-use show_image::{create_window, ImageInfo, ImageView, PixelFormat};
 use crate::pipeline::hdb_clusterer::create_pointcloud_labels_thread;
-use crate::pipeline::highlight_extractor::{create_extract_thread, Pointcloud};
+use crate::pipeline::highlight_extractor::{create_extract_thread};
 use crate::pipeline::m_estimator::create_mestimator_thread;
 
 #[show_image::main]
@@ -20,7 +18,9 @@ fn main() {
     let ctx = r2r::Context::create().unwrap();
     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 lane_left_pub = node.create_publisher::<r2r::nav_msgs::msg::Path>("/lane_detection/left", QosProfile::default()).unwrap();
+    let lane_right_pub = node.create_publisher::<r2r::nav_msgs::msg::Path>("/lane_detection/right", QosProfile::default()).unwrap();
+    let pcd_pub = node.create_publisher::<r2r::sensor_msgs::msg::PointCloud2>("/lane_detection/highlights", QosProfile::default()).unwrap();
 
     let (img_tx, img_rx) = mpsc::sync_channel::<Arc<r2r::sensor_msgs::msg::Image>>(1);
 
@@ -28,9 +28,9 @@ fn main() {
     let mut pool = LocalPool::new();
     let spawner = pool.spawner();
 
-    let (pointcloud_rx, usage_extract) = create_extract_thread(img_rx, 196, 0.025);
+    let (pointcloud_rx, usage_extract) = create_extract_thread(img_rx, 32, 0.05, pcd_pub);
     let (cluster_labels_rx, usage_cluster) = create_pointcloud_labels_thread(pointcloud_rx);
-    let (z_rx, usage_detect) = create_mestimator_thread(cluster_labels_rx);
+    let (z_rx, usage_detect) = create_mestimator_thread(cluster_labels_rx, lane_left_pub, lane_right_pub);
 
     let multi_stats = MultiProgress::new();
     let p_style = ProgressStyle::default_bar().template("{prefix:15} {bar:50.gray/white} {pos:>6} us | {len:>6} us").unwrap();
@@ -45,12 +45,10 @@ fn main() {
     let mut t: VecDeque<Duration> = VecDeque::with_capacity(25);
     let mut now: Instant = Instant::now();
 
-    let img = ImageReader::open("./images/lane_detection_loop_80.png").unwrap().decode().unwrap();
+
-    // Run the subscriber in one task, printing the messages
     spawner.spawn_local(async move {
         subscriber
-            .for_each(|mut msg| {
+            .for_each(|msg| {
-                msg.data = img.clone().into_bytes();
                 img_tx.send(Arc::new(msg)).unwrap();
                 future::ready(())
             })
@@ -58,28 +56,7 @@ fn main() {
     }).unwrap();
 
     thread::spawn(move || {
-        let window = create_window("image", Default::default()).unwrap();
         for z in z_rx {
-            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]);
-                }
-            });
-
-            let image = ImageView::new(ImageInfo::new(PixelFormat::Rgb8, 800, 800), rgb_image.iter().as_slice());
-            window.set_image("camera", image).unwrap();
 
         }
     });
@@ -93,7 +70,7 @@ fn main() {
         let u_detect = *usage_detect.lock().unwrap();
 
         let u_vec = vec![u_extract, u_cluster, u_detect];
-        
+
         p_extract.set_length(u_extract.1.as_micros() as u64);
         p_extract.set_position(u_extract.0.as_micros() as u64);
         p_cluster.set_length(u_cluster.1.as_micros() as u64);

src/pipeline/hdb_clusterer.rs

@@ -26,7 +26,7 @@ impl SortLabeled for PointcloudLabeled {
             if !averages.contains_key(label) {
                 averages.insert(*label, Vec::new());
             }
-            averages.get_mut(label).unwrap().push(point[1]);
+            averages.get_mut(label).unwrap().push(point[0]);
         });
 
         let mut average: Vec<(i32, f64)> = averages.into_iter().map(|(label, y_values)| (label, y_values.iter().sum::<f64>() / y_values.len() as f64)).collect();
@@ -49,7 +49,7 @@ impl Label for Pointcloud {
             .epsilon(10000.0)
             .min_samples(3)
             .allow_single_cluster(false)
-            .min_cluster_size(20)
+            .min_cluster_size(30)
             .build();
 
         let labels = Hdbscan::new(self, params).cluster().unwrap();

src/pipeline/highlight_extractor.rs

@@ -2,16 +2,26 @@ use std::sync::{mpsc, Arc, Mutex};
 use std::sync::mpsc::Receiver;
 use std::thread;
 use std::time::{Duration, Instant};
+use r2r::Publisher;
 use rand::{rng, Rng};
 use r2r::sensor_msgs::msg::Image;
+use r2r::sensor_msgs::msg::{PointField, PointCloud2};
+use r2r::builtin_interfaces::msg::Time;
+use r2r::std_msgs::msg::Header;
 
 pub type Pointcloud = Arc<Vec<Vec<f64>>>;
 
 trait ExtractHighlights {
     fn get_highlights(&self, threshold: u8, downsample: f64) -> Pointcloud;
 }
+
+trait Visualize{
+    fn visualize(&self, pub_pcd: &Publisher<PointCloud2>);
+}
+
 impl ExtractHighlights for Image {
     fn get_highlights(&self, threshold: u8, downsample: f64) -> Pointcloud {
+
         let h_i = [
             [-7.74914499e-03, 3.95733793e-18, 3.10353257e00],
             [8.56519716e-18, 9.42313768e-05, -1.86052093e00],
@@ -24,7 +34,7 @@ impl ExtractHighlights for Image {
             .iter()
             .enumerate() 
             .map(|(i, pixel)| (pixel, i as f64 % self.width as f64, i as f64 / self.width as f64))
-            .filter(|(pixel, _, _)| **pixel > threshold)
+            .filter(|(pixel, _, h)| **pixel > threshold && **pixel < 100 && *h > self.height as f64 * 0.6)
             .filter(|_| {
                 rng.random::<u32>() < max_rng
             })
@@ -40,11 +50,49 @@ impl ExtractHighlights for Image {
 
                  vec![u_norm, v_norm]
             })
-            //.filter(|point| point[0].abs() <= 5.0)
+            .filter(|point| point[0].abs() <= 4.0)
             .collect())
     }
 }
-pub fn create_extract_thread(image_rx: Receiver<Arc<Image>>, threshold: u8, downsample: f64) -> (Receiver<Pointcloud>, Arc<Mutex<(Duration, Duration)>>) {
+
+impl Visualize for Pointcloud {
+    fn visualize(&self, pub_pcd: &Publisher<PointCloud2>) {
+        let header = Header {
+            stamp: Time {
+                sec: 0,
+                nanosec: 0,
+            },
+            frame_id: "front_camera_link".to_string(),
+        };
+
+        let pcd_bytes = self.
+            iter()
+            .flat_map(|point| vec![point[1], -point[0], -0.7])
+            .map(|f| (f as f32).to_le_bytes())
+            .flatten()
+            .collect::<Vec<u8>>();
+
+        let msg = PointCloud2 {
+            header,
+            height: 1,
+            width: self.len() as u32,
+            fields: vec![
+                PointField { name: "x".to_string(), offset: 0, datatype: 7, count: 1 },
+                PointField { name: "y".to_string(), offset: 4, datatype: 7, count: 1 },
+                PointField { name: "z".to_string(), offset: 8, datatype: 7, count: 1 },
+            ],
+            is_bigendian: false,
+            point_step: 12,
+            row_step: 12,
+            data: pcd_bytes,
+            is_dense: true,
+        };
+
+        pub_pcd.publish(&msg).unwrap();
+    }
+}
+
+pub fn create_extract_thread(image_rx: Receiver<Arc<Image>>, threshold: u8, downsample: f64, pub_pcd: Publisher<PointCloud2>) -> (Receiver<Pointcloud>, Arc<Mutex<(Duration, Duration)>>) {
     let (tx, rx) = mpsc::sync_channel::<Pointcloud>(1);
     let usage = Arc::new(Mutex::new((Duration::ZERO, Duration::ZERO)));
     let local_usage = usage.clone();
@@ -55,9 +103,10 @@ pub fn create_extract_thread(image_rx: Receiver<Arc<Image>>, threshold: u8, down
             let start = Instant::now();
             let poi = image.get_highlights(threshold, downsample);
             let end = Instant::now();
-            
+
+            poi.visualize(&pub_pcd);
             tx.send(poi).unwrap();
-            
+
             *local_usage.lock().unwrap() = (end - start, t_loop.elapsed());
             t_loop = Instant::now();
         }

src/pipeline/m_estimator.rs

@@ -5,9 +5,56 @@ use std::time::{Duration, Instant};
 use ndarray::{Array1, Array2};
 use crate::pipeline::hdb_clusterer::PointcloudLabeled;
 use ndarray_linalg::Inverse;
+use r2r::builtin_interfaces::msg::Time;
+use r2r::std_msgs::msg::Header;
+use r2r::nav_msgs::msg::Path;
+use r2r::geometry_msgs::msg::{Point, Pose, PoseStamped, Quaternion};
+use r2r::Publisher;
 
 pub type LaneEstimation = Arc<Vec<f64>>;
-pub fn create_mestimator_thread(lanes_rx: Receiver<PointcloudLabeled>) -> (Receiver<LaneEstimation>, Arc<Mutex<(Duration, Duration)>>) {
+
+trait Visualize {
+    fn visualize(&self, pub_left: &Publisher<Path>, pub_right: &Publisher<Path>);
+}
+
+impl Visualize for Vec<f64> {
+    fn visualize(&self, pub_left: &Publisher<Path>, pub_right: &Publisher<Path>) {
+        let mut lane_left = Path {
+            header: Header {stamp: Time {sec: 0, nanosec: 0}, frame_id: "front_camera_link".to_string()},
+            poses: Vec::<PoseStamped>::new()
+        };
+
+        let mut right_lane = Path {
+            header: Header {stamp: Time {sec: 0, nanosec: 0}, frame_id: "front_camera_link".to_string()},
+            poses: Vec::<PoseStamped>::new()
+        };
+
+
+        (10..250).for_each(|x| {
+            let x_m = x as f64 / 10.0;
+            let const_part = - self[1] - x_m * self[2] + 0.5 * self[3] * x_m.powi(2);
+            let l = 0.5 * self[0]  + const_part;
+            let r = -0.5 * self[0] + const_part;
+
+            if l.abs() < 10.0 {
+                lane_left.poses.push(PoseStamped {header: lane_left.header.clone(), pose: Pose {
+                    position: Point {x: x_m, y: -l, z: -0.7},
+                    orientation: Quaternion {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
+                }})};
+
+            if r.abs() < 10.0 {
+                right_lane.poses.push(PoseStamped {header: lane_left.header.clone(), pose: Pose {
+                    position: Point {x: x_m, y: -r, z: -0.7},
+                    orientation: Quaternion {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
+                }})};
+        });
+
+        pub_left.publish(&lane_left).unwrap();
+        pub_right.publish(&right_lane).unwrap();
+    }
+}
+
+pub fn create_mestimator_thread(lanes_rx: Receiver<PointcloudLabeled>, pub_left: Publisher<Path>, pub_right: Publisher<Path>) -> (Receiver<LaneEstimation>, Arc<Mutex<(Duration, Duration)>>) {
     let (z_tx, z_rx) = mpsc::sync_channel::<LaneEstimation>(1);
     let usage = Arc::new(Mutex::new((Duration::ZERO, Duration::ZERO)));
     let local_usage = usage.clone();
@@ -50,6 +97,9 @@ pub fn create_mestimator_thread(lanes_rx: Receiver<PointcloudLabeled>) -> (Recei
             let end = Instant::now();
             z_tx.send(Arc::new(z.clone())).unwrap();
             *local_usage.lock().unwrap() = (end - start, t_loop.elapsed());
+
+            z.visualize(&pub_left, &pub_right);
+
             t_loop = Instant::now();
         }
     });