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

73
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 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<u8> = self.data
let data: Vec<u8> = self
.data
.chunks_exact(self.point_step as usize)
.map(|chunk| PointF32::from_bytes(chunk))
.filter(|point| point.is_valid())
@@ -22,7 +23,8 @@ 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
})
@@ -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,
@@ -129,31 +151,34 @@ impl Sub for VectorF32 {
}
}
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();
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| {
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));