added simple downsampling node, todo: expose params

src/main.rs

@@ -0,0 +1,162 @@
+use byteorder::{LittleEndian, BigEndian, ByteOrder};
+use futures::{executor::LocalPool, future, stream::StreamExt, task::LocalSpawnExt};
+use itertools::Itertools;
+use r2r;
+use r2r::sensor_msgs::msg::{PointField, PointCloud2};
+use std::f32::consts::PI;
+use std::ops::{Add, Sub};
+use std::time::{Duration, Instant};
+
+trait Downsample {
+    fn downsample(self) -> Self;
+}
+
+impl Downsample for PointCloud2 {
+    fn downsample(self) -> Self {
+        let data: Vec<u8> = self.data
+            .chunks_exact(self.point_step as usize)
+            .map(|chunk| PointF32::from_bytes(chunk))
+            .filter(|point| point.is_valid())
+            .tuple_windows::<(_, _, _)>()
+            .filter(|(last, current, next)| {
+                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;
+                
+                angle_deg > 30f32
+            })
+            .flat_map(|(_, point, _)| point.to_bytes_vec())
+            .collect();
+
+        PointCloud2 {
+            header: self.header,
+            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 },
+            ],
+            is_bigendian: false,
+            point_step: 16,
+            row_step: 16,
+            data,
+            is_dense: true,
+        }
+    }
+}
+
+#[derive(Debug, Clone, Copy)]
+struct VectorF32 {
+    x: f32,
+    y: f32,
+    z: f32,
+}
+
+#[derive(Debug, Clone, Copy)]
+struct PointF32 {
+    position: VectorF32,
+    intensity: f32, 
+}
+
+impl PointF32 {
+    fn from_bytes(chunk: &[u8]) -> Self {
+        Self {
+            position: VectorF32 {
+                x: LittleEndian::read_f32(&chunk[0..4]),
+                y: LittleEndian::read_f32(&chunk[4..8]),
+                z: LittleEndian::read_f32(&chunk[8..12]),
+            },
+            intensity: LittleEndian::read_f32(&chunk[12..16]),
+        }
+    }
+
+    fn to_bytes_vec(&self) -> Vec<u8> {
+        let mut buf = [0u8; 16];
+        buf[0..4].copy_from_slice(&self.position.x.to_le_bytes());
+        buf[4..8].copy_from_slice(&self.position.y.to_le_bytes());
+        buf[8..12].copy_from_slice(&self.position.z.to_le_bytes());
+        buf[12..16].copy_from_slice(&self.intensity.to_le_bytes());
+        buf.to_vec()
+    }
+
+    fn is_valid(&self) -> bool {
+        self.position.is_valid() && !self.intensity.is_nan()
+    }
+
+    fn vec_to(&self, target: &Self) -> VectorF32 {
+        target.position - self.position
+    }
+}
+
+impl VectorF32 {
+    fn is_valid(&self) -> bool {
+        !(self.x.is_nan() || self.y.is_nan() || self.z.is_nan())
+    }
+
+    fn abs(&self) -> f32 {
+        (self.x.powi(2) + self.y.powi(2) + self.z.powi(2)).sqrt()
+    }
+
+    fn dot(&self, rhs: &Self) -> f32 {
+        (self.x * rhs.x) + (self.y * rhs.y) + (self.z * rhs.z)
+    }
+}
+
+impl Add for VectorF32 {
+    type Output = Self;
+
+    fn add(self, rhs: Self) -> Self::Output {
+        Self::Output {
+            x: self.x + rhs.x, 
+            y: self.y + rhs.y,
+            z: self.z + rhs.z,
+        }
+    }
+}
+
+impl Sub for VectorF32 {
+    type Output = Self;
+
+    fn sub(self, rhs: Self) -> Self::Output {
+        Self::Output {
+            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::<PointCloud2>("/velodyne_points", r2r::QosProfile::default()).unwrap();
+    let publisher = node.create_publisher::<PointCloud2>("/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| {
+
+                let start = Instant::now();
+                let downsampled = msg.downsample();
+                println!("Duration: {} ms", start.elapsed().as_millis());
+                publisher.publish(&downsampled);
+
+                future::ready(())
+            })
+            .await
+    }).unwrap();
+
+    loop {
+        node.spin_once(Duration::from_millis(10));
+        pool.run_until_stalled();
+    }
+}