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

wip: add ros2 interface

Browse Source
This commit is contained in:
Timo Schneider
2025-05-10 19:54:15 +02:00
parent f5662de699
commit 57cb97fcd0
2 changed files with 31 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
Cargo.toml
View File

@@ -15,3 +15,6 @@ rayon = "1.10.0"
hdbscan = "0.10.0"
nalgebra = "0.33.2"
indicatif = "0.17.11"
r2r = "0.9.5"
futures-core = "0.3.31"
futures = "0.3.31"

66
src/main.rs
View File

@@ -13,6 +13,8 @@ use std::time::{Duration, Instant};
use indicatif::{MultiProgress, ProgressBar, ProgressStyle};
use nalgebra::{DMatrix};
use ndarray::{Array1, Array2};
use r2r::QosProfile;
use futures::{executor::LocalPool, future, stream::StreamExt, task::LocalSpawnExt};
fn create_extract_thread(image_rx: Receiver<Arc<Vec<u8>>>, width: u32, threshold: u8, downsample: f64) -> (Receiver<Arc<Vec<Vec<f64>>>>, Arc<Mutex<(Duration, Duration)>>) {
let (tx, rx) = mpsc::sync_channel::<Arc<Vec<Vec<f64>>>>(1);
@@ -269,8 +271,28 @@ fn dummy_image_spawner_thread() -> Receiver<Arc<Vec<u8>>> {
#[show_image::main]
fn main() {
let image_rx = dummy_image_spawner_thread();
let (pointcloud_rx, usage_extract) = create_extract_thread(image_rx, 800, 196, 0.025);
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 (img_tx, img_rx) = mpsc::sync_channel::<Arc<Vec<u8>>>(1);
// Set up a simple task executor.
let mut pool = LocalPool::new();
let spawner = pool.spawner();
// Run the subscriber in one task, printing the messages
spawner.spawn_local(async move {
subscriber
.for_each(|msg| {
img_tx.send(Arc::new(msg.data)).unwrap();
future::ready(())
})
.await
}).unwrap();
let (pointcloud_rx, usage_extract) = create_extract_thread(img_rx, 800, 196, 0.025);
let (projection_rx, usage_transform) = create_transform_thread(pointcloud_rx);
let (cluster_labels_rx, usage_cluster) = create_pointcloud_labels_thread(projection_rx);
let (clusters_rx, usage_split) = create_cluster_separator_thread(cluster_labels_rx);
@@ -294,7 +316,9 @@ fn main() {
let mut t: VecDeque<Duration> = VecDeque::with_capacity(25);
let mut now: Instant = Instant::now();
for (z, lanes) in z_rx {
loop {
node.spin_once(Duration::from_millis(10));
pool.run_until_stalled();
let u_extract = *usage_extract.lock().unwrap();
let u_transform = *usage_transform.lock().unwrap();
@@ -322,42 +346,6 @@ fn main() {
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);
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]);
}
});
lanes[0].iter().for_each(|point| {
let x = point[0] * 800.0/45.0 + rgb_image.width() as f64 / 2.0;
let y = rgb_image.height() as f64 - 1.0 - point[1] * 800.0/45.0;
if x < rgb_image.width() as f64 && y < rgb_image.width() as f64 {
rgb_image.put_pixel(x as u32, y as u32, Rgb([0, 255, 255]));
}
});
lanes[1].iter().for_each(|point| {
let x = point[0] * 800.0/45.0 + rgb_image.width() as f64 / 2.0;
let y = rgb_image.height() as f64 - 1.0 - point[1] * 800.0/45.0;
if x < rgb_image.width() as f64 && y < rgb_image.width() as f64 {
rgb_image.put_pixel(x as u32, y as u32, Rgb([255, 255, 0]));
}
});
let image = ImageView::new(ImageInfo::new(PixelFormat::Rgb8, 800, 800), rgb_image.iter().as_slice());
window.set_image("camera", image).unwrap();
if t.len() == 25 {
t.pop_front();