From 7bfe430fda4278aaabe879fba57b3e61afdad35f Mon Sep 17 00:00:00 2001 From: Timo Date: Mon, 12 May 2025 20:34:07 +0200 Subject: [PATCH] added ros2 visualization --- src/main.rs | 41 +++++--------------- src/pipeline/hdb_clusterer.rs | 4 +- src/pipeline/highlight_extractor.rs | 59 ++++++++++++++++++++++++++--- src/pipeline/m_estimator.rs | 52 ++++++++++++++++++++++++- 4 files changed, 116 insertions(+), 40 deletions(-) diff --git a/src/main.rs b/src/main.rs index af763c9..d350616 100644 --- a/src/main.rs +++ b/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::("/camera/front", QosProfile::default()).unwrap(); - let mut timer = node.create_wall_timer(Duration::from_millis(1000)).unwrap(); + let lane_left_pub = node.create_publisher::("/lane_detection/left", QosProfile::default()).unwrap(); + let lane_right_pub = node.create_publisher::("/lane_detection/right", QosProfile::default()).unwrap(); + let pcd_pub = node.create_publisher::("/lane_detection/highlights", QosProfile::default()).unwrap(); let (img_tx, img_rx) = mpsc::sync_channel::>(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 = 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| { - msg.data = img.clone().into_bytes(); + .for_each(|msg| { 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); diff --git a/src/pipeline/hdb_clusterer.rs b/src/pipeline/hdb_clusterer.rs index 63f98b4..fd630cc 100644 --- a/src/pipeline/hdb_clusterer.rs +++ b/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::() / 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(); diff --git a/src/pipeline/highlight_extractor.rs b/src/pipeline/highlight_extractor.rs index 0f0c95f..d66267d 100644 --- a/src/pipeline/highlight_extractor.rs +++ b/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>>; trait ExtractHighlights { fn get_highlights(&self, threshold: u8, downsample: f64) -> Pointcloud; } + +trait Visualize{ + fn visualize(&self, pub_pcd: &Publisher); +} + 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::() < 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>, threshold: u8, downsample: f64) -> (Receiver, Arc>) { + +impl Visualize for Pointcloud { + fn visualize(&self, pub_pcd: &Publisher) { + 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::>(); + + 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>, threshold: u8, downsample: f64, pub_pcd: Publisher) -> (Receiver, Arc>) { let (tx, rx) = mpsc::sync_channel::(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>, 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(); } diff --git a/src/pipeline/m_estimator.rs b/src/pipeline/m_estimator.rs index efabb34..ffb314d 100644 --- a/src/pipeline/m_estimator.rs +++ b/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>; -pub fn create_mestimator_thread(lanes_rx: Receiver) -> (Receiver, Arc>) { + +trait Visualize { + fn visualize(&self, pub_left: &Publisher, pub_right: &Publisher); +} + +impl Visualize for Vec { + fn visualize(&self, pub_left: &Publisher, pub_right: &Publisher) { + let mut lane_left = Path { + header: Header {stamp: Time {sec: 0, nanosec: 0}, frame_id: "front_camera_link".to_string()}, + poses: Vec::::new() + }; + + let mut right_lane = Path { + header: Header {stamp: Time {sec: 0, nanosec: 0}, frame_id: "front_camera_link".to_string()}, + poses: Vec::::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, pub_left: Publisher, pub_right: Publisher) -> (Receiver, Arc>) { let (z_tx, z_rx) = mpsc::sync_channel::(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) -> (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(); } });