From 04d12c8bfcff3b25f99a7091128ed09715d942b1 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Fri, 7 Apr 2023 15:07:38 +0900 Subject: [PATCH] feat(pointcloud_preprocessor): add data layout check utils When input cloud data layout is compatible filters can copy data faster. Signed-off-by: Vincent Richard --- .../utility/utilities.hpp | 6 ++ .../src/utility/utilities.cpp | 77 +++++++++++++++++++ 2 files changed, 83 insertions(+) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp index 982b6210eca7a..ca98163f28a9f 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp @@ -78,6 +78,12 @@ void remove_polygon_cgal_from_cloud( bool point_within_cgal_polys( const pcl::PointXYZ & point, const std::vector & polyline_polygons); +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */ +bool is_data_layout_compatible_with_PointXYZI(const sensor_msgs::msg::PointCloud2 & input); + +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRADRT. That is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */ +bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::PointCloud2 & input); + } // namespace pointcloud_preprocessor::utils #endif // POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp index fe39ccbcccddc..08be2c5131ac1 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp @@ -14,6 +14,8 @@ #include "pointcloud_preprocessor/utility/utilities.hpp" +#include + namespace pointcloud_preprocessor::utils { void to_cgal_polygon(const geometry_msgs::msg::Polygon & polygon_in, PolygonCgal & polygon_out) @@ -147,4 +149,79 @@ bool point_within_cgal_polys( return false; } + +bool is_data_layout_compatible_with_PointXYZI(const sensor_msgs::msg::PointCloud2 & input) +{ + using autoware_point_types::PointXYZI; + using autoware_point_types::PointIndex; + if (input.fields.size() < 4) { + return false; + } + bool same_layout = true; + const auto &field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= offsetof(PointXYZI, x) == field_x.offset; + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto& field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= offsetof(PointXYZI, y) == field_y.offset; + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto& field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= offsetof(PointXYZI, z) == field_z.offset; + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto& field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= offsetof(PointXYZI, intensity) == field_intensity.offset; + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_intensity.count == 1; + return same_layout; +} + +bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::PointCloud2 & input) +{ + using autoware_point_types::PointXYZIRADRT; + using autoware_point_types::PointIndex; + if (input.fields.size() < 9) { + return false; + } + bool same_layout = true; + const auto &field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= offsetof(PointXYZIRADRT, x) == field_x.offset; + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto& field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= offsetof(PointXYZIRADRT, y) == field_y.offset; + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto& field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= offsetof(PointXYZIRADRT, z) == field_z.offset; + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto& field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= offsetof(PointXYZIRADRT, intensity) == field_intensity.offset; + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_intensity.count == 1; + const auto& field_ring = input.fields.at(static_cast(PointIndex::Ring)); + same_layout &= offsetof(PointXYZIRADRT, ring) == field_ring.offset; + same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; + same_layout &= field_ring.count == 1; + const auto& field_azimuth = input.fields.at(static_cast(PointIndex::Azimuth)); + same_layout &= offsetof(PointXYZIRADRT, azimuth) == field_azimuth.offset; + same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_azimuth.count == 1; + const auto& field_distance = input.fields.at(static_cast(PointIndex::Distance)); + same_layout &= offsetof(PointXYZIRADRT, distance) == field_distance.offset; + same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_distance.count == 1; + const auto& field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + same_layout &= offsetof(PointXYZIRADRT, return_type) == field_return_type.offset; + same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_return_type.count == 1; + const auto& field_time_stamp = input.fields.at(static_cast(PointIndex::TimeStamp)); + same_layout &= offsetof(PointXYZIRADRT, time_stamp) == field_time_stamp.offset; + same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::FLOAT64; + same_layout &= field_time_stamp.count == 1; + return same_layout; +} + } // namespace pointcloud_preprocessor::utils