From 1fcf75be89688cdb8193d5ebcf83ea32f5f1b437 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Thu, 30 Jan 2025 14:28:14 -0800 Subject: [PATCH] replacing rosrust with roslibrust entirely, instead of maintaining both, and using roslibrust_util for roslibrust message definitions --- Cargo.toml | 2 +- README.md | 9 +- derive-test/Cargo.toml | 2 +- ros_pointcloud2/Cargo.toml | 19 ++-- ros_pointcloud2/src/iterator.rs | 6 +- ros_pointcloud2/src/lib.rs | 2 +- ros_pointcloud2/src/ros.rs | 116 +++------------------- ros_pointcloud2/tests/rosrust_msg_test.rs | 8 +- 8 files changed, 36 insertions(+), 128 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index bb3c954..ddbcf43 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,5 +1,5 @@ [workspace] -members = [ "derive-test","rpcl2", "rpcl2-derive"] +members = ["derive-test", "ros_pointcloud2", "rpcl2-derive"] resolver = "2" diff --git a/README.md b/README.md index c6cd2ab..0d67f5c 100644 --- a/README.md +++ b/README.md @@ -43,8 +43,8 @@ let processed_cloud = in_msg.try_into_iter().unwrap() There are currently 3 integrations for common ROS crates. -- [rosrust_msg](https://github.com/adnanademovic/rosrust) - - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rosrust_noetic.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rosrust_noetic.yml) +- [roslibrust_msg](https://github.com/roslibrust/roslibrust) + - TBD roslibrust test to replace old rosrust one - [r2r_msg](https://github.com/sequenceplanner/r2r) - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_galactic.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_galactic.yml) - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_humble.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_humble.yml) @@ -55,13 +55,14 @@ There are currently 3 integrations for common ROS crates. - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rclrs_iron.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rclrs_iron.yml) - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rclrs_jazzy.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rclrs_jazzy.yml) -You can use `rosrust` and `r2r` by enabling the respective feature: +You can use `roslibrust` and `r2r` by enabling the respective feature: ```toml [dependencies] ros_pointcloud2 = { version = "*", features = ["r2r_msg"]} # or -ros_pointcloud2 = { version = "*", features = ["rosrust_msg"]} +# ros_pointcloud2 = { version = "*", features = ["roslibrust_msg"]} +ros_pointcloud2 = { version = "*", features = ["roslibrust_msg"]} ``` ### rclrs (ros2_rust) diff --git a/derive-test/Cargo.toml b/derive-test/Cargo.toml index 92b6f05..8aac038 100644 --- a/derive-test/Cargo.toml +++ b/derive-test/Cargo.toml @@ -15,5 +15,5 @@ categories = [ ] [dependencies] -ros_pointcloud2 = { path = "../rpcl2", features = ["std", "derive"] } +ros_pointcloud2 = { path = "../ros_pointcloud2", features = ["std", "derive"] } rpcl2-derive = { path = "../rpcl2-derive" } diff --git a/ros_pointcloud2/Cargo.toml b/ros_pointcloud2/Cargo.toml index ff6630f..51dc84e 100644 --- a/ros_pointcloud2/Cargo.toml +++ b/ros_pointcloud2/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "ros_pointcloud2" -version = "0.5.0" +version = "0.6.0" edition = "2021" authors = ["Christopher Sieh "] description = "Customizable conversions for working with sensor_msgs/PointCloud2." @@ -31,8 +31,6 @@ exclude = [ rust-version = "1.63" [dependencies] -rosrust_msg = { version = "0.1", optional = true } -rosrust = { version = "0.9.11", optional = true } r2r = { version = "0.9", optional = true } rayon = { version = "1", optional = true } nalgebra = { version = "0.33", optional = true, default-features = false } @@ -40,16 +38,14 @@ rpcl2-derive = { version = "0.3", optional = true, path = "../rpcl2-derive" } memoffset = { version = "0.9", optional = true } [dependencies.roslibrust] -version = "0.10.2" -optional = true +git = "https://github.com/lucasw/roslibrust" +branch = "get_rx_msg_def_v0_12" features = ["ros1"] - -[dependencies.roslibrust_codegen] -version = "0.10" optional = true -[dependencies.roslibrust_codegen_macro] -version = "0.10" +[dependencies.roslibrust_util] +git = "https://github.com/lucasw/tf_roslibrust" +version = "0.2.0" optional = true [dev-dependencies] @@ -63,8 +59,7 @@ harness = false path = "benches/roundtrip.rs" [features] -roslibrust_msg = ["dep:roslibrust", "dep:roslibrust_codegen", "dep:roslibrust_codegen_macro"] -rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"] +roslibrust_msg = ["dep:roslibrust", "dep:roslibrust_util"] r2r_msg = ["dep:r2r"] rayon = ["dep:rayon"] derive = ["dep:rpcl2-derive", "dep:memoffset"] diff --git a/ros_pointcloud2/src/iterator.rs b/ros_pointcloud2/src/iterator.rs index a17f4a9..0674380 100644 --- a/ros_pointcloud2/src/iterator.rs +++ b/ros_pointcloud2/src/iterator.rs @@ -15,15 +15,15 @@ use alloc::vec::Vec; /// When using within a ROS node, the PointCloud2 (created by the ROS crate) must be converted first. /// The cost of this operation is low, as it mostly moves ownership without iterating over the point data. /// -/// ROS1 with rosrust: -/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback +/// ROS1 with roslibrust: +/// let msg: roslibrust_msg::sensor_msgs::PointCloud2; // inside the callback /// let converted: ros_pointcloud2::PointCloud2Msg = msg.into(); /// /// ROS2 with r2r: /// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into(); /// let converted: ros_pointcloud2::PointCloud2Msg = msg.into(); /// -/// `ros_pointcloud2` supports r2r, rclrs and rosrust as conversion targets out of the box via feature flags. +/// `ros_pointcloud2` supports r2r, rclrs and roslibrust as conversion targets out of the box via feature flags. /// pub struct PointCloudIterator where diff --git a/ros_pointcloud2/src/lib.rs b/ros_pointcloud2/src/lib.rs index b446dd4..968a5bf 100644 --- a/ros_pointcloud2/src/lib.rs +++ b/ros_pointcloud2/src/lib.rs @@ -54,7 +54,7 @@ //! //! # Features //! - r2r_msg — Integration for the ROS2 library [r2r](https://github.com/sequenceplanner/r2r). -//! - rosrust_msg — Integration with the [rosrust](https://github.com/adnanademovic/rosrust) library for ROS1 message types. +//! - roslibrust_msg — Integration with the [roslibrust](https://github.com/roslibrust/roslibrust) library for ROS1 message types. //! - (rclrs_msg) — Integration for ROS2 [rclrs](https://github.com/ros2-rust/ros2_rust) but it currently needs [this workaround](https://github.com/stelzo/ros_pointcloud2?tab=readme-ov-file#rclrs-ros2_rust). //! - derive — Offers implementations for the [`PointConvertible`] trait needed for custom points. //! - rayon — Parallel iterator support for `_par_iter` functions. diff --git a/ros_pointcloud2/src/ros.rs b/ros_pointcloud2/src/ros.rs index 3cffda2..17f740e 100644 --- a/ros_pointcloud2/src/ros.rs +++ b/ros_pointcloud2/src/ros.rs @@ -26,7 +26,13 @@ use alloc::string::String; #[cfg(feature = "roslibrust_msg")] -roslibrust_codegen_macro::find_and_generate_ros_messages!(); +use roslibrust::codegen::integral_types::Time as RosTime; + +#[cfg(feature = "roslibrust_msg")] +use roslibrust_util::sensor_msgs; + +#[cfg(feature = "roslibrust_msg")] +use roslibrust_util::std_msgs; /// [Time](https://docs.ros2.org/latest/api/builtin_interfaces/msg/Time.html) representation for ROS messages. #[derive(Clone, Debug, Default)] @@ -36,21 +42,11 @@ pub struct TimeMsg { } #[cfg(feature = "roslibrust_msg")] -impl From for TimeMsg { - fn from(time: roslibrust_codegen::Time) -> Self { +impl From for TimeMsg { + fn from(time: RosTime) -> Self { Self { sec: time.secs as i32, - nanosec: time.nsecs, - } - } -} - -#[cfg(feature = "rosrust_msg")] -impl From for TimeMsg { - fn from(time: rosrust::Time) -> Self { - Self { - sec: time.sec as i32, - nanosec: time.nsec, + nanosec: time.nsecs as u32, } } } @@ -172,7 +168,7 @@ impl From for crate::PointCloud2Msg { seq: msg.header.seq, stamp: TimeMsg { sec: msg.header.stamp.secs as i32, - nanosec: msg.header.stamp.nsecs, + nanosec: msg.header.stamp.nsecs as u32, }, frame_id: msg.header.frame_id, }, @@ -213,9 +209,9 @@ impl From for sensor_msgs::PointCloud2 { sensor_msgs::PointCloud2 { header: std_msgs::Header { seq: msg.header.seq, - stamp: roslibrust_codegen::Time { - secs: msg.header.stamp.sec as u32, - nsecs: msg.header.stamp.nanosec, + stamp: RosTime { + secs: msg.header.stamp.sec, + nsecs: msg.header.stamp.nanosec as i32, }, frame_id: msg.header.frame_id, }, @@ -247,87 +243,3 @@ impl From for sensor_msgs::PointCloud2 { } } } - -#[cfg(feature = "rosrust_msg")] -impl From for crate::PointCloud2Msg { - fn from(msg: rosrust_msg::sensor_msgs::PointCloud2) -> Self { - Self { - header: HeaderMsg { - seq: msg.header.seq, - stamp: TimeMsg { - sec: msg.header.stamp.sec as i32, - nanosec: msg.header.stamp.nsec, - }, - frame_id: msg.header.frame_id, - }, - dimensions: crate::CloudDimensions { - width: msg.width, - height: msg.height, - }, - fields: msg - .fields - .into_iter() - .map(|field| PointFieldMsg { - name: field.name, - offset: field.offset, - datatype: field.datatype, - count: field.count, - }) - .collect(), - endian: if msg.is_bigendian { - crate::Endian::Big - } else { - crate::Endian::Little - }, - point_step: msg.point_step, - row_step: msg.row_step, - data: msg.data, - dense: if msg.is_dense { - crate::Denseness::Dense - } else { - crate::Denseness::Sparse - }, - } - } -} - -#[cfg(feature = "rosrust_msg")] -impl From for rosrust_msg::sensor_msgs::PointCloud2 { - fn from(msg: crate::PointCloud2Msg) -> Self { - rosrust_msg::sensor_msgs::PointCloud2 { - header: rosrust_msg::std_msgs::Header { - seq: msg.header.seq, - stamp: rosrust::Time { - sec: msg.header.stamp.sec as u32, - nsec: msg.header.stamp.nanosec, - }, - frame_id: msg.header.frame_id, - }, - height: msg.dimensions.height, - width: msg.dimensions.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: if msg.endian == crate::Endian::Big { - true - } else { - false - }, - point_step: msg.point_step, - row_step: msg.row_step, - data: msg.data, - is_dense: if msg.dense == crate::Denseness::Dense { - true - } else { - false - }, - } - } -} diff --git a/ros_pointcloud2/tests/rosrust_msg_test.rs b/ros_pointcloud2/tests/rosrust_msg_test.rs index 2e7c795..be5514a 100644 --- a/ros_pointcloud2/tests/rosrust_msg_test.rs +++ b/ros_pointcloud2/tests/rosrust_msg_test.rs @@ -1,6 +1,6 @@ -#[cfg(feature = "rosrust_msg")] +#[cfg(feature = "roslibrust_msg")] #[test] -fn convertxyz_rosrust_msg() { +fn convertxyz_roslibrust_msg() { use ros_pointcloud2::{points::PointXYZ, PointCloud2Msg}; let cloud = vec![ @@ -22,8 +22,8 @@ fn convertxyz_rosrust_msg() { ]; let copy = cloud.clone(); let internal_cloud = PointCloud2Msg::try_from_iter(cloud).unwrap(); - let rosrust_msg_cloud: rosrust_msg::sensor_msgs::PointCloud2 = internal_cloud.into(); - let convert_back_internal: PointCloud2Msg = rosrust_msg_cloud.into(); + let roslibrust_msg_cloud: roslibrust_util::sensor_msgs::PointCloud2 = internal_cloud.into(); + let convert_back_internal: PointCloud2Msg = roslibrust_msg_cloud.into(); let to_convert = convert_back_internal.try_into_iter().unwrap(); let back_to_type = to_convert.collect::>(); assert_eq!(copy, back_to_type);