asd lab

src/main.rs

@@ -2,16 +2,16 @@ mod pipeline;
 
 use rand::Rng;
 use rayon::prelude::*;
-use std::collections::VecDeque;
+use std::sync::{mpsc, Arc};
-use std::sync::{mpsc, Arc, Mutex};
 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::<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 cmd_vel_pub = node.create_publisher::<r2r::geometry_msgs::msg::Twist>("/cmd_vel_in", QosProfile::default()).unwrap();
 
     let (img_tx, img_rx) = mpsc::sync_channel::<Arc<r2r::sensor_msgs::msg::Image>>(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<Duration> = 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::<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);
-
-        if t.len() == 25 {
-            t.pop_front();
-        }
-        t.push_back(now.elapsed());
-        now = Instant::now();
     }
 
 }

src/pipeline/hdb_clusterer.rs

@@ -43,13 +43,13 @@ impl SortLabeled for PointcloudLabeled {
 
 impl Label for Pointcloud {
     fn label(&self) -> Option<PointcloudLabeled> {
-        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() {

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],
+            [-1.31918135e-20, -1.55163853e-06,  2.44274454e-02],
-            [8.56519716e-18, 9.42313768e-05, -1.86052093e00],
+            [-5.89622642e-05, -3.10327706e-07,  2.37534136e-02],
-            [2.57498016e-18, -2.73825295e-03, 1.00000000e00],
+            [-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::<u32>() < 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())
     }
 }

src/pipeline/m_estimator.rs

@@ -91,7 +91,9 @@ pub fn create_mestimator_thread(lanes_rx: Receiver<PointcloudLabeled>, 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();