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

55
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,7 +23,8 @@ 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
}) })
@@ -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,
@@ -129,22 +151,24 @@ impl Sub for VectorF32 {
} }
} }
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
.spawn_local(async move {
subscriber subscriber
.for_each(|msg| { .for_each(|msg| {
let start = Instant::now(); let start = Instant::now();
let downsampled = msg.downsample(); let downsampled = msg.downsample();
println!("Duration: {} ms", start.elapsed().as_millis()); println!("Duration: {} ms", start.elapsed().as_millis());
@@ -153,7 +177,8 @@ fn main() {
future::ready(()) future::ready(())
}) })
.await .await
}).unwrap(); })
.unwrap();
loop { loop {
node.spin_once(Duration::from_millis(10)); node.spin_once(Duration::from_millis(10));