SHERPA/pointcloud_downsampler
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

reformatted using rustfmt

Browse Source
This commit is contained in:
Timo Schneider
2025-09-27 10:34:19 +02:00
parent 6190292efa
commit 17536935af
1 changed files with 54 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
src/main.rs
View File

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