From 866bc890a6ed8d67c1d7884b4f5ae6f663df2470 Mon Sep 17 00:00:00 2001 From: Timo Schneider Date: Tue, 13 May 2025 21:58:37 +0200 Subject: [PATCH] asd lab --- src/main.rs | 28 +++++++++++----------------- src/pipeline/hdb_clusterer.rs | 4 ++-- src/pipeline/highlight_extractor.rs | 17 ++++++++--------- src/pipeline/m_estimator.rs | 4 +++- 4 files changed, 24 insertions(+), 29 deletions(-) diff --git a/src/main.rs b/src/main.rs index d350616..70529ed 100644 --- a/src/main.rs +++ b/src/main.rs @@ -2,16 +2,16 @@ mod pipeline; use rand::Rng; use rayon::prelude::*; -use std::collections::VecDeque; -use std::sync::{mpsc, Arc, Mutex}; +use std::sync::{mpsc, Arc}; use std::thread; -use std::time::{Duration, Instant}; +use std::time::{Duration}; use indicatif::{MultiProgress, ProgressBar, ProgressStyle}; use r2r::QosProfile; use futures::{executor::LocalPool, future, stream::StreamExt, task::LocalSpawnExt}; use crate::pipeline::hdb_clusterer::create_pointcloud_labels_thread; use crate::pipeline::highlight_extractor::{create_extract_thread}; use crate::pipeline::m_estimator::create_mestimator_thread; +use r2r::geometry_msgs::msg::Vector3; #[show_image::main] fn main() { @@ -21,6 +21,7 @@ fn main() { 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 cmd_vel_pub = node.create_publisher::("/cmd_vel_in", QosProfile::default()).unwrap(); let (img_tx, img_rx) = mpsc::sync_channel::>(1); @@ -40,11 +41,6 @@ fn main() { let p_max = multi_stats.add(ProgressBar::new(100).with_prefix("Pipeline load").with_style(ProgressStyle::default_bar().template("{prefix:15} {bar:50.red/white} {percent:>3}%").unwrap())); let p_avg_ms = multi_stats.add(ProgressBar::new(100_000).with_prefix("Pipeline delay").with_style(ProgressStyle::default_bar().template("{prefix:15} {bar:50.cyan/white} {pos:>6} us").unwrap())); - let p_framerate = multi_stats.add(ProgressBar::new(150).with_prefix("Framerate").with_style(ProgressStyle::default_bar().template("{prefix:15} {bar:50.blue/white} {pos:>3} FPS").unwrap())); - - let mut t: VecDeque = VecDeque::with_capacity(25); - let mut now: Instant = Instant::now(); - spawner.spawn_local(async move { subscriber @@ -57,12 +53,17 @@ fn main() { thread::spawn(move || { for z in z_rx { - + let x_m = 1.0; + let cmd_vel = r2r::geometry_msgs::msg::Twist { + linear: Vector3 {x: 1.0, y: 0.0, z: 0.0}, + angular: Vector3 {x: 0.0, y: 0.0, z: -2.0 * ((-z[1] - x_m * z[2] + 0.5 * z[3] * x_m.powi(2)) / x_m).atan()}, + }; + cmd_vel_pub.publish(&cmd_vel).unwrap(); } }); loop { - node.spin_once(Duration::from_millis(10)); + node.spin_once(Duration::from_millis(16)); pool.run_until_stalled(); let u_extract = *usage_extract.lock().unwrap(); @@ -80,13 +81,6 @@ fn main() { p_max.set_position(u_vec.iter().map(|(t, d)| ((t.as_secs_f64() / d.as_secs_f64()) * 100.0) as u64).max().unwrap()); p_avg_ms.set_position(u_vec.iter().map(|(t, _)| t).sum::().as_micros() as u64); - p_framerate.set_position((1.0 / (t.iter().map(|d| d.as_secs_f64()).sum::() / t.len() as f64)) as u64); - - if t.len() == 25 { - t.pop_front(); - } - t.push_back(now.elapsed()); - now = Instant::now(); } } diff --git a/src/pipeline/hdb_clusterer.rs b/src/pipeline/hdb_clusterer.rs index 0254015..fed50e1 100644 --- a/src/pipeline/hdb_clusterer.rs +++ b/src/pipeline/hdb_clusterer.rs @@ -43,13 +43,13 @@ impl SortLabeled for PointcloudLabeled { impl Label for Pointcloud { fn label(&self) -> Option { - if self.len() < 75 { return None } + if self.len() < 125 { return None } let params = HdbscanHyperParams::builder() .epsilon(10000.0) .min_samples(3) .allow_single_cluster(false) - .min_cluster_size(30) + .min_cluster_size(10) .build(); if let Ok(labels) = Hdbscan::new(self, params).cluster() { diff --git a/src/pipeline/highlight_extractor.rs b/src/pipeline/highlight_extractor.rs index d66267d..4f9fa23 100644 --- a/src/pipeline/highlight_extractor.rs +++ b/src/pipeline/highlight_extractor.rs @@ -21,20 +21,19 @@ trait Visualize{ 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], - [2.57498016e-18, -2.73825295e-03, 1.00000000e00], + [-1.31918135e-20, -1.55163853e-06, 2.44274454e-02], + [-5.89622642e-05, -3.10327706e-07, 2.37534136e-02], + [-5.57265378e-21, 6.51688183e-05, -2.59527061e-02], ]; - + let max_rng = (u32::MAX as f64 * downsample) as u32; let mut rng = rand::rng(); Arc::new(self.data .iter() .enumerate() .map(|(i, pixel)| (pixel, i as f64 % self.width as f64, i as f64 / self.width as f64)) - .filter(|(pixel, _, h)| **pixel > threshold && **pixel < 100 && *h > self.height as f64 * 0.6) + .filter(|(pixel, _, h)| **pixel > threshold && **pixel < 100 && *h > self.height as f64 * 0.4) .filter(|_| { rng.random::() < max_rng }) @@ -46,11 +45,11 @@ impl ExtractHighlights for Image { let w = h_i[2][0] * x + h_i[2][1] * y + h_i[2][2]; let u_norm = u / w; - let v_norm = v / w; + let v_norm = -v / w; - vec![u_norm, v_norm] + vec![v_norm, u_norm] }) - .filter(|point| point[0].abs() <= 4.0) + .filter(|point| point[0].abs() <= 1.5 && point[1] <= 3.0) .collect()) } } diff --git a/src/pipeline/m_estimator.rs b/src/pipeline/m_estimator.rs index ffb314d..d66cd6e 100644 --- a/src/pipeline/m_estimator.rs +++ b/src/pipeline/m_estimator.rs @@ -91,7 +91,9 @@ pub fn create_mestimator_thread(lanes_rx: Receiver, pub_left: w[[i, i]] = diag_value; } - z = H_t.dot(&w).dot(&H).inv().unwrap().dot(&H_t).dot(&w).dot(&y).to_vec(); + if let Ok(first_part) = H_t.dot(&w).dot(&H).inv() { + z = first_part.dot(&H_t).dot(&w).dot(&y).to_vec(); + } } let end = Instant::now();