ROS PointCloud2

A lightweight Rust implementation for fast, safe and customizable conversions to and from the sensor_msgs/PointCloud2 ROS message.

[dependencies]
ros_pointcloud2 = "0.2.0"

Providing a fast and memory efficient way for message conversion while allowing user defined types without the cost of iterations.

Instead of converting the entire cloud into a Vec, you get an Iterator that converts each point from the message on the fly. An example for using this crate is this filter node. It is also a good starting point for implementing ROS1 nodes in Rust inside a catkin environment.

To keep the crate a general purpose library for the problem and support ROS1 and ROS2, it uses its own type for the message.

use ros_pointcloud2::ConvertXYZ;
use ros_pointcloud2::pcl_utils::PointXYZ;
use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::fallible_iterator::FallibleIterator;

// Your points (here using the predefined type PointXYZ).
let cloud_points = vec![
  PointXYZ { x: 1.3, y: 1.6, z: 5.7 },
  PointXYZ { x: f32::MAX, y: f32::MIN, z: f32::MAX },
];

let cloud_copy = cloud_points.clone(); // Only for checking equality later.

// Vector -> Convert -> Message
let internal_msg: PointCloud2Msg = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap();

// Convert to your favorite ROS crate message type, we will use rosrust here.
// let msg: rosrust_msg::sensor_msgs::PointCloud2 = internal_msg.into();

// Back to this crate's message type.
// let internal_msg: PointCloud2Msg = msg.into();

// Message -> Convert -> Vector
let convert: ConvertXYZ = ConvertXYZ::try_from(internal_msg).unwrap();
let new_cloud_points = convert.map(|point: PointXYZ| {
    // Insert your point business logic here or use other methods like .for_each().
    // I will just copy the points into a vector as an example.
    // Also, since we are using a fallible iterator, we need to return a Result.
    Ok(point)
}).collect::<Vec<PointXYZ>>()
  .unwrap(); // Handle point conversion or byte errors here.

assert_eq!(new_cloud_points, cloud_copy);

To use ros_pointcloud2 in your favorite ROS crate, you can either use this crate's features (see Integration section below) or implement the Into and From traits for PointCloud2Msg.

Please avoid cloning the data: Vec<u8> of the message.

use ros_pointcloud2::ros_types::PointCloud2Msg;

struct YourROSPointCloud2 {} // Likely to be generated by your ROS crate.

impl Into<YourROSPointCloud2> for PointCloud2Msg {
  fn into(self) -> YourROSPointCloud2 {
    todo!()
  }
}

impl From<YourROSPointCloud2> for PointCloud2Msg {
  fn from(msg: YourROSPointCloud2) -> Self {
    todo!()
  }
}

Integrations

Currently, we only implement a conversion for the following ROS crate:

• rosrust

You can use one by enabling the corresponding feature.

[dependencies]
ros_pointcloud2 = { version = "0.2.0", features = ["rosrust_msg"]}

Please open an issue or PR if you want to see support for other crates.

Features

• Easy to integrate into your favorite ROS1 or ROS2 crate
• Custom point types
• Predefined types for common conversions (compared to PCL)
  • PointXYZ
  • PointXYZI
  • PointXYZL
  • PointXYZRGB
  • PointXYZRGBA
  • PointXYZRGBL
  • PointXYZNormal
  • PointXYZINormal
  • PointXYZRGBNormal
• 2D and 3D

Custom Types

You can freely convert to your own point types without reiterating the entire cloud.

You just need to implement the Into and From traits.

use ros_pointcloud2::mem_macros::size_of;
use ros_pointcloud2::{Convert, MetaNames, PointMeta, ConversionError, PointConvertible};
use ros_pointcloud2::ros_types::PointCloud2Msg;

const DIM : usize = 3;
const METADIM : usize = 4;

#[derive(Debug, PartialEq, Clone)]
struct CustomPoint {
  x: f32, // DIM 1
  y: f32, // DIM 2
  z: f32, // DIM 3
  r: u8,  // METADIM 1
  g: u8,  // METADIM 2
  b: u8,  // METADIM 3
  a: u8,  // METADIM 4
}

// Converting your custom point to the crate's internal representation
impl Into<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
  fn into(self) -> ([f32; DIM], [PointMeta; METADIM]) {
    ([self.x, self.y, self.z], [PointMeta::new(self.r), PointMeta::new(self.g), PointMeta::new(self.b), PointMeta::new(self.a)])
  }
}

// The mappings for index of meta idx to field names. Example: 0 -> "r", 1 -> "g", 2 -> "b", 3 -> "a"
impl MetaNames<METADIM> for CustomPoint {
  fn meta_names() -> [String; METADIM] {
    ["r", "g", "b", "a"].map(|s| s.to_string())
  }
}

// Converting crate's internal representation to your custom point
impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
  type Error = ConversionError;
  fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result<Self, Self::Error> {
    Ok(Self {
      x: data.0[0],
      y: data.0[1],
      z: data.0[2],
      r: data.1[0].get()?,
      g: data.1[1].get()?,
      b: data.1[2].get()?,
      a: data.1[3].get()?,
    })
  }
}

impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}

type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;

// Your custom cloud (Vector)
let custom_cloud = vec![
  CustomPoint { x: 0.0, y: 1.0, z: 5.0, r: 0, g: 0, b: 0, a: 0 },
  CustomPoint { x: 1.0, y: 1.5, z: 5.0, r: 1, g: 1, b: 1, a: 1 },
  CustomPoint { x: 1.3, y: 1.6, z: 5.7, r: 2, g: 2, b: 2, a: 2 },
  CustomPoint { x: f32::MAX, y: f32::MIN, z: f32::MAX, r: u8::MAX, g: u8::MAX, b: u8::MAX, a: u8::MAX },
];


// Cloud -> ROS message
let custom_msg: PointCloud2Msg = MyConverter::try_from(custom_cloud).unwrap().try_into().unwrap();

// ROS message -> Cloud
let to_custom_type = MyConverter::try_from(custom_msg).unwrap();

Future Work

• Benchmark vs PCL
• Proper error passing to the iterator result (currently merged into PointConversionError)
• remove allocations
• introduce no-panic for maximum stability
• Add more predefined types

License

MIT