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

merge main

This commit is contained in:
stelzo 2024-04-29 23:09:33 +02:00
parent 12d0716b9b a0ca014e13
commit fe73a1b613
No known key found for this signature in database
GPG Key ID: FC4EF89052319374
7 changed files with 192 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
README.md
View File

@ -10,8 +10,6 @@ Providing an easy to use, generics defined, point-wise iterator abstraction over
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`.
## Examples
Using a `Vec<>`.
```rust
use ros_pointcloud2::{
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
@ -19,24 +17,15 @@ use ros_pointcloud2::{
// Your points (here using the predefined type PointXYZ).
let cloud_points = vec![
PointXYZ {
x: 91.486,
y: -4.1,
z: 42.0001,
},
PointXYZ {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
},
PointXYZ {x: 91.486, y: -4.1, z: 42.0001,},
PointXYZ {x: f32::MAX, y: f32::MIN, z: f32::MAX,},
];
// For equality test later
let cloud_copy = cloud_points.clone();
// Vector -> Writer -> Message.
// You can also just give the Vec or anything that implements `IntoIter`.
// You can also just give the Vec or anything that implements `IntoIterator`.
let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter())
.try_into() // iterating points here O(n)
.unwrap();
@ -49,7 +38,7 @@ let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter())
// ... now incoming from a topic.
// let internal_msg: PointCloud2Msg = msg.into();
// Message -> Reader. The Reader implements the Iterator trait.
// Message -> Reader -> your pipeline. The Reader implements the Iterator trait.
let reader = ReaderXYZ::try_from(internal_msg).unwrap();
let new_cloud_points = reader
.map(|point: PointXYZ| {

55
src/convert.rs
View File

@ -8,10 +8,8 @@ pub enum FieldDatatype {
I32,
U8,
U16,
#[default]
U32,
I8,
I16,
}
@ -135,41 +133,32 @@ 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.
/// Always make sure to use the same order as in your conversion implementation to have a correct mapping.
///
/// This trait is needed to implement the `PointConvertible` trait.
///
/// # Example for full point conversion.
/// ```
/// use ros_pointcloud2::{Point, PointConvertible, MetaNames, size_of};
///
/// #[derive(Clone, Debug, PartialEq, Copy)]
/// pub struct MyPointXYZI {
/// pub x: f32,
/// pub y: f32,
/// pub z: f32,
/// pub intensity: f32,
/// }
///
/// impl MetaNames<1> for MyPointXYZI {
/// fn meta_names() -> [&'static str; 1] {
/// ["intensity"]
/// }
/// }
/// ```
pub trait MetaNames<const METADIM: usize> {
fn meta_names() -> [&'static str; METADIM];
}
#[inline(always)]
pub(crate) fn add_point_to_byte_buffer<
T: FromBytes,
const SIZE: usize,
const DIM: usize,
const METADIM: usize,
C: PointConvertible<T, SIZE, DIM, METADIM>,
>(
coords: C,
cloud: &mut PointCloud2Msg,
) -> Result<bool, ConversionError> {
let point: Point<T, DIM, METADIM> = coords.into();
// (x, y, z...)
point
.coords
.iter()
.for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice()));
// meta data description
point.meta.iter().for_each(|meta| {
let truncated_bytes = &meta.bytes[0..meta.datatype.size()];
cloud.data.extend_from_slice(truncated_bytes);
});
cloud.width += 1;
Ok(true)
}
/// This trait is used to convert a byte slice to a primitive type.
/// All PointField types are supported.
pub trait FromBytes: Default + Sized + Copy + GetFieldDatatype {

109
src/lib.rs
View File

@ -1,6 +1,6 @@
//! A library to work with the PointCloud2 message type from ROS.
//!
//! ros_pointcloud2 mainly provides iterator-based [`ros_pointcloud2::Reader`] and [`ros_pointcloud2::Writer`] to interact
//! ros_pointcloud2 mainly provides iterator-based [`ros_pointcloud2::reader`] and [`ros_pointcloud2::writer`] to interact
//! with the internal message type [`ros_pointcloud2::PointCloud2Msg`] without forcing an iteration.
//!
//! For ROS interoperability, the message type generated by the respective crate must be converted to
@ -23,25 +23,15 @@
//!
//! // 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,
//! },
//! 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`.
//! // You can also just give the Vec or anything that implements `IntoIterator`.
//! let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter())
//! .try_into() // iterating points here O(n)
//! .unwrap();
@ -54,7 +44,7 @@
//! // ... now incoming from a topic.
//! // let internal_msg: PointCloud2Msg = msg.into();
//!
//! // Message -> Reader. The Reader implements the Iterator trait.
//! // Message -> Reader -> your pipeline. The Reader implements the Iterator trait.
//! let reader = ReaderXYZ::try_from(internal_msg).unwrap();
//! let new_cloud_points = reader
//! .map(|point: PointXYZ| {
@ -91,10 +81,14 @@ macro_rules! size_of {
pub mod reader;
pub mod writer;
pub use convert::MetaNames;
pub use ros_types::PointCloud2Msg;
use crate::convert::*;
use crate::pcl_utils::*;
use crate::convert::{
FieldDatatype,
FromBytes,
};
use crate::ros_types::PointFieldMsg;
/// All errors that can occur for creating Reader and Writer.
@ -119,6 +113,42 @@ pub enum ConversionError {
DataLengthMismatch,
}
/// Internal point representation. It is used to store the coordinates and meta data of a point.
/// In each iteration, the Reader will convert the internal representation to the desired point type.
/// Implement the `From` traits for your point type to use the conversion as part of the [`PointConvertible`] trait.
///
/// # Example
/// ```
/// use ros_pointcloud2::Point;
///
/// #[derive(Clone, Debug, PartialEq, Copy)]
/// pub struct MyPointXYZI {
/// pub x: f32,
/// pub y: f32,
/// pub z: f32,
/// pub intensity: f32,
/// }
///
/// impl From<MyPointXYZI> for Point<f32, 3, 1> {
/// fn from(point: MyPointXYZI) -> Self {
/// Point {
/// coords: [point.x, point.y, point.z],
/// meta: [point.intensity.into()],
/// }
/// }
/// }
///
/// impl From<Point<f32, 3, 1>> for MyPointXYZI {
/// fn from(point: Point<f32, 3, 1>) -> Self {
/// Self {
/// x: point.coords[0],
/// y: point.coords[1],
/// z: point.coords[2],
/// intensity: point.meta[0].get(),
/// }
/// }
/// }
/// ```
pub struct Point<T, const DIM: usize, const METADIM: usize> {
pub coords: [T; DIM],
pub meta: [PointMeta; METADIM],
@ -126,6 +156,47 @@ pub struct Point<T, const DIM: usize, const METADIM: usize> {
/// Trait to convert a point to a tuple of coordinates and meta data.
/// Implement this trait for your point type to use the conversion.
///
/// # Example
/// ```
/// use ros_pointcloud2::{Point, PointConvertible, MetaNames, size_of};
///
/// #[derive(Clone, Debug, PartialEq, Copy)]
/// pub struct MyPointXYZI {
/// pub x: f32,
/// pub y: f32,
/// pub z: f32,
/// pub intensity: f32,
/// }
///
/// impl From<MyPointXYZI> for Point<f32, 3, 1> {
/// fn from(point: MyPointXYZI) -> Self {
/// Point {
/// coords: [point.x, point.y, point.z],
/// meta: [point.intensity.into()],
/// }
/// }
/// }
///
/// impl From<Point<f32, 3, 1>> for MyPointXYZI {
/// fn from(point: Point<f32, 3, 1>) -> Self {
/// Self {
/// x: point.coords[0],
/// y: point.coords[1],
/// z: point.coords[2],
/// intensity: point.meta[0].get(),
/// }
/// }
/// }
///
/// impl MetaNames<1> for MyPointXYZI {
/// fn meta_names() -> [&'static str; 1] {
/// ["intensity"]
/// }
/// }
///
/// impl PointConvertible<f32, {size_of!(f32)}, 3, 1> for MyPointXYZI {}
/// ```
pub trait PointConvertible<T, const SIZE: usize, const DIM: usize, const METADIM: usize>:
From<Point<T, DIM, METADIM>> + Into<Point<T, DIM, METADIM>> + MetaNames<METADIM> + Clone + 'static
where
@ -155,14 +226,14 @@ pub struct PointMeta {
impl Default for PointMeta {
fn default() -> Self {
Self {
bytes: [0; std::mem::size_of::<f64>()],
bytes: [u8::default(); std::mem::size_of::<f64>()],
datatype: FieldDatatype::F32,
}
}
}
impl PointMeta {
/// Create a new PointMeta from a value
/// Create a new PointMeta from a value.
///
/// # Example
/// ```

19
src/reader.rs
View File

@ -1,4 +1,19 @@
use crate::*;
use crate::{
pcl_utils::*,
Point,
PointCloud2Msg,
PointConvertible,
ConversionError,
MetaNames,
PointMeta,
convert::{
FromBytes,
FieldDatatype,
load_loadable,
Endianness,
check_coord,
},
};
/// 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> =
@ -124,7 +139,7 @@ pub type ReaderXYZL = ReaderF32<3, 1, PointXYZL>;
/// ## Example
/// ```
/// use ros_pointcloud2::{
/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, convert::MetaNames, PointMeta,
/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta,
/// };
///
/// type Xyz = f32; // coordinate type

83
src/ros_types.rs
View File

@ -142,75 +142,16 @@ 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: msg.header.seq,
stamp: TimeMsg {
sec: msg.header.stamp.sec,
nsec: msg.header.stamp.nsec,
seq: self.header.seq,
stamp: rosrust::Time {
sec: self.header.stamp.sec,
nsec: self.header.stamp.nsec,
},
frame_id: msg.header.frame_id,
},
height: msg.height,
width: msg.width,
fields: msg
.fields
.into_iter()
.map(|field| rosrust_msg::sensor_msgs::PointField {
name: field.name,
offset: field.offset,
datatype: field.datatype,
count: field.count,
})
.collect(),
is_bigendian: msg.is_bigendian,
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
is_dense: msg.is_dense,
}
}
}
#[cfg(feature = "rclrs_msg")]
impl From<sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
fn from(msg: sensor_msgs::msg::PointCloud2) -> Self {
Self {
header: HeaderMsg {
seq: 0,
stamp: msg.header.stamp,
frame_id: msg.header.frame_id,
},
height: msg.height,
width: msg.width,
fields: msg
.fields
.into_iter()
.map(|field| PointFieldMsg {
name: field.name,
offset: field.offset,
datatype: field.datatype,
count: field.count,
})
.collect(),
is_bigendian: msg.is_bigendian,
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
is_dense: msg.is_dense,
}
}
}
#[cfg(feature = "rclrs_msg")]
impl Into<sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
fn into(self) -> sensor_msgs::msg::PointCloud2 {
sensor_msgs::msg::PointCloud2 {
header: std_msgs::msg::Header {
stamp: self.header.stamp,
frame_id: self.header.frame_id,
},
height: self.height,
width: self.width,
fields: self
height: msg.height,
width: msg.width,
fields: msg
.fields
.into_iter()
.map(|field| sensor_msgs::msg::PointField {
@ -220,11 +161,11 @@ impl Into<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,
}
}
}

45
src/writer.rs
View File

@ -1,4 +1,15 @@
use crate::*;
use crate::{
pcl_utils::*,
Point,
PointCloud2Msg,
PointConvertible,
ConversionError,
ros_types::PointFieldMsg,
convert::{
FromBytes,
FieldDatatype,
},
};
/// Convenience type for a Writer that reads coordinates as f32. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterF32<const DIM: usize, const METADIM: usize, C> =
@ -105,7 +116,7 @@ pub type WriterXYZL = WriterF32<3, 1, PointXYZL>;
/// ## Example
/// ```
/// use ros_pointcloud2::{
/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, convert::MetaNames, PointMeta,
/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta,
/// };
///
/// type Xyz = f32; // coordinate type
@ -303,3 +314,33 @@ where
}
}
}
#[inline(always)]
fn add_point_to_byte_buffer<
T: FromBytes,
const SIZE: usize,
const DIM: usize,
const METADIM: usize,
C: PointConvertible<T, SIZE, DIM, METADIM>,
>(
coords: C,
cloud: &mut PointCloud2Msg,
) -> Result<bool, ConversionError> {
let point: Point<T, DIM, METADIM> = coords.into();
// (x, y, z...)
point
.coords
.iter()
.for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice()));
// meta data description
point.meta.iter().for_each(|meta| {
let truncated_bytes = &meta.bytes[0..meta.datatype.size()];
cloud.data.extend_from_slice(truncated_bytes);
});
cloud.width += 1;
Ok(true)
}

8
tests/e2e_test.rs
View File

@ -65,7 +65,7 @@ fn custom_xyz_f32() {
}
}
impl convert::MetaNames<METADIM> for CustomPoint {
impl MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [&'static str; METADIM] {
[]
}
@ -130,7 +130,7 @@ fn custom_xyzi_f32() {
}
}
impl convert::MetaNames<METADIM> for CustomPoint {
impl MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [&'static str; METADIM] {
["intensity"]
}
@ -215,7 +215,7 @@ fn custom_rgba_f32() {
}
}
impl convert::MetaNames<METADIM> for CustomPoint {
impl MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [&'static str; METADIM] {
["r", "g", "b", "a"]
}
@ -741,7 +741,7 @@ fn write_less_than_available() {
}
}
impl convert::MetaNames<METADIM> for CustomPoint {
impl MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [&'static str; METADIM] {
[]
}