Merge 47b09c60af into dbe88e97d0

.gitattributes

@@ -0,0 +1 @@
+rpcl2/tests/*.bash linguist-vendored

.github/workflows/roslibrust_noetic.yml

@@ -0,0 +1,108 @@
+name: roslibrust_noetic
+
+on:
+  push:
+    branches-ignore:
+      - rclrs
+  pull_request:
+    branches-ignore:
+      - rclrs
+  workflow_dispatch:
+
+jobs:
+  build:
+    runs-on: ubuntu-20.04
+    env:
+      ROS_CI_DESKTOP: "`lsb_release -cs`" # e.g. [trusty|xenial|...]
+      # CI_SOURCE_PATH: $(pwd)
+      ROSINSTALL_FILE: $CI_SOURCE_PATH/dependencies.rosinstall
+      CATKIN_OPTIONS: $CI_SOURCE_PATH/catkin.options
+      ROS_PARALLEL_JOBS: "-j8 -l6"
+      # Set the python path manually to include /usr/-/python2.7/dist-packages
+      # as this is where apt-get installs python packages.
+      PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages
+      ROS_DISTRO: noetic
+    steps:
+      - name: roslibrust
+        uses: actions/checkout@v1
+      - name: Install latest rust
+        run: |
+          curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | sh -s -- -y
+          rustc --version
+          cargo --version
+      - name: Configure ROS for install
+        run: |
+          sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
+          sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
+          sudo apt-get update -qq
+          sudo apt-get install dpkg
+          sudo apt-get install -y libyaml-cpp-dev
+      - name: Install ROS basic packages
+        run: |
+          sudo apt-get install -y python3-catkin-pkg
+          sudo apt-get install -y python3-catkin-tools
+          sudo apt-get install -y python3-rosdep
+          sudo apt-get install -y python3-wstool
+          sudo apt-get install -y python3-osrf-pycommon
+          sudo apt-get install -y ros-cmake-modules
+          sudo apt-get install -y ros-$ROS_DISTRO-ros-base
+          source /opt/ros/$ROS_DISTRO/setup.bash
+          sudo rosdep init
+          rosdep update  # --include-eol-distros  # Support EOL distros.
+      - name: Install ROS additional packages
+        # Does installing these mean roslibrust_msg builds more messages, which is a better
+        # check?  Or does it just slow down the action?
+        run: |
+          sudo apt-get install -y ros-$ROS_DISTRO-actionlib
+          sudo apt-get install -y ros-$ROS_DISTRO-actionlib-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-camera-info-manager
+          sudo apt-get install -y ros-$ROS_DISTRO-compressed-image-transport
+          sudo apt-get install -y ros-$ROS_DISTRO-catkin
+          sudo apt-get install -y ros-$ROS_DISTRO-class-loader
+          sudo apt-get install -y ros-$ROS_DISTRO-cmake-modules
+          sudo apt-get install -y ros-$ROS_DISTRO-cv-bridge
+          sudo apt-get install -y ros-$ROS_DISTRO-dynamic-reconfigure
+          sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure
+          # Not in noetic yet
+          # sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure-python
+          sudo apt-get install -y ros-$ROS_DISTRO-eigen-conversions
+          sudo apt-get install -y ros-$ROS_DISTRO-geometry-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-genmsg
+          sudo apt-get install -y ros-$ROS_DISTRO-image-geometry
+          sudo apt-get install -y ros-$ROS_DISTRO-image-proc
+          sudo apt-get install -y ros-$ROS_DISTRO-image-transport
+          sudo apt-get install -y ros-$ROS_DISTRO-message-generation
+          sudo apt-get install -y ros-$ROS_DISTRO-message-runtime
+          # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-core
+          # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-topic-tools
+          # sudo apt-get install -y ros-$ROS_DISTRO-pcl-conversions
+          # sudo apt-get install -y ros-$ROS_DISTRO-pcl-ros
+          sudo apt-get install -y ros-$ROS_DISTRO-pluginlib
+          sudo apt-get install -y ros-$ROS_DISTRO-roscpp
+          sudo apt-get install -y ros-$ROS_DISTRO-roslib
+          sudo apt-get install -y ros-$ROS_DISTRO-roslint
+          sudo apt-get install -y ros-$ROS_DISTRO-rospy
+          sudo apt-get install -y ros-$ROS_DISTRO-rospy-message-converter
+          sudo apt-get install -y ros-$ROS_DISTRO-rostest
+          sudo apt-get install -y ros-$ROS_DISTRO-sensor-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-std-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-tf
+          sudo apt-get install -y ros-$ROS_DISTRO-tf-conversions
+          sudo apt-get install -y ros-$ROS_DISTRO-tf2-geometry-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-tf2-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-tf2-py
+          sudo apt-get install -y ros-$ROS_DISTRO-tf2-ros
+          sudo apt-get install -y ros-$ROS_DISTRO-tf2-sensor-msgs
+      - name: build
+        run: |
+          source /opt/ros/$ROS_DISTRO/setup.bash
+          ROS_PACKAGE_PATH=`rospack find geometry_msgs`:`rospack find tf2_msgs`:`rospack find sensor_msgs`:`rospack find std_msgs`:`rospack find actionlib_msgs` cargo build  # --verbose
+      - name: Install ROS packages for tests
+        run: |
+          sudo apt-get install -y ros-$ROS_DISTRO-actionlib-tutorials
+          sudo apt-get install -y ros-$ROS_DISTRO-roscpp-tutorials
+          sudo apt-get install -y ros-$ROS_DISTRO-rospy-tutorials
+      - name: test
+        run: |
+          source /opt/ros/$ROS_DISTRO/setup.bash
+          ROS_PACKAGE_PATH=`rospack find geometry_msgs`:`rospack find tf2_msgs`:`rospack find sensor_msgs`:`rospack find std_msgs`:`rospack find actionlib_msgs` cargo test --features roslibrust_msg,derive,nalgebra,rayon

CHANGELOG.md

@@ -1,5 +1,9 @@
 # Changelog
 
+## v0.5.0 -> v0.5.1
+
+- Fixes a bug, where the conversion of larger to smaller types results in a false buffer interpretation.
+
 ## v0.5.0-rc.3 -> v0.5.0
 
 - `PointConvertible` trait is now `unsafe` since the offset is used for raw memory access, where safety can not be guaranteed by the compiler.

README.md

@@ -7,7 +7,7 @@
 
 ros_pointcloud2 uses its own type for the message `PointCloud2Msg` to keep the library framework agnostic. ROS1 and ROS2 are supported with feature flags.
 
-Get started with the example below, check out the other use cases in the `examples` folder or see the [Documentation](https://docs.rs/ros_pointcloud2/0.5.0/) for a complete guide.
+Get started with the example below, check out the other use cases in the `examples` folder or see the [Documentation](https://docs.rs/ros_pointcloud2/0.5.1/) for a complete guide.
 
 ## Quickstart
 
@@ -70,7 +70,7 @@ Features do not work properly with `rcrls` because the messages are linked exter
 
 ```toml
 [dependencies]
-ros_pointcloud2 = { git = "https://github.com/stelzo/ros_pointcloud2", tag = "v0.5.0_rclrs" }
+ros_pointcloud2 = { git = "https://github.com/stelzo/ros_pointcloud2", tag = "v0.5.1_rclrs" }
 ```
 
 Also, indicate the following dependencies to your linker inside the `package.xml` of your package.

rpcl2/Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "ros_pointcloud2"
-version = "0.5.0"
+version = "0.5.1"
 edition = "2021"
 authors = ["Christopher Sieh <stelzo@steado.de>"]
 description = "Customizable conversions for working with sensor_msgs/PointCloud2."
@@ -39,6 +39,19 @@ nalgebra = { version = "0.33", optional = true, default-features = false }
 rpcl2-derive = { version = "0.3", optional = true, path = "../rpcl2-derive" }
 memoffset = { version = "0.9", optional = true }
 
+[dependencies.roslibrust]
+version = "0.10.2"
+optional = true
+features = ["ros1"]
+
+[dependencies.roslibrust_codegen]
+version = "0.10"
+optional = true
+
+[dependencies.roslibrust_codegen_macro]
+version = "0.10"
+optional = true
+
 [dev-dependencies]
 rand = "0.8"
 criterion = { version = "0.5", features = ["html_reports"] }
@@ -50,6 +63,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"]
 r2r_msg = ["dep:r2r"]
 rayon = ["dep:rayon"]

rpcl2/src/iterator.rs

@@ -260,8 +260,6 @@ where
     /// The theoretical time complexity is O(n) where n is the number of fields defined in the message for a single point which is typically small.
     /// It therefore has a constant time complexity O(1) for practical purposes.
     fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> {
-        let mut pdata_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N];
-
         let fields_only = crate::ordered_field_names::<N, C>();
 
         let not_found_fieldnames = fields_only
@@ -281,13 +279,14 @@ where
             return Err(MsgConversionError::FieldsNotFound(names_not_found));
         }
 
-        for (field, with_offset) in cloud.fields.iter().zip(pdata_with_offsets.iter_mut()) {
+        let mut pdata_with_offsets = Vec::with_capacity(N);
+        for field in cloud.fields.iter() {
             if fields_only.contains(&field.name) {
-                *with_offset = (
+                pdata_with_offsets.push((
                     field.name.clone(),
                     field.datatype.try_into()?,
                     field.offset as usize,
-                );
+                ));
             }
         }
 

rpcl2/src/lib.rs

@@ -32,9 +32,9 @@
 //!     PointXYZI::new(46.0, 5.47, 0.5, 0.1),
 //! ];
 //! let cloud_copy = cloud_points.clone(); // For equality test later.
-//!     
+//!
 //! let out_msg = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
-//!     
+//!
 //! // Convert to your ROS crate message type.
 //! // let msg: r2r::sensor_msgs::msg::PointCloud2 = in_msg.into();
 //! // Publish ...
@@ -42,7 +42,7 @@
 //! // ... now incoming from a topic.
 //! // let in_msg: PointCloud2Msg = msg.into();
 //! let in_msg = out_msg;
-//!     
+//!
 //! let processed_cloud = in_msg.try_into_iter().unwrap()
 //!     .map(|point: PointXYZ| { // Define the data you want from the point.
 //!         // Some logic here.
@@ -127,7 +127,7 @@
 //! ```
 #![crate_type = "lib"]
 #![cfg_attr(docsrs, feature(doc_cfg))]
-#![doc(html_root_url = "https://docs.rs/ros_pointcloud2/0.5.0")]
+#![doc(html_root_url = "https://docs.rs/ros_pointcloud2/0.5.1")]
 #![warn(clippy::print_stderr)]
 #![warn(clippy::print_stdout)]
 #![warn(clippy::unwrap_used)]
@@ -312,6 +312,7 @@ pub enum Denseness {
     Sparse,
 }
 
+#[derive(Clone, Debug, PartialEq)]
 enum ByteSimilarity {
     Equal,
     Overlapping,
@@ -501,7 +502,7 @@ impl PointCloud2Msg {
         let target_layout = KnownLayoutInfo::try_from(C::layout())?;
 
         debug_assert!(field_names.len() <= target_layout.fields.len());
-        debug_assert!(self.fields.len() <= target_layout.fields.len());
+        debug_assert!(self.fields.len() >= target_layout.fields.len());
 
         let mut offset: u32 = 0;
         let mut field_counter = 0;
@@ -1460,3 +1461,216 @@ impl FromBytes for u8 {
         Self::from_le_bytes([bytes[0]])
     }
 }
+
+mod test {
+    #![allow(clippy::unwrap_used)]
+
+    use crate::prelude::*;
+
+    #[derive(Debug, Default, Clone, PartialEq)]
+    #[repr(C)]
+    struct PointA {
+        x: f32,
+        y: f32,
+        z: f32,
+        intensity: f32,
+        t: u32,
+        reflectivity: u16,
+        ring: u16,
+        ambient: u16,
+        range: u32,
+    }
+
+    impl From<RPCL2Point<9>> for PointA {
+        fn from(point: RPCL2Point<9>) -> Self {
+            Self::new(point[0].get(), point[1].get(), point[2].get())
+        }
+    }
+
+    impl From<PointA> for RPCL2Point<9> {
+        fn from(point: PointA) -> Self {
+            [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                point.intensity.into(),
+                point.t.into(),
+                point.reflectivity.into(),
+                point.ring.into(),
+                point.ambient.into(),
+                point.range.into(),
+            ]
+            .into()
+        }
+    }
+
+    unsafe impl PointConvertible<9> for PointA {
+        fn layout() -> LayoutDescription {
+            LayoutDescription::new(&[
+                LayoutField::new("x", "f32", 4),
+                LayoutField::new("y", "f32", 4),
+                LayoutField::new("z", "f32", 4),
+                LayoutField::new("intensity", "f32", 4),
+                LayoutField::new("t", "u32", 4),
+                LayoutField::new("reflectivity", "u16", 2),
+                LayoutField::padding(2),
+                LayoutField::new("ring", "u16", 2),
+                LayoutField::padding(2),
+                LayoutField::new("ambient", "u16", 2),
+                LayoutField::padding(2),
+                LayoutField::new("range", "u32", 4),
+            ])
+        }
+    }
+
+    impl PointA {
+        fn new(x: f32, y: f32, z: f32) -> Self {
+            Self {
+                x,
+                y,
+                z,
+                intensity: 0.0,
+                t: 0,
+                reflectivity: 0,
+                ring: 0,
+                ambient: 0,
+                range: 0,
+            }
+        }
+    }
+
+    #[derive(Debug, Clone, Default, PartialEq)]
+    #[repr(C)]
+    struct PointB {
+        pub x: f32,
+        pub y: f32,
+        pub z: f32,
+        pub t: u32,
+    }
+
+    impl PointB {
+        fn new(x: f32, y: f32, z: f32) -> Self {
+            Self { x, y, z, t: 0 }
+        }
+    }
+
+    impl From<RPCL2Point<4>> for PointB {
+        fn from(point: RPCL2Point<4>) -> Self {
+            Self::new(point[0].get(), point[1].get(), point[2].get())
+        }
+    }
+
+    impl From<PointB> for RPCL2Point<4> {
+        fn from(point: PointB) -> Self {
+            [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                point.t.into(),
+            ]
+            .into()
+        }
+    }
+
+    unsafe impl PointConvertible<4> for PointB {
+        fn layout() -> LayoutDescription {
+            LayoutDescription::new(&[
+                LayoutField::new("x", "f32", 4),
+                LayoutField::new("y", "f32", 4),
+                LayoutField::new("z", "f32", 4),
+                LayoutField::new("t", "u32", 4),
+            ])
+        }
+    }
+
+    #[derive(Debug, Clone, Default, PartialEq)]
+    #[repr(C)]
+    struct PointD {
+        x: f32,
+        y: f32,
+        z: f32,
+        t: u32,
+        ring: u16,
+        range: u32,
+        signal: u16,
+        reflectivity: u16,
+        near_ir: u16,
+    }
+
+    impl From<RPCL2Point<9>> for PointD {
+        fn from(point: RPCL2Point<9>) -> Self {
+            Self::new(point[0].get(), point[1].get(), point[2].get())
+        }
+    }
+
+    impl From<PointD> for RPCL2Point<9> {
+        fn from(point: PointD) -> Self {
+            [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                point.t.into(),
+                point.ring.into(),
+                point.range.into(),
+                point.signal.into(),
+                point.reflectivity.into(),
+                point.near_ir.into(),
+            ]
+            .into()
+        }
+    }
+
+    unsafe impl PointConvertible<9> for PointD {
+        fn layout() -> LayoutDescription {
+            LayoutDescription::new(&[
+                LayoutField::new("x", "f32", 4),
+                LayoutField::new("y", "f32", 4),
+                LayoutField::new("z", "f32", 4),
+                LayoutField::new("t", "u32", 4),
+                LayoutField::new("ring", "u16", 2),
+                LayoutField::padding(2),
+                LayoutField::new("range", "u32", 4),
+                LayoutField::new("signal", "u16", 2),
+                LayoutField::padding(2),
+                LayoutField::new("reflectivity", "u16", 2),
+                LayoutField::padding(2),
+                LayoutField::new("near_ir", "u16", 2),
+                LayoutField::padding(2),
+            ])
+        }
+    }
+
+    impl PointD {
+        fn new(x: f32, y: f32, z: f32) -> Self {
+            Self {
+                x,
+                y,
+                z,
+                t: 0,
+                ring: 0,
+                range: 0,
+                signal: 0,
+                reflectivity: 0,
+                near_ir: 0,
+            }
+        }
+    }
+
+    #[test]
+    fn subtype_iterator_fallback() {
+        let cloud_a = PointCloud2Msg::try_from_iter(vec![
+            PointA::new(1.0, 2.0, 3.0),
+            PointA::new(4.0, 5.0, 6.0),
+            PointA::new(7.0, 8.0, 9.0),
+        ])
+        .unwrap();
+
+        let cloud_c: PointB = cloud_a.clone().try_into_iter().unwrap().next().unwrap();
+        assert_eq!(cloud_c, PointB::new(1.0, 2.0, 3.0));
+
+        let cloud_b: Vec<PointB> = cloud_a.try_into_vec().unwrap();
+        assert_eq!(cloud_b[0], PointB::new(1.0, 2.0, 3.0));
+        assert_eq!(cloud_b[1], PointB::new(4.0, 5.0, 6.0));
+        assert_eq!(cloud_b[2], PointB::new(7.0, 8.0, 9.0));
+    }
+}

rpcl2/src/ros.rs

@@ -25,6 +25,9 @@
 
 use alloc::string::String;
 
+#[cfg(feature = "roslibrust_msg")]
+roslibrust_codegen_macro::find_and_generate_ros_messages!();
+
 /// [Time](https://docs.ros2.org/latest/api/builtin_interfaces/msg/Time.html) representation for ROS messages.
 #[derive(Clone, Debug, Default)]
 pub struct TimeMsg {
@@ -32,6 +35,16 @@ pub struct TimeMsg {
     pub nanosec: u32,
 }
 
+#[cfg(feature = "roslibrust_msg")]
+impl From<roslibrust_codegen::Time> for TimeMsg {
+    fn from(time: roslibrust_codegen::Time) -> Self {
+        Self {
+            sec: time.secs as i32,
+            nanosec: time.nsecs,
+        }
+    }
+}
+
 #[cfg(feature = "rosrust_msg")]
 impl From<rosrust::Time> for TimeMsg {
     fn from(time: rosrust::Time) -> Self {
@@ -151,6 +164,90 @@ impl From<crate::PointCloud2Msg> for r2r::sensor_msgs::msg::PointCloud2 {
     }
 }
 
+#[cfg(feature = "roslibrust_msg")]
+impl From<sensor_msgs::PointCloud2> for crate::PointCloud2Msg {
+    fn from(msg: sensor_msgs::PointCloud2) -> Self {
+        Self {
+            header: HeaderMsg {
+                seq: msg.header.seq,
+                stamp: TimeMsg {
+                    sec: msg.header.stamp.secs as i32,
+                    nanosec: msg.header.stamp.nsecs,
+                },
+                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 = "roslibrust_msg")]
+impl From<crate::PointCloud2Msg> for sensor_msgs::PointCloud2 {
+    fn from(msg: crate::PointCloud2Msg) -> Self {
+        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,
+                },
+                frame_id: msg.header.frame_id,
+            },
+            height: msg.dimensions.height,
+            width: msg.dimensions.width,
+            fields: msg
+                .fields
+                .into_iter()
+                .map(|field| 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
+            },
+        }
+    }
+}
+
 #[cfg(feature = "rosrust_msg")]
 impl From<rosrust_msg::sensor_msgs::PointCloud2> for crate::PointCloud2Msg {
     fn from(msg: rosrust_msg::sensor_msgs::PointCloud2) -> Self {