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

added ros2 visualization

Browse Source
This commit is contained in:
Timo Schneider
2025-05-12 20:34:07 +02:00
parent deef3bc819
commit 7bfe430fda
4 changed files with 116 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/main.rs
View File

@@ -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::<r2r::sensor_msgs::msg::Image>("/camera/front", QosProfile::default()).unwrap();
let mut timer = node.create_wall_timer(Duration::from_millis(1000)).unwrap();
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 (img_tx, img_rx) = mpsc::sync_channel::<Arc<r2r::sensor_msgs::msg::Image>>(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<Duration> = 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);

4
src/pipeline/hdb_clusterer.rs
View File

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

59
src/pipeline/highlight_extractor.rs
View File

@@ -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<Vec<Vec<f64>>>;
trait ExtractHighlights {
fn get_highlights(&self, threshold: u8, downsample: f64) -> Pointcloud;
}
trait Visualize{
fn visualize(&self, pub_pcd: &Publisher<PointCloud2>);
}
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::<u32>() < 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<Arc<Image>>, threshold: u8, downsample: f64) -> (Receiver<Pointcloud>, Arc<Mutex<(Duration, Duration)>>) {
impl Visualize for Pointcloud {
fn visualize(&self, pub_pcd: &Publisher<PointCloud2>) {
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::<Vec<u8>>();
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<Arc<Image>>, threshold: u8, downsample: f64, pub_pcd: Publisher<PointCloud2>) -> (Receiver<Pointcloud>, Arc<Mutex<(Duration, Duration)>>) {
let (tx, rx) = mpsc::sync_channel::<Pointcloud>(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<Arc<Image>>, 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();
}

52
src/pipeline/m_estimator.rs
View File

@@ -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<Vec<f64>>;
pub fn create_mestimator_thread(lanes_rx: Receiver<PointcloudLabeled>) -> (Receiver<LaneEstimation>, Arc<Mutex<(Duration, Duration)>>) {
trait Visualize {
fn visualize(&self, pub_left: &Publisher<Path>, pub_right: &Publisher<Path>);
}
impl Visualize for Vec<f64> {
fn visualize(&self, pub_left: &Publisher<Path>, pub_right: &Publisher<Path>) {
let mut lane_left = Path {
header: Header {stamp: Time {sec: 0, nanosec: 0}, frame_id: "front_camera_link".to_string()},
poses: Vec::<PoseStamped>::new()
};
let mut right_lane = Path {
header: Header {stamp: Time {sec: 0, nanosec: 0}, frame_id: "front_camera_link".to_string()},
poses: Vec::<PoseStamped>::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<PointcloudLabeled>, pub_left: Publisher<Path>, pub_right: Publisher<Path>) -> (Receiver<LaneEstimation>, Arc<Mutex<(Duration, Duration)>>) {
let (z_tx, z_rx) = mpsc::sync_channel::<LaneEstimation>(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<PointcloudLabeled>) -> (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();
}
});