使用rosrust处理点云数据

use anyhow::{Result, anyhow};
use rosrust::Time;
use rosrust::{Publisher, ros_info};
use rosrust_msg::sensor_msgs::PointCloud2;
use rosrust_msg::sensor_msgs::PointField;
use rosrust_msg::std_msgs::Header;

#[derive(Debug, Clone, Copy)]
struct Point {
    pub x: f32,
    pub y: f32,
    pub z: f32,
    pub intensity: f32,
}

#[derive(Debug)]
struct RosPointCloud2Msg(PointCloud2);

fn proc_pointcloud(msg: &PointCloud2) -> Vec<Point> {
    let mut points = Vec::new();
    let point_step = msg.point_step as usize;

    for chunk in msg.data.chunks_exact(point_step) {
        let x = f32::from_le_bytes(chunk[0..4].try_into().unwrap());
        let y = f32::from_le_bytes(chunk[4..8].try_into().unwrap());
        let z = f32::from_le_bytes(chunk[8..12].try_into().unwrap());
        let intensity = f32::from_le_bytes(chunk[12..16].try_into().unwrap());
        points.push(Point { x, y, z, intensity });
    }
    points
}

type Points = Vec<Point>;

impl RosPointCloud2Msg {
    fn new() -> Self {
        Self {
            0: PointCloud2::default(),
        }
    }
}

impl From<&Points> for RosPointCloud2Msg {
    fn from(points: &Points) -> RosPointCloud2Msg {
        let mut cloud = PointCloud2::default();
        cloud.is_dense = true;
        cloud.height = 1;
        cloud.width = points.len() as u32;
        cloud.point_step = 16;
        cloud.fields = vec![
            PointField {
                name: "x".to_string(),
                offset: 0,
                datatype: PointField::FLOAT32,
                count: 1,
            },
            PointField {
                name: "y".to_string(),
                offset: 4,
                datatype: PointField::FLOAT32,
                count: 1,
            },
            PointField {
                name: "z".to_string(),
                offset: 8,
                datatype: PointField::FLOAT32,
                count: 1,
            },
            PointField {
                name: "intensity".to_string(),
                offset: 12,
                datatype: PointField::FLOAT32,
                count: 1,
            },
        ];
        cloud.data = points
            .into_iter()
            .flat_map(|p| {
                vec![
                    p.x.to_le_bytes(),
                    p.y.to_le_bytes(),
                    p.z.to_le_bytes(),
                    p.intensity.to_le_bytes(),
                ]
            })
            .collect::<Vec<_>>()
            .concat();
        RosPointCloud2Msg { 0: cloud }
    }
}

fn main() -> Result<()> {
    rosrust::init("pointcloud_processor");
    let publisher: Publisher<PointCloud2> = rosrust::publish("/processed_cloud", 1).unwrap();

    let subscriber = rosrust::subscribe("timoo_points", 1, move |msg: PointCloud2| {
        let points = proc_pointcloud(&msg);

        let mut processed_cloud = RosPointCloud2Msg::from(&points).0;
        processed_cloud.header = msg.header;
        publisher.send(processed_cloud).unwrap();
    })
    .map_err(|e| anyhow!("Failed to subscribe timoo_points: {}", e))?;
    rosrust::spin();

    Ok(())
}