timoschneider/ros2_lane_detect
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

asd lab

Browse Source
This commit is contained in:
Timo Schneider
2025-05-13 21:58:37 +02:00
parent dd0834a69a
commit 866bc890a6
4 changed files with 24 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
src/main.rs
View File

@@ -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::<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();
}
}

4
src/pipeline/hdb_clusterer.rs
View File

@@ -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() {

15
src/pipeline/highlight_extractor.rs
View File

@@ -21,11 +21,10 @@ 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;
@@ -34,7 +33,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, _, 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())
}
}

4
src/pipeline/m_estimator.rs
View File

@@ -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();