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

more general pointcloud2 with into/from

This commit is contained in:
Christopher Sieh 2023-04-26 10:00:00 +02:00
parent 36a8272e23
commit 1d9525147a
6 changed files with 263 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Cargo.toml
View File

@ -1,6 +1,6 @@
[package]
name = "ros_pointcloud2"
version = "0.1.0"
version = "0.2.0"
edition = "2021"
authors = ["Christopher Sieh <stelzo@steado.de>"]
description = "A library for working with pointcloud2 messages in Rust"
@ -16,6 +16,9 @@ exclude = ["**/tests/**", "**/examples/**", "**/benches/**", "**/target/**", "**
[dependencies]
mem_macros = "1.0.1"
num-traits = "0.2.15"
rosrust_msg = "0.1.7"
fallible-iterator = "0.2.0"
rosrust_msg = { version = "0.1.0", optional = true }
rosrust = { version = "0.9.0", optional = true }
[features]
rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"]

85
README.md
View File

@ -1,18 +1,25 @@
# ROS PointCloud2
A Rust implementation for fast, safe and customizable conversions to and from the `sensor_msgs/PointCloud2` ROS message.
A lightweight Rust implementation for fast, safe and customizable conversions to and from the `sensor_msgs/PointCloud2` ROS message.
```toml
[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.
*Currently only supports [rosrust_msg](https:://github.com/adnanademovic/rosrust) with ROS1.*
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](https://github.com/stelzo/cloudfilter). 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.
```rust
use ros_pointcloud2::ConvertXYZ;
use ros_pointcloud2::pcl_utils::PointXYZ;
use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::fallible_iterator::FallibleIterator;
use rosrust_msg::sensor_msgs::PointCloud2;
// Your points (here using the predefined type PointXYZ).
let cloud_points = vec![
PointXYZ { x: 1.3, y: 1.6, z: 5.7 },
@ -22,10 +29,16 @@ let cloud_points = vec![
let cloud_copy = cloud_points.clone(); // Only for checking equality later.
// Vector -> Convert -> Message
let msg: PointCloud2 = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap();
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(msg).unwrap();
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.
@ -37,14 +50,44 @@ let new_cloud_points = convert.map(|point: PointXYZ| {
assert_eq!(new_cloud_points, cloud_copy);
```
Instead of converting the entire cloud into a `Vector`, you get an `Iterator` that converts each point from the message on the fly.
An example for using this crate is [this filter node](https://github.com/stelzo/cloudfilter). It is also a good starting point for
implementing ROS1 nodes in Rust inside a catkin environment.
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.
```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!()
}
}
```
## Integrations
Currently, we only implement a conversion for the following ROS crate:
- [rosrust](https://github.com/adnanademovic/rosrust)
You can use one by enabling the corresponding feature.
```toml
[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
- Full support for `sensor_msgs/PointCloud2` messages
- Custom types with `From` and `Into` traits
- Easy to integrate into your favorite ROS1 or ROS2 crate
- Custom point types
- Predefined types for common conversions (compared to PCL)
- PointXYZ
- PointXYZI
@ -55,27 +98,17 @@ implementing ROS1 nodes in Rust inside a catkin environment.
- PointXYZNormal
- PointXYZINormal
- PointXYZRGBNormal
- 2D and 3D support
## Usage
Add this to your Cargo.toml:
```toml
[dependencies]
ros_pointcloud2 = "0.1.0"
```
When building, the `rosrust_msg` crate needs to have the ROS environment sourced or use `ROSRUST_MSG_PATH=/opt/ros/$ROS_DISTRO/share cargo build`.
- 2D and 3D
## Custom Types
You can freely convert to your own types without reiterating the entire cloud.
You can freely convert to your own point types without reiterating the entire cloud.
You just need to implement the `Into` and `From` traits.
```rust
use ros_pointcloud2::mem_macros::size_of;
use ros_pointcloud2::{Convert, MetaNames, PointMeta, ConversionError, PointConvertible};
use rosrust_msg::sensor_msgs::PointCloud2;
use ros_pointcloud2::ros_types::PointCloud2Msg;
const DIM : usize = 3;
const METADIM : usize = 4;
@ -135,15 +168,13 @@ let custom_cloud = vec![
// Cloud -> ROS message
let custom_msg: PointCloud2 = MyConverter::try_from(custom_cloud).unwrap().try_into().unwrap();
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
- ROS2 support(!)
- Removing rosrust dependency
- Benchmark vs PCL
- Proper error passing to the iterator result (currently merged into `PointConversionError`)
- remove allocations

50
src/lib.rs
View File

@ -1,8 +1,10 @@
#![doc = include_str!("../README.md")]
pub mod pcl_utils;
pub mod ros_types;
use num_traits::Zero;
use crate::pcl_utils::*;
use crate::ros_types::{PointCloud2Msg, PointFieldMsg};
#[macro_use]
pub extern crate mem_macros;
@ -21,7 +23,7 @@ pub enum ConversionError {
MetaIndexLengthMismatch,
EndOfBuffer,
PointConversionError,
MetaDatatypeMissmatch,
MetaDatatypeMismatch,
}
/// Trait to convert a point to a tuple of coordinates and meta data.
@ -120,7 +122,7 @@ fn convert_msg_code_to_type(code: u8) -> Result<FieldDatatype, ConversionError>
}
}
fn check_coord(coord: Option<usize>, fields: &Vec<rosrust_msg::sensor_msgs::PointField>, xyz_field_type: &FieldDatatype) -> Result<rosrust_msg::sensor_msgs::PointField, ConversionError> {
fn check_coord(coord: Option<usize>, fields: &Vec<PointFieldMsg>, xyz_field_type: &FieldDatatype) -> Result<PointFieldMsg, ConversionError> {
match coord {
Some(y_idx) => {
let field = &fields[y_idx];
@ -149,7 +151,8 @@ pub trait MetaNames<const METADIM: usize> {
/// # Example
/// ```
/// use ros_pointcloud2::mem_macros::size_of;
/// use ros_pointcloud2::{Convert, PointMeta};;
/// use ros_pointcloud2::{Convert, PointMeta};
/// use ros_pointcloud2::ros_types::PointCloud2Msg;
/// const DIM : usize = 3; // how many dimensions your point has (e.g. x, y, z)
/// const METADIM : usize = 4; // how many meta fields you have (e.g. r, g, b, a)
/// type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, ([f32; DIM], [PointMeta; METADIM])>;
@ -157,7 +160,7 @@ pub trait MetaNames<const METADIM: usize> {
pub struct Convert<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> {
iteration: usize,
coordinates: Vec<C>,
_phantom: std::marker::PhantomData<T>,
phantom_t: std::marker::PhantomData<T>,
data: Vec<u8>,
point_step_size: usize,
cloud_length: usize,
@ -199,7 +202,6 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
///
/// # Example
/// ```
/// use rosrust_msg::sensor_msgs::PointCloud2;
/// use ros_pointcloud2::{ConvertXYZ, ConversionError};
/// use ros_pointcloud2::pcl_utils::PointXYZ;
///
@ -228,7 +230,7 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
}
Ok(Self {
_phantom: std::marker::PhantomData,
phantom_t: std::marker::PhantomData,
iteration: usize::zero(),
coordinates: cloud,
data: Vec::new(),
@ -312,7 +314,7 @@ impl PointMeta {
/// ```
pub fn get<T: FromBytes>(&self) -> Result<T, ConversionError> {
if self.datatype != T::field_datatype() {
return Err(ConversionError::MetaDatatypeMissmatch);
return Err(ConversionError::MetaDatatypeMismatch);
}
let size = datatype_size(&T::field_datatype());
if let Some(bytes) = self.bytes.get(0..size) {
@ -323,7 +325,7 @@ impl PointMeta {
}
}
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> TryFrom<rosrust_msg::sensor_msgs::PointCloud2> for Convert<T, SIZE, DIM, METADIM, C>
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> TryFrom<PointCloud2Msg> for Convert<T, SIZE, DIM, METADIM, C>
{
type Error = ConversionError;
@ -333,7 +335,7 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
/// # Example
/// Since we do not have a ROS node here, we first create a PointCloud2 message and then convert back to a Convert struct.
/// ```
/// use rosrust_msg::sensor_msgs::PointCloud2;
/// use ros_pointcloud2::ros_types::PointCloud2Msg;
/// use ros_pointcloud2::ConvertXYZ;
/// use ros_pointcloud2::pcl_utils::PointXYZ;
///
@ -341,12 +343,13 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
/// let msg: PointCloud2 = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap();
/// let msg: PointCloud2Msg = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap();
///
/// let convert: ConvertXYZ = ConvertXYZ::try_from(msg).unwrap(); // parse message
/// ```
fn try_from(cloud: rosrust_msg::sensor_msgs::PointCloud2) -> Result<Self, Self::Error> {
if cloud.fields.len() < 2 {
fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> {
let cloud: PointCloud2Msg = cloud.into();
if cloud.fields.len() < DIM {
return Err(ConversionError::NotEnoughFields);
}
@ -433,7 +436,7 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
}
Ok(Self {
_phantom: std::marker::PhantomData,
phantom_t: std::marker::PhantomData,
iteration: usize::zero(),
coordinates: Vec::new(),
data: cloud.data,
@ -460,7 +463,7 @@ fn datatype_size(datatype: &FieldDatatype) -> usize {
}
}
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> TryInto<rosrust_msg::sensor_msgs::PointCloud2> for Convert<T, SIZE, DIM, METADIM, C> {
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> TryInto<PointCloud2Msg> for Convert<T, SIZE, DIM, METADIM, C> {
type Error = ConversionError;
/// Convert the point cloud to a ROS message.
@ -469,7 +472,7 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
///
/// # Example
/// ```
/// use rosrust_msg::sensor_msgs::PointCloud2;
/// use ros_pointcloud2::ros_types::PointCloud2Msg;
/// use ros_pointcloud2::ConvertXYZ;
/// use ros_pointcloud2::pcl_utils::PointXYZ;
///
@ -477,10 +480,10 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
/// let msg_out: PointCloud2 = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap();
/// let msg_out: PointCloud2Msg = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap();
/// ```
fn try_into(self) -> Result<rosrust_msg::sensor_msgs::PointCloud2, Self::Error> {
let mut cloud = rosrust_msg::sensor_msgs::PointCloud2::default();
fn try_into(self) -> Result<PointCloud2Msg, Self::Error> {
let mut cloud = PointCloud2Msg::default();
// Define the message fields
let mut fields = Vec::new();
@ -491,13 +494,13 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
let datatype: u8 = convert_to_msg_code(&self.xyz_field_type);
if DIM > 1 {
fields.push(rosrust_msg::sensor_msgs::PointField {
fields.push(PointFieldMsg {
name: "x".to_string(),
offset: 0 * SIZE as u32,
datatype,
count: 1,
});
fields.push(rosrust_msg::sensor_msgs::PointField {
fields.push(PointFieldMsg {
name: "y".to_string(),
offset: 1 * SIZE as u32,
datatype,
@ -506,7 +509,7 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
}
if DIM == 3 {
fields.push(rosrust_msg::sensor_msgs::PointField {
fields.push(PointFieldMsg {
name: "z".to_string(),
offset: 2 * SIZE as u32,
datatype,
@ -534,7 +537,7 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
let field_size = datatype_size(&field_type);
offset += field_size as u32;
}
fields.push(rosrust_msg::sensor_msgs::PointField {
fields.push(PointFieldMsg {
name: meta_names[idx].to_string(),
offset,
datatype,
@ -582,8 +585,7 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
}
}
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> fallible_iterator::FallibleIterator
for Convert<T, SIZE, DIM, METADIM, C>
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> fallible_iterator::FallibleIterator for Convert<T, SIZE, DIM, METADIM, C>
{
type Item = C;
type Error = ConversionError;

146
src/ros_types.rs Normal file
View File

@ -0,0 +1,146 @@
#[derive(Clone, Debug)]
pub struct TimeMsg {
pub sec: u32,
pub nsec: u32,
}
impl Default for TimeMsg {
fn default() -> Self {
Self {
sec: 0,
nsec: 0,
}
}
}
#[derive(Clone, Debug)]
pub struct HeaderMsg {
pub seq: u32,
pub stamp: TimeMsg,
pub frame_id: String,
}
impl Default for HeaderMsg {
fn default() -> Self {
Self {
seq: 0,
stamp: TimeMsg::default(),
frame_id: String::new(),
}
}
}
#[derive(Clone, Debug)]
pub struct PointFieldMsg {
pub name: String,
pub offset: u32,
pub datatype: u8,
pub count: u32,
}
impl Default for PointFieldMsg {
fn default() -> Self {
Self {
name: String::new(),
offset: 0,
datatype: 0,
count: 0,
}
}
}
#[derive(Clone, Debug)]
pub struct PointCloud2Msg {
pub header: HeaderMsg,
pub height: u32,
pub width: u32,
pub fields: Vec<PointFieldMsg>,
pub is_bigendian: bool,
pub point_step: u32,
pub row_step: u32,
pub data: Vec<u8>,
pub is_dense: bool,
}
impl Default for PointCloud2Msg {
fn default() -> Self {
Self {
header: HeaderMsg::default(),
height: 0,
width: 0,
fields: Vec::new(),
is_bigendian: false,
point_step: 0,
row_step: 0,
data: Vec::new(),
is_dense: false,
}
}
}
#[cfg(feature = "rosrust_msg")]
impl From<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
fn from(msg: rosrust_msg::sensor_msgs::PointCloud2) -> Self {
Self {
header: HeaderMsg {
seq: msg.header.seq,
stamp: TimeMsg {
sec: msg.header.stamp.sec,
nsec: msg.header.stamp.nsec,
},
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 = "rosrust_msg")]
impl Into<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
fn into(self) -> rosrust_msg::sensor_msgs::PointCloud2 {
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,
},
frame_id: self.header.frame_id,
},
height: self.height,
width: self.width,
fields: self
.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: self.is_bigendian,
point_step: self.point_step,
row_step: self.row_step,
data: self.data,
is_dense: self.is_dense,
}
}
}

14
tests/e2e_test.rs
View File

@ -44,7 +44,7 @@ fn custom_xyz_f32() {
CustomPoint { x: 7.0, y: 8.0, z: 9.0 },
];
let copy = custom_cloud.clone();
let custom_msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = MyConverter::try_from(custom_cloud).unwrap().try_into();
let custom_msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = MyConverter::try_from(custom_cloud).unwrap().try_into();
assert!(custom_msg.is_ok());
let to_custom_type = MyConverter::try_from(custom_msg.unwrap());
assert!(to_custom_type.is_ok());
@ -94,7 +94,7 @@ fn custom_xyzi_f32() {
CustomPoint { x: f32::MAX, y: f32::MIN, z: f32::MAX, i: u8::MAX },
];
let copy = custom_cloud.clone();
let custom_msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = MyConverter::try_from(custom_cloud).unwrap().try_into();
let custom_msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = MyConverter::try_from(custom_cloud).unwrap().try_into();
assert!(custom_msg.is_ok());
let to_custom_type = MyConverter::try_from(custom_msg.unwrap());
assert!(to_custom_type.is_ok());
@ -150,7 +150,7 @@ fn custom_rgba_f32() {
CustomPoint { x: f32::MAX, y: f32::MIN, z: f32::MAX, r: u8::MAX, g: u8::MAX, b: u8::MAX, a: u8::MAX },
];
let copy = custom_cloud.clone();
let custom_msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = MyConverter::try_from(custom_cloud).unwrap().try_into();
let custom_msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = MyConverter::try_from(custom_cloud).unwrap().try_into();
assert!(custom_msg.is_ok());
let to_custom_type = MyConverter::try_from(custom_msg.unwrap());
assert!(to_custom_type.is_ok());
@ -169,7 +169,7 @@ fn converterxyz() {
];
let copy = cloud.clone();
let msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = ConvertXYZ::try_from(cloud).unwrap().try_into();
let msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = ConvertXYZ::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_xyz_type = ConvertXYZ::try_from(msg.unwrap());
assert!(to_xyz_type.is_ok());
@ -189,7 +189,7 @@ fn converterxyzrgba() {
];
let copy = cloud.clone();
let msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = ConvertXYZRGBA::try_from(cloud).unwrap().try_into();
let msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = ConvertXYZRGBA::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_xyzrgba_type = ConvertXYZRGBA::try_from(msg.unwrap());
assert!(to_xyzrgba_type.is_ok());
@ -208,7 +208,7 @@ fn converterxyzinormal() {
];
let copy = cloud.clone();
let msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = ConvertXYZINormal::try_from(cloud).unwrap().try_into();
let msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = ConvertXYZINormal::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_xyzinormal_type = ConvertXYZINormal::try_from(msg.unwrap());
assert!(to_xyzinormal_type.is_ok());
@ -227,7 +227,7 @@ fn converterxyzrgbnormal() {
];
let copy = cloud.clone();
let msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = ConvertXYZRGBNormal::try_from(cloud).unwrap().try_into();
let msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = ConvertXYZRGBNormal::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_xyzrgbnormal_type = ConvertXYZRGBNormal::try_from(msg.unwrap());
assert!(to_xyzrgbnormal_type.is_ok());

21
tests/rosrust_msg_test.rs Normal file
View File

@ -0,0 +1,21 @@
#[cfg(feature = "rosrust_msg")]
#[test]
fn convertxyz_rosrust_msg() {
use ros_pointcloud2::{ConvertXYZ};
use ros_pointcloud2::pcl_utils::PointXYZ;
use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::fallible_iterator::FallibleIterator;
let cloud = vec![
PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
PointXYZ { x: 7.0, y: 8.0, z: 9.0 },
];
let copy = cloud.clone();
let internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().try_into().unwrap();
let rosrust_msg_cloud: rosrust_msg::sensor_msgs::PointCloud2 = internal_cloud.into();
let convert_back_internal: PointCloud2Msg = rosrust_msg_cloud.into();
let to_convert: ConvertXYZ = ConvertXYZ::try_from(convert_back_internal).unwrap();
let back_to_type = to_convert.map(|point| Ok(point)).collect::<Vec<PointXYZ>>();
assert_eq!(copy, back_to_type.unwrap());
}