ros_pointcloud2/README.md

ROS PointCloud2

Customizable conversions to and from the PointCloud2 ROS message.

Providing a easy to use, generics defined, point-wise iterator abstraction of the byte buffer of PointCloud2 to minimize iterations in your processing pipeline.

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

Example

use ros_pointcloud2::{
  pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
};

// 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,
  },
];

// For equality test later
let cloud_copy = cloud_points.clone();

// Some changes to demonstrate lazy iterator usage
let changed_it = cloud_points.iter().map(|p| {
  p.x = 0.5;
});

// Vector -> Writer -> Message
let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points)
    .try_into() // iterating points here O(n)
    .unwrap();

// Anything that implements `IntoIterator` also works - like another iterator.
let internal_msg_changed: PointCloud2Msg = WriterXYZ::from(changed_it)
    .try_into() // iterating points here O(n)
    .unwrap();

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

// Publish ...

// ... now incoming from a topic.
// let internal_msg: PointCloud2Msg = msg.into();

// Message -> Reader. The Reader implements the Iterator trait.
let reader = ReaderXYZ::try_from(internal_msg).unwrap();
let new_cloud_points = reader
    .map(|point: PointXYZ| {
      // Some logic here

      point
    })
    .collect::<Vec<PointXYZ>>();

assert_eq!(new_cloud_points, cloud_copy);

Integrations

There are currently 3 integrations for common ROS crates.

• rosrust_msg
  • 
• r2r_msg
  • 
  • 
  • 
• rclrs_msg
  • 
  • 

You can use rosrust and r2r by enabling the respective feature:

[dependencies]
ros_pointcloud2 = { version = "*", features = ["r2r_msg"]}
# or
ros_pointcloud2 = { version = "*", features = ["rosrust_msg"]}

rclrs (ros2_rust)

Features do not work properly with rcrls because the messages are linked externally. You need to use tags instead:

[dependencies]
ros_pointcloud2 = { git = "https://github.com/stelzo/ros_pointcloud2", tag = "v0.4.0_rclrs" }

Also, indicate the following dependencies to your linker inside the package.xml of your package.

<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>builtin_interfaces</depend>

Others

To use ros_pointcloud2 somewhere else, you can also implement the Into and From traits for PointCloud2Msg.

Try to avoid cloning the data: Vec<u8> field.

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!()
  }
}

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 some traits.

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

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 From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) {
  fn from(point: CustomPoint) -> Self {
    (
      [point.x, point.y, point.z],
      [
        PointMeta::new(point.r),
        PointMeta::new(point.g),
        PointMeta::new(point.b),
        PointMeta::new(point.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: 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
• Add more predefined types
• Optional derive macros for custom point implementations

Changelog 1.0.0

• Convert is now Reader and Writer. Convert had to translate both ways and keep a different state depending on the way it was created. This led to an edge case where the user could create a point cloud that always returns uninitialized points. While the reproduction was unlikely to occur in normal use cases, the possibility of it alone is unnecessary ambiguity. The new Reader and Writer not only eliminates ambiguity but also makes the code much more readable and concise.
• Zero-cost iterator abstraction. Reader and Writer now either take or implement iterable traits. This means, you can read a message, implement a filter function and pass the resulting iterator directly to the Writer. The entire pipeline is then compiled to a typesafe message-to-message filter with a single iteration.
• Divide error types to message and human errors. Corrupted messages with valid descriptions and correct byte buffer length can not be classified as corrupted at runtime, thus point conversions can be assumed to always work when passing these checks. There now are less error types and the point conversions within the iteration can omit the try_* prefix. Discrepancies within the point cloud message and the described point are checked at Reader and Writer creation instead. Human errors are possible with wrong point types or internal crate bugs that could lead to out-of-bound panics. These cases are checked in debug mode via debug_assert now. In release builds, the crate fully leans into performance optimizations and assumes a correct type description via generics (mainly the first 2 parameters coord_type and sizeof(coord_type)). Previously, these errors resulted in an error at runtime but the only possible source for them is the crate user, so the decision was made to promote them to a panic like a compile error. TODO check coord_type and sizeof at compile time statically?
• More Documentation and more relevant examples for every function and type. The example for custom points is moved from the Readme into the documentation as well.
• Performance and efficiency. By leaning into iterators whenever possible, the amount of bound checks have been reduced. There are less iterations in every function and dynamic memory usage is reduced.
• Removed dependencies. By switching to normal iterators and intializing different start values, the crate only depends on the std now.

API changes

• replace every use of ([T; DIM], [ros_pointcloud2::PointMeta; METADIM]) with ros_pointcloud2::Point<T, DIM, METADIM>

License

MIT