mirror
/
ros_pointcloud2
mirror of https://github.com/stelzo/ros_pointcloud2.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

docs and clippy

This commit is contained in:
stelzo 2024-04-29 20:32:55 +02:00
parent 77c222414f
commit 91b0ae7ecf
No known key found for this signature in database
GPG Key ID: FC4EF89052319374
8 changed files with 350 additions and 345 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

185
README.md
View File

@ -5,11 +5,13 @@
</p>
</p>
Providing a easy to use, generics defined, point-wise iterator abstraction of the byte buffer of `PointCloud2` to minimize iterations in your processing pipeline.
Providing an easy to use, generics defined, point-wise iterator abstraction over the byte buffer in `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
## Examples
Using a `Vec<>`.
```rust
use ros_pointcloud2::{
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
@ -18,10 +20,11 @@ use ros_pointcloud2::{
// Your points (here using the predefined type PointXYZ).
let cloud_points = vec![
PointXYZ {
x: 1.3,
y: 1.6,
z: 5.7,
x: 91.486,
y: -4.1,
z: 42.0001,
},
PointXYZ {
x: f32::MAX,
y: f32::MIN,
@ -32,20 +35,11 @@ let cloud_points = vec![
// 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();
// Vector -> Writer -> Message.
// You can also just give the Vec or anything that implements `IntoIter`.
let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter())
.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();
@ -58,12 +52,12 @@ let internal_msg_changed: PointCloud2Msg = WriterXYZ::from(changed_it)
// 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
.map(|point: PointXYZ| {
// Some logic here
point
})
.collect::<Vec<PointXYZ>>();
point
})
.collect::<Vec<PointXYZ>>(); // iterating points here O(n)
assert_eq!(new_cloud_points, cloud_copy);
```
@ -102,156 +96,13 @@ Also, indicate the following dependencies to your linker inside the `package.xml
<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.
```rust
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.
```rust
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](https://choosealicense.com/licenses/mit/)

62
src/convert.rs
View File

@ -1,24 +1,21 @@
use crate::*;
/// Datatypes from the [PointField message](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html).
#[derive(Clone, Debug, PartialEq, Copy)]
#[derive(Default, Clone, Debug, PartialEq, Copy)]
pub enum FieldDatatype {
F32,
F64,
I32,
U8,
U16,
#[default]
U32,
I8,
I16,
}
impl Default for FieldDatatype {
fn default() -> Self {
FieldDatatype::F32
}
}
impl FieldDatatype {
pub fn size(&self) -> usize {
match self {
@ -105,9 +102,9 @@ impl TryFrom<u8> for FieldDatatype {
}
}
impl Into<u8> for FieldDatatype {
fn into(self) -> u8 {
match self {
impl From<FieldDatatype> for u8 {
fn from(val: FieldDatatype) -> Self {
match val {
FieldDatatype::I8 => 1,
FieldDatatype::U8 => 2,
FieldDatatype::I16 => 3,
@ -140,7 +137,7 @@ pub(crate) fn check_coord(
/// Matching field names from each meta data per point to the PointField name.
/// Always make sure to use the same order as in your Into<> implementation to have a correct mapping.
pub trait MetaNames<const METADIM: usize> {
fn meta_names() -> [String; METADIM];
fn meta_names() -> [&'static str; METADIM];
}
#[inline(always)]
@ -183,71 +180,92 @@ pub trait FromBytes: Default + Sized + Copy + GetFieldDatatype {
}
impl FromBytes for i8 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0]])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0]])
}
#[inline]
fn bytes(x: &i8) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for i16 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1]])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1]])
}
#[inline]
fn bytes(x: &i16) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for u16 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1]])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1]])
}
#[inline]
fn bytes(x: &u16) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for u32 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
#[inline]
fn bytes(x: &u32) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for f32 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
#[inline]
fn bytes(x: &f32) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for i32 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
@ -261,47 +279,52 @@ impl FromBytes for i32 {
}
impl FromBytes for f64 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([
bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7],
])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([
bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7],
])
}
#[inline]
fn bytes(x: &f64) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for u8 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0]])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0]])
}
#[inline]
fn bytes(x: &u8) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
#[derive(Clone, Debug, PartialEq)]
#[derive(Default, Clone, Debug, PartialEq)]
pub(crate) enum Endianness {
Big,
#[default]
Little,
}
impl Default for Endianness {
fn default() -> Self {
Endianness::Little
}
}
#[inline(always)]
pub(crate) fn load_loadable<T: FromBytes, const SIZE: usize>(
start_idx: usize,
data: &[u8],
@ -313,7 +336,8 @@ pub(crate) fn load_loadable<T: FromBytes, const SIZE: usize>(
}
}
/// Note: check if the data slice is long enough to load the bytes beforehand!
/// Note: check if the data slice is long enough to load the bytes beforehand! Uses unsafe indexing.
#[inline(always)]
fn load_bytes<const S: usize>(start_idx: usize, data: &[u8]) -> [u8; S] {
let mut target = [u8::default(); S];

74
src/lib.rs
View File

@ -1,4 +1,72 @@
//#![doc = include_str!("../README.md")]
//! A library to work with the PointCloud2 message type from ROS.
//!
//! ros_pointcloud2 mainly provides iterator-based `Reader` and `Writer` to interact
//! with the internal message type `PointCloud2Msg` without forcing an iteration.
//!
//! For ROS interoperability, the message type generated by the respective crate must be converted to
//! the internal message type `PointCloud2Msg` using the `From` trait,
//! which is mostly a ownership transfer without copying the point data.
//!
//! There are implementations for the `r2r`, `rclrs` (ros2_rust) and `rosrust_msg` message types
//! available in the feature flags. If you miss a message type, please open an issue or a PR.
//! See the `ros_types` module for more information.
//!
//! Common point types like `PointXYZ` are provided in the `pcl_utils` module. You can implement any custom point type
//! that can be described by the specification of the `PointCloud2Msg` message type.
//! See the `Reader` or `Writer` struct documentation for an example.
//!
//! # 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: 9.0006,
//! y: 42.0,
//! z: -6.2,
//! },
//!
//! PointXYZ {
//! x: f32::MAX,
//! y: f32::MIN,
//! z: f32::MAX,
//! },
//! ];
//!
//! // For equality test later
//! let cloud_copy = cloud_points.clone();
//!
//! // Vector -> Writer -> Message.
//! // Vector -> Writer -> Message.
//! // You can also just give the Vec or anything that implements `IntoIter`.
//! let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter())
//! .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>>(); // iterating points here O(n)
//!
//! assert_eq!(new_cloud_points, cloud_copy);
//! ```
pub mod convert;
pub mod pcl_utils;
pub mod ros_types;
@ -100,6 +168,7 @@ impl PointMeta {
/// ```
/// let meta = ros_pointcloud2::PointMeta::new(1.0);
/// ```
#[inline(always)]
pub fn new<T: FromBytes>(value: T) -> Self {
let raw_bytes = T::bytes(&value);
let mut bytes = [0; std::mem::size_of::<f64>()];
@ -113,12 +182,13 @@ impl PointMeta {
}
}
#[inline(always)]
fn from_buffer(data: &[u8], offset: usize, datatype: &FieldDatatype) -> Self {
debug_assert!(data.len() >= offset + datatype.size());
let bytes = unsafe { data.get_unchecked(offset..offset + datatype.size()) };
let mut bytes_array = [0; std::mem::size_of::<f64>()]; // 8 bytes as f64 is the largest type
for (byte, save_byte) in bytes.into_iter().zip(bytes_array.iter_mut()) {
for (byte, save_byte) in bytes.iter().zip(bytes_array.iter_mut()) {
*save_byte = *byte;
}

153
src/pcl_utils.rs
View File

@ -36,17 +36,17 @@ impl From<Point<f32, 3, 0>> for PointXYZ {
}
}
impl Into<Point<f32, 3, 0>> for PointXYZ {
fn into(self) -> Point<f32, 3, 0> {
impl From<PointXYZ> for Point<f32, 3, 0> {
fn from(point: PointXYZ) -> Self {
Point {
coords: [self.x, self.y, self.z],
meta: Default::default(),
coords: [point.x, point.y, point.z],
meta: [],
}
}
}
impl MetaNames<0> for PointXYZ {
fn meta_names() -> [String; 0] {
fn meta_names() -> [&'static str; 0] {
[]
}
}
@ -63,6 +63,15 @@ pub struct PointXYZI {
pub intensity: f32,
}
impl From<PointXYZI> for Point<f32, 3, 1> {
fn from(point: PointXYZI) -> Self {
Point {
coords: [point.x, point.y, point.z],
meta: [point.intensity.into()],
}
}
}
impl From<Point<f32, 3, 1>> for PointXYZI {
fn from(point: Point<f32, 3, 1>) -> Self {
Self {
@ -74,18 +83,9 @@ impl From<Point<f32, 3, 1>> for PointXYZI {
}
}
impl Into<Point<f32, 3, 1>> for PointXYZI {
fn into(self) -> Point<f32, 3, 1> {
Point {
coords: [self.x, self.y, self.z],
meta: [self.intensity.into()],
}
}
}
impl MetaNames<1> for PointXYZI {
fn meta_names() -> [String; 1] {
["intensity"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 1] {
["intensity"]
}
}
@ -118,19 +118,18 @@ impl From<Point<f32, 3, 1>> for PointXYZRGB {
}
}
impl Into<Point<f32, 3, 1>> for PointXYZRGB {
fn into(self) -> Point<f32, 3, 1> {
let rgb = pack_rgb(self.r, self.g, self.b);
impl From<PointXYZRGB> for Point<f32, 3, 1> {
fn from(point: PointXYZRGB) -> Self {
Point {
coords: [self.x, self.y, self.z],
meta: [rgb.into()],
coords: [point.x, point.y, point.z],
meta: [pack_rgb(point.r, point.g, point.b).into()],
}
}
}
impl MetaNames<1> for PointXYZRGB {
fn meta_names() -> [String; 1] {
["rgb"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 1] {
["rgb"]
}
}
@ -166,19 +165,19 @@ impl From<Point<f32, 3, 2>> for PointXYZRGBA {
}
}
impl Into<Point<f32, 3, 2>> for PointXYZRGBA {
fn into(self) -> Point<f32, 3, 2> {
let rgb = pack_rgb(self.r, self.g, self.b);
impl From<PointXYZRGBA> for Point<f32, 3, 2> {
fn from(point: PointXYZRGBA) -> Self {
let rgb = pack_rgb(point.r, point.g, point.b);
Point {
coords: [self.x, self.y, self.z],
meta: [rgb.into(), self.a.into()],
coords: [point.x, point.y, point.z],
meta: [rgb.into(), point.a.into()],
}
}
}
impl MetaNames<2> for PointXYZRGBA {
fn meta_names() -> [String; 2] {
["rgb", "a"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 2] {
["rgb", "a"]
}
}
@ -217,24 +216,24 @@ impl From<Point<f32, 3, 4>> for PointXYZRGBNormal {
}
}
impl Into<Point<f32, 3, 4>> for PointXYZRGBNormal {
fn into(self) -> Point<f32, 3, 4> {
let rgb = pack_rgb(self.r, self.g, self.b);
impl From<PointXYZRGBNormal> for Point<f32, 3, 4> {
fn from(point: PointXYZRGBNormal) -> Self {
let rgb = pack_rgb(point.r, point.g, point.b);
Point {
coords: [self.x, self.y, self.z],
coords: [point.x, point.y, point.z],
meta: [
rgb.into(),
self.normal_x.into(),
self.normal_y.into(),
self.normal_z.into(),
point.normal_x.into(),
point.normal_y.into(),
point.normal_z.into(),
],
}
}
}
impl MetaNames<4> for PointXYZRGBNormal {
fn meta_names() -> [String; 4] {
["rgb", "normal_x", "normal_y", "normal_z"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 4] {
["rgb", "normal_x", "normal_y", "normal_z"]
}
}
@ -253,6 +252,20 @@ pub struct PointXYZINormal {
pub normal_z: f32,
}
impl From<PointXYZINormal> for Point<f32, 3, 4> {
fn from(point: PointXYZINormal) -> Self {
Point {
coords: [point.x, point.y, point.z],
meta: [
point.intensity.into(),
point.normal_x.into(),
point.normal_y.into(),
point.normal_z.into(),
],
}
}
}
impl From<Point<f32, 3, 4>> for PointXYZINormal {
fn from(point: Point<f32, 3, 4>) -> Self {
Self {
@ -267,23 +280,9 @@ impl From<Point<f32, 3, 4>> for PointXYZINormal {
}
}
impl Into<Point<f32, 3, 4>> for PointXYZINormal {
fn into(self) -> Point<f32, 3, 4> {
Point {
coords: [self.x, self.y, self.z],
meta: [
self.intensity.into(),
self.normal_x.into(),
self.normal_y.into(),
self.normal_z.into(),
],
}
}
}
impl MetaNames<4> for PointXYZINormal {
fn meta_names() -> [String; 4] {
["intensity", "normal_x", "normal_y", "normal_z"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 4] {
["intensity", "normal_x", "normal_y", "normal_z"]
}
}
@ -310,18 +309,18 @@ impl From<Point<f32, 3, 1>> for PointXYZL {
}
}
impl Into<Point<f32, 3, 1>> for PointXYZL {
fn into(self) -> Point<f32, 3, 1> {
impl From<PointXYZL> for Point<f32, 3, 1> {
fn from(point: PointXYZL) -> Self {
Point {
coords: [self.x, self.y, self.z],
meta: [self.label.into()],
coords: [point.x, point.y, point.z],
meta: [point.label.into()],
}
}
}
impl MetaNames<1> for PointXYZL {
fn meta_names() -> [String; 1] {
["label".to_string()]
fn meta_names() -> [&'static str; 1] {
["label"]
}
}
@ -356,19 +355,21 @@ impl From<Point<f32, 3, 2>> for PointXYZRGBL {
}
}
impl Into<Point<f32, 3, 2>> for PointXYZRGBL {
fn into(self) -> Point<f32, 3, 2> {
let rgb = pack_rgb(self.r, self.g, self.b);
impl From<PointXYZRGBL> for Point<f32, 3, 2> {
fn from(point: PointXYZRGBL) -> Self {
Point {
coords: [self.x, self.y, self.z],
meta: [rgb.into(), self.label.into()],
coords: [point.x, point.y, point.z],
meta: [
pack_rgb(point.r, point.g, point.b).into(),
point.label.into(),
],
}
}
}
impl MetaNames<2> for PointXYZRGBL {
fn meta_names() -> [String; 2] {
["rgb", "label"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 2] {
["rgb", "label"]
}
}
@ -399,22 +400,22 @@ impl From<Point<f32, 3, 3>> for PointXYZNormal {
}
}
impl Into<Point<f32, 3, 3>> for PointXYZNormal {
fn into(self) -> Point<f32, 3, 3> {
impl From<PointXYZNormal> for Point<f32, 3, 3> {
fn from(point: PointXYZNormal) -> Self {
Point {
coords: [self.x, self.y, self.z],
coords: [point.x, point.y, point.z],
meta: [
self.normal_x.into(),
self.normal_y.into(),
self.normal_z.into(),
point.normal_x.into(),
point.normal_y.into(),
point.normal_z.into(),
],
}
}
}
impl MetaNames<3> for PointXYZNormal {
fn meta_names() -> [String; 3] {
["normal_x", "normal_y", "normal_z"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 3] {
["normal_x", "normal_y", "normal_z"]
}
}

25
src/reader.rs
View File

@ -1,5 +1,4 @@
use crate::*;
use convert::*;
/// Convenience type for a Reader that reads coordinates as f32. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderF32<const DIM: usize, const METADIM: usize, C> =
@ -152,18 +151,20 @@ pub type ReaderXYZL = ReaderF32<3, 1, PointXYZL>;
/// }
/// }
///
///impl Into<Point<f32, 3, 1>> for CustomPoint {
/// fn into(self) -> Point<f32, 3, 1> {
///impl From<CustomPoint> for Point<f32, 3, 1> {
/// fn from(point: CustomPoint) -> Self {
/// Point {
/// coords: [self.x, self.y, self.z],
/// meta: [self.i.into()],
/// coords: [point.x, point.y, point.z],
/// meta: [
/// point.i.into(),
/// ],
/// }
/// }
///}
///
/// impl MetaNames<METADIM> for CustomPoint {
/// fn meta_names() -> [String; METADIM] {
/// [format!("intensity")]
/// fn meta_names() -> [&'static str; METADIM] {
/// ["intensity"]
/// }
/// }
///
@ -218,7 +219,7 @@ where
return None; // iteration finished
}
let mut xyz: [T; DIM] = [T::default(); DIM];
let mut xyz = [T::default(); DIM];
xyz.iter_mut()
.zip(self.offsets.iter())
.for_each(|(p_xyz, in_point_offset)| {
@ -235,7 +236,7 @@ where
"Offset length mismatch"
);
let mut meta: [PointMeta; METADIM] = [PointMeta::default(); METADIM];
let mut meta = [PointMeta::default(); METADIM];
meta.iter_mut()
.zip(self.offsets.iter().skip(DIM))
.zip(self.meta.iter())
@ -387,10 +388,8 @@ where
if size_with_last_meta > point_step_size {
return Err(ConversionError::DataLengthMismatch);
}
} else {
if last_offset + xyz_field_type.size() > point_step_size {
return Err(ConversionError::DataLengthMismatch);
}
} else if last_offset + xyz_field_type.size() > point_step_size {
return Err(ConversionError::DataLengthMismatch);
}
Ok(Self {

56
src/ros_types.rs
View File

@ -70,19 +70,19 @@ impl From<r2r::sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
}
#[cfg(feature = "r2r_msg")]
impl Into<r2r::sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
fn into(self) -> r2r::sensor_msgs::msg::PointCloud2 {
impl From<PointCloud2Msg> for r2r::sensor_msgs::msg::PointCloud2 {
fn from(msg: PointCloud2Msg) -> Self {
r2r::sensor_msgs::msg::PointCloud2 {
header: r2r::std_msgs::msg::Header {
stamp: r2r::builtin_interfaces::msg::Time {
sec: self.header.stamp.sec as i32,
nanosec: self.header.stamp.nsec,
sec: msg.header.stamp.sec as i32,
nanosec: msg.header.stamp.nsec,
},
frame_id: self.header.frame_id,
frame_id: msg.header.frame_id,
},
height: self.height,
width: self.width,
fields: self
height: msg.height,
width: msg.width,
fields: msg
.fields
.into_iter()
.map(|field| r2r::sensor_msgs::msg::PointField {
@ -92,11 +92,11 @@ impl Into<r2r::sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
count: field.count,
})
.collect(),
is_bigendian: self.is_bigendian,
point_step: self.point_step,
row_step: self.row_step,
data: self.data,
is_dense: self.is_dense,
is_bigendian: msg.is_bigendian,
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
is_dense: msg.is_dense,
}
}
}
@ -135,20 +135,20 @@ impl From<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
}
#[cfg(feature = "rosrust_msg")]
impl Into<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
fn into(self) -> rosrust_msg::sensor_msgs::PointCloud2 {
impl From<PointCloud2Msg> for rosrust_msg::sensor_msgs::PointCloud2 {
fn from(msg: PointCloud2Msg) -> Self {
rosrust_msg::sensor_msgs::PointCloud2 {
header: rosrust_msg::std_msgs::Header {
seq: self.header.seq,
stamp: rosrust::Time {
sec: self.header.stamp.sec,
nsec: self.header.stamp.nsec,
seq: msg.header.seq,
stamp: rosrust_msg::Time {
sec: msg.header.stamp.sec,
nsec: msg.header.stamp.nsec,
},
frame_id: self.header.frame_id,
frame_id: msg.header.frame_id,
},
height: self.height,
width: self.width,
fields: self
height: msg.height,
width: msg.width,
fields: msg
.fields
.into_iter()
.map(|field| rosrust_msg::sensor_msgs::PointField {
@ -158,11 +158,11 @@ impl Into<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
count: field.count,
})
.collect(),
is_bigendian: self.is_bigendian,
point_step: self.point_step,
row_step: self.row_step,
data: self.data,
is_dense: self.is_dense,
is_bigendian: msg.is_bigendian,
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
is_dense: msg.is_dense,
}
}
}

45
src/writer.rs
View File

@ -132,18 +132,20 @@ pub type WriterXYZL = WriterF32<3, 1, PointXYZL>;
/// }
/// }
///
///impl Into<Point<f32, 3, 1>> for CustomPoint {
/// fn into(self) -> Point<f32, 3, 1> {
///impl From<CustomPoint> for Point<f32, 3, 1> {
/// fn from(point: CustomPoint) -> Self {
/// Point {
/// coords: [self.x, self.y, self.z],
/// meta: [self.i.into()],
/// coords: [point.x, point.y, point.z],
/// meta: [
/// point.i.into(),
/// ],
/// }
/// }
///}
///
/// impl MetaNames<METADIM> for CustomPoint {
/// fn meta_names() -> [String; METADIM] {
/// [format!("intensity")]
/// fn meta_names() -> [&'static str; METADIM] {
/// ["intensity"]
/// }
/// }
/// impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
@ -198,14 +200,14 @@ where
if DIM > 1 {
fields.push(PointFieldMsg {
name: "x".to_string(),
name: "x".into(),
offset: 0,
datatype: T::field_datatype().into(),
count: 1,
});
fields.push(PointFieldMsg {
name: "y".to_string(),
name: "y".into(),
offset: SIZE as u32,
datatype: T::field_datatype().into(),
count: 1,
@ -214,42 +216,45 @@ where
if DIM == 3 {
fields.push(PointFieldMsg {
name: "z".to_string(),
name: "z".into(),
offset: 2 * SIZE as u32,
datatype: T::field_datatype().into(),
count: 1,
});
}
let first_point = self.coordinates.next().ok_or(ConversionError::NoPoints)?;
let point: Point<T, DIM, METADIM> = first_point.clone().into();
let meta_names = C::meta_names();
let mut meta_offsets_acc = DIM as u32 * SIZE as u32;
for(meta_value, meta_name) in point.meta.into_iter().zip(meta_names.into_iter()) {
for (meta_value, meta_name) in point.meta.into_iter().zip(meta_names.into_iter()) {
let datatype_code = meta_value.datatype.into();
if FieldDatatype::try_from(datatype_code).is_err() {
return Err(ConversionError::UnsupportedFieldType);
}
fields.push(PointFieldMsg {
name: meta_name,
name: meta_name.into(),
offset: meta_offsets_acc,
datatype: datatype_code,
count: 1,
});
meta_offsets_acc += meta_value.datatype.size() as u32
}
let mut cloud = PointCloud2Msg {
point_step: fields.iter().fold(Default::default(), |acc, field| {
let field_type: FieldDatatype = field
.datatype
.try_into()
.expect("Unsupported field but checked before.");
let field_size = field_type.size();
acc + field.count * field_size as u32
}),
..Default::default()
};
let mut cloud = PointCloud2Msg::default();
cloud.point_step = fields.iter().fold(0, |acc, field| {
let field_type: FieldDatatype = field.datatype.try_into().expect("Unsupported field but checked before.");
let field_size = field_type.size();
acc + field.count * field_size as u32
});
// actual point -> byte conversion -- O(n)
add_point_to_byte_buffer(first_point, &mut cloud)?;
for coords in self.coordinates {

95
tests/e2e_test.rs
View File

@ -56,17 +56,17 @@ fn custom_xyz_f32() {
}
}
impl Into<Point<f32, 3, 0>> for CustomPoint {
fn into(self) -> Point<f32, 3, 0> {
impl From<CustomPoint> for Point<f32, 3, 0> {
fn from(point: CustomPoint) -> Self {
Point {
coords: [self.x, self.y, self.z],
coords: [point.x, point.y, point.z],
meta: [],
}
}
}
impl convert::MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
fn meta_names() -> [&'static str; METADIM] {
[]
}
}
@ -121,18 +121,18 @@ fn custom_xyzi_f32() {
}
}
impl Into<Point<f32, 3, 1>> for CustomPoint {
fn into(self) -> Point<f32, 3, 1> {
impl From<CustomPoint> for Point<f32, 3, 1> {
fn from(point: CustomPoint) -> Self {
Point {
coords: [self.x, self.y, self.z],
meta: [self.i.into()],
coords: [point.x, point.y, point.z],
meta: [point.i.into()],
}
}
}
impl convert::MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
[format!("intensity")]
fn meta_names() -> [&'static str; METADIM] {
["intensity"]
}
}
@ -201,17 +201,23 @@ fn custom_rgba_f32() {
}
}
impl Into<Point<f32, 3, 4>> for CustomPoint {
fn into(self) -> Point<f32, 3, 4> {
impl From<CustomPoint> for Point<f32, 3, 4> {
fn from(point: CustomPoint) -> Self {
Point {
coords: [self.x, self.y, self.z],
meta: [self.r.into(), self.g.into(), self.b.into(), self.a.into()],
coords: [point.x, point.y, point.z],
meta: [
point.r.into(),
point.g.into(),
point.b.into(),
point.a.into(),
],
}
}
}
impl convert::MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
["r", "g", "b", "a"].map(|s| s.to_string())
fn meta_names() -> [&'static str; METADIM] {
["r", "g", "b", "a"]
}
}
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, DIM, METADIM> for CustomPoint {}
@ -726,17 +732,17 @@ fn write_less_than_available() {
}
}
impl Into<Point<f32, 3, 0>> for CustomPoint {
fn into(self) -> Point<f32, 3, 0> {
impl From<CustomPoint> for Point<f32, 3, 0> {
fn from(point: CustomPoint) -> Self {
Point {
coords: [self.x, self.y, self.z],
coords: [point.x, point.y, point.z],
meta: [],
}
}
}
impl convert::MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
fn meta_names() -> [&'static str; METADIM] {
[]
}
}
@ -794,3 +800,52 @@ fn write_less_than_available() {
CustomPoint
);
}
#[test]
fn readme() {
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: 1337.0,
y: 42.0,
z: 69.0,
},
PointXYZ {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
},
];
// For equality test later
let cloud_copy = cloud_points.clone();
// Vector -> Writer -> Message
let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points)
.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);
}