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 <richard-v@macnica.co.jp>
autowarefoundation · Apr 8, 2023 · 04d12c8 · 04d12c8
1 parent 1ca02e1
commit 04d12c8
Show file tree

Hide file tree

Showing 2 changed files with 83 additions and 0 deletions.
diff --git 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<PolygonCgal> & 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
@@ -14,6 +14,8 @@
 
 #include "pointcloud_preprocessor/utility/utilities.hpp"
 
+#include <autoware_point_types/types.hpp>
+
 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<size_t>(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<size_t>(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<size_t>(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<size_t>(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<size_t>(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<size_t>(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<size_t>(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<size_t>(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<size_t>(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<size_t>(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<size_t>(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<size_t>(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<size_t>(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