diff --git a/src/main.rs b/src/main.rs index d8c4782..675841f 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,8 +1,8 @@ -use byteorder::{LittleEndian, BigEndian, ByteOrder}; +use byteorder::{BigEndian, ByteOrder, LittleEndian}; use futures::{executor::LocalPool, future, stream::StreamExt, task::LocalSpawnExt}; use itertools::Itertools; use r2r; -use r2r::sensor_msgs::msg::{PointField, PointCloud2}; +use r2r::sensor_msgs::msg::{PointCloud2, PointField}; use std::f32::consts::PI; use std::ops::{Add, Sub}; use std::time::{Duration, Instant}; @@ -13,7 +13,8 @@ trait Downsample { impl Downsample for PointCloud2 { fn downsample(self) -> Self { - let data: Vec = self.data + let data: Vec = self + .data .chunks_exact(self.point_step as usize) .map(|chunk| PointF32::from_bytes(chunk)) .filter(|point| point.is_valid()) @@ -22,8 +23,9 @@ impl Downsample for PointCloud2 { let v1: VectorF32 = last.vec_to(current); let v2: VectorF32 = current.vec_to(next); - let angle_deg: f32 = (v1.dot(&v2).abs() / (v1.abs() * v2.abs())).acos() * 180f32 / PI; - + let angle_deg: f32 = + (v1.dot(&v2).abs() / (v1.abs() * v2.abs())).acos() * 180f32 / PI; + angle_deg > 30f32 }) .flat_map(|(_, point, _)| point.to_bytes_vec()) @@ -34,10 +36,30 @@ impl Downsample for PointCloud2 { height: 1, width: data.len() as u32 / 16, 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 }, - PointField { name: "intensity".to_string(), offset: 12, datatype: 7, count: 1 }, + 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, + }, + PointField { + name: "intensity".to_string(), + offset: 12, + datatype: 7, + count: 1, + }, ], is_bigendian: false, point_step: 16, @@ -58,7 +80,7 @@ struct VectorF32 { #[derive(Debug, Clone, Copy)] struct PointF32 { position: VectorF32, - intensity: f32, + intensity: f32, } impl PointF32 { @@ -110,7 +132,7 @@ impl Add for VectorF32 { fn add(self, rhs: Self) -> Self::Output { Self::Output { - x: self.x + rhs.x, + x: self.x + rhs.x, y: self.y + rhs.y, z: self.z + rhs.z, } @@ -122,41 +144,44 @@ impl Sub for VectorF32 { fn sub(self, rhs: Self) -> Self::Output { Self::Output { - x: self.x - rhs.x, + x: self.x - rhs.x, y: self.y - rhs.y, z: self.z - rhs.z, } } } - fn main() { let ctx = r2r::Context::create().unwrap(); let mut node = r2r::Node::create(ctx, "pointcloud_downsampling_node", "").unwrap(); - let subscriber = node.subscribe::("/velodyne_points", r2r::QosProfile::default()).unwrap(); - let publisher = node.create_publisher::("/filtered_points", r2r::QosProfile::default()).unwrap(); - + let subscriber = node + .subscribe::("/velodyne_points", r2r::QosProfile::default()) + .unwrap(); + let publisher = node + .create_publisher::("/filtered_points", r2r::QosProfile::default()) + .unwrap(); // Set up a simple task executor. let mut pool = LocalPool::new(); let spawner = pool.spawner(); - spawner.spawn_local(async move { - subscriber - .for_each(|msg| { + spawner + .spawn_local(async move { + subscriber + .for_each(|msg| { + let start = Instant::now(); + let downsampled = msg.downsample(); + println!("Duration: {} ms", start.elapsed().as_millis()); + publisher.publish(&downsampled); - let start = Instant::now(); - let downsampled = msg.downsample(); - println!("Duration: {} ms", start.elapsed().as_millis()); - publisher.publish(&downsampled); - - future::ready(()) - }) - .await - }).unwrap(); + future::ready(()) + }) + .await + }) + .unwrap(); loop { node.spin_once(Duration::from_millis(10)); pool.run_until_stalled(); } -} \ No newline at end of file +}