feat(pointcloud_preprocessor): improve cloud validation (#11853)

Signed-off-by: Mete Fatih Cırıt <mfc@autoware.org>

Author: xmfcx

perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp

@@ -106,7 +106,7 @@ void VoxelBasedCompareMapFilterComponent::checkStatus(
 void VoxelBasedCompareMapFilterComponent::input_indices_callback(
   const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud, const PointIndicesConstPtr indices)
 {
-  if (!is_valid(cloud)) {
+  if (!is_valid(cloud, this->get_logger(), *this->get_clock())) {
     RCLCPP_ERROR(this->get_logger(), "[input_indices_callback] Invalid input!");
     return;
   }

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp

@@ -53,6 +53,7 @@
 #define AUTOWARE__POINTCLOUD_PREPROCESSOR__FILTER_HPP_
 
 #include "autoware/pointcloud_preprocessor/transform_info.hpp"
+#include "autoware/pointcloud_preprocessor/utility/memory.hpp"
 
 #include <boost/thread/mutex.hpp>
 
@@ -244,17 +245,141 @@ class Filter : public rclcpp::Node
 
   std::unique_ptr<managed_transform_buffer::ManagedTransformBuffer> managed_tf_buffer_{nullptr};
 
-  inline bool is_valid(const PointCloud2ConstPtr & cloud)
+  /**
+   * @brief Validate a sensor_msgs::msg::PointCloud2 message for structural consistency and layout.
+   *
+   * This function performs a series of lightweight sanity checks to determine whether a
+   * PointCloud2 message is safe to consume by algorithms expecting an XYZ-compatible,
+   * organized (dense) point cloud.
+   *
+   * Validation errors and warnings are reported using throttled ROS logging to avoid
+   * excessive log spam when invalid data is repeatedly received.
+   *
+   * @details
+   * The following checks are performed in order:
+   *
+   * 1. **Null pointer check**
+   *    - Ensures the input PointCloud2 pointer is valid.
+   *
+   * 2. **Point step validation**
+   *    - Verifies that `point_step` is non-zero.
+   *
+   * 3. **Dense (organized) cloud requirement**
+   *    - Warns if `is_dense` is false.
+   *    - Currently does *not* fail validation, but will become a hard error
+   *      starting **July 2026**.
+   *
+   * 4. **Point field layout compatibility**
+   *    - Ensures the point data layout is compatible with `PointXYZ`
+   *      (fields must begin with `x`, `y`, `z` of type `FLOAT32`).
+   *
+   * 5. **Row step consistency**
+   *    - Validates that `row_step == width * point_step`.
+   *    - Currently logs a warning on mismatch.
+   *    - Will become a hard error starting **July 2026**.
+   *
+   * 6. **Data buffer size consistency**
+   *    - Ensures `data.size() == height * row_step`.
+   *
+   * @param cloud
+   *   Shared pointer to the input PointCloud2 message to validate.
+   *
+   * @param logger
+   *   ROS 2 logger used for emitting validation warnings.
+   *
+   * @param clock
+   *   ROS 2 clock used for throttled logging.
+   *
+   * @return true
+   *   If the point cloud passes all mandatory validation checks.
+   *
+   * @return false
+   *   If a critical validation failure is detected (e.g., null pointer,
+   *   incompatible data layout, or inconsistent buffer size).
+   *
+   * @note
+   * - All warnings are throttled with a fixed duration of 5000 ms.
+   * - Some warnings are marked as *future errors* and are expected to
+   *   become fatal after July 2026.
+   *
+   * @warning
+   * This function assumes consumers expect an XYZ-only point layout.
+   * Additional fields (e.g., intensity, color) are allowed only if they
+   * appear *after* the XYZ fields and do not alter the layout compatibility.
+   */
+  static bool is_valid(
+    const PointCloud2ConstPtr & cloud, const rclcpp::Logger & logger, rclcpp::Clock & clock)
   {
-    if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) {
-      RCLCPP_WARN(
-        this->get_logger(),
-        "Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, "
-        "and frame %s received!",
-        cloud->data.size(), cloud->width, cloud->height, cloud->point_step,
-        rclcpp::Time(cloud->header.stamp).seconds(), cloud->header.frame_id.c_str());
+    static constexpr rcutils_duration_value_t throttle_duration_ms = 5000;
+
+    if (!cloud) {
+      RCLCPP_WARN_THROTTLE(
+        logger, clock, throttle_duration_ms, "Invalid PointCloud: Null pointer received.");
+      return false;
+    }
+
+    // Check: Point Step
+    if (cloud->point_step == 0) {
+      RCLCPP_WARN_THROTTLE(
+        logger, clock, throttle_duration_ms,
+        "Invalid PointCloud: point_step is 0. "
+        "Frame: '%s', Stamp: %d.%09u",
+        cloud->header.frame_id.c_str(), cloud->header.stamp.sec, cloud->header.stamp.nanosec);
+      return false;
+    }
+
+    // Check: Only accept organized (dense) point clouds
+    if (!cloud->is_dense) {
+      RCLCPP_WARN_THROTTLE(
+        logger, clock, throttle_duration_ms,
+        "Invalid PointCloud: is_dense is false. "
+        "The point cloud should be organized (dense) and contain only valid points. "
+        "This will be an ERROR starting in 2026 July"
+        "Frame: '%s', Stamp: %d.%09u",
+        cloud->header.frame_id.c_str(), cloud->header.stamp.sec, cloud->header.stamp.nanosec);
+      // TODO(mfc): return false; After 2026 July
+    }
+
+    // Check: Point field layout compatibility
+    if (!utils::is_data_layout_compatible_with_point_xyz(*cloud)) {
+      RCLCPP_WARN_THROTTLE(
+        logger, clock, throttle_duration_ms,
+        "The pointcloud layout is not compatible with PointXYZ. Aborting. "
+        "Make sure Point fields start with x, y, z as FLOAT32.");
+      return false;
+    }
+
+    // Check: Row step consistency
+    size_t expected_row_step =
+      static_cast<size_t>(cloud->width) * static_cast<size_t>(cloud->point_step);
+
+    if (expected_row_step != static_cast<size_t>(cloud->row_step)) {
+      RCLCPP_WARN_THROTTLE(
+        logger, clock, throttle_duration_ms,
+        "Invalid PointCloud: row_step mismatch. "
+        "Expected: %zu (width %u * point_step %u), Got: %u. "
+        "Frame: '%s', Stamp: %d.%09u"
+        " Please fill in the `cloud->row_step` field accordingly."
+        " This will be an ERROR starting in 2026 July",
+        expected_row_step, cloud->width, cloud->point_step, cloud->row_step,
+        cloud->header.frame_id.c_str(), cloud->header.stamp.sec, cloud->header.stamp.nanosec);
+      // TODO(mfc): return false; After 2026 July
+    }
+
+    // Check: Data buffer size consistency
+    size_t expected_data_size = static_cast<size_t>(cloud->height) * expected_row_step;
+
+    if (expected_data_size != cloud->data.size()) {
+      RCLCPP_WARN_THROTTLE(
+        logger, clock, throttle_duration_ms,
+        "Invalid PointCloud: data size mismatch. "
+        "Expected: %zu (height %u * row_step %u), Got: %zu. "
+        "Frame: '%s', Stamp: %d.%09u",
+        expected_data_size, cloud->height, cloud->row_step, cloud->data.size(),
+        cloud->header.frame_id.c_str(), cloud->header.stamp.sec, cloud->header.stamp.nanosec);
       return false;
     }
+
     return true;
   }
 

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/memory.hpp

@@ -57,6 +57,40 @@ bool is_data_layout_compatible_with_point_xyzircaedt(
  * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRCAEDT */
 bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::PointCloud2 & input);
 
+/**
+ * @brief Check whether a PointField sequence is compatible with a PointXYZ memory layout.
+ *
+ * This function verifies that the first three fields describe a tightly packed
+ * PointXYZ prefix:
+ *   - Field 0: "x", FLOAT32, count = 1, offset = offsetof(PointXYZ, x)
+ *   - Field 1: "y", FLOAT32, count = 1, offset = offsetof(PointXYZ, y)
+ *   - Field 2: "z", FLOAT32, count = 1, offset = offsetof(PointXYZ, z)
+ *
+ * Additional fields after X, Y, and Z are permitted and are not inspected.
+ *
+ * @param fields Vector of PointField descriptors from a PointCloud2 message.
+ * @return true if the layout begins with a compatible PointXYZ prefix; false otherwise.
+ */
+bool is_data_layout_compatible_with_point_xyz(
+  const std::vector<sensor_msgs::msg::PointField> & fields);
+
+/**
+ * @brief Check whether a PointCloud2 message is compatible with a PointXYZ memory layout.
+ *
+ * This is a convenience overload that inspects the PointCloud2::fields array
+ * and applies the same rules as
+ * is_data_layout_compatible_with_point_xyz(const std::vector<PointField>&).
+ *
+ * The layout is considered compatible if the first three fields correspond to
+ * a tightly packed PointXYZ (x, y, z as FLOAT32 with correct offsets). Any
+ * additional fields are allowed.
+ *
+ * @param input PointCloud2 message to inspect.
+ * @return true if the PointCloud2 layout begins with a compatible PointXYZ prefix;
+ *         false otherwise.
+ */
+bool is_data_layout_compatible_with_point_xyz(const sensor_msgs::msg::PointCloud2 & input);
+
 }  // namespace autoware::pointcloud_preprocessor::utils
 
 #endif  // AUTOWARE__POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_

sensing/autoware_pointcloud_preprocessor/src/filter.cpp

@@ -227,7 +227,7 @@ autoware::pointcloud_preprocessor::Filter::filter_param_callback(
 void autoware::pointcloud_preprocessor::Filter::input_indices_callback(
   const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices)
 {
-  if (!is_valid(cloud)) {
+  if (!is_valid(cloud, this->get_logger(), *this->get_clock())) {
     RCLCPP_ERROR(this->get_logger(), "[input_indices_callback] Invalid input!");
     return;
   }
@@ -389,7 +389,7 @@ void autoware::pointcloud_preprocessor::Filter::faster_input_indices_callback(
     return;
   }
 
-  if (!is_valid(cloud)) {
+  if (!is_valid(cloud, this->get_logger(), *this->get_clock())) {
     RCLCPP_ERROR(this->get_logger(), "[input_indices_callback] Invalid input!");
     return;
   }

sensing/autoware_pointcloud_preprocessor/src/utility/memory.cpp

@@ -234,4 +234,45 @@ bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::Poi
   return is_data_layout_compatible_with_point_xyzircaedt(input.fields);
 }
 
+bool is_data_layout_compatible_with_point_xyz(
+  const std::vector<sensor_msgs::msg::PointField> & fields)
+{
+  if (fields.size() < 3) {
+    return false;
+  }
+
+  // Expected PointXYZ layout:
+  // struct { float x; float y; float z; }
+  constexpr size_t offset_x = 0;
+  constexpr size_t offset_y = sizeof(float);
+  constexpr size_t offset_z = sizeof(float) * 2;
+
+  bool same_layout = true;
+
+  const auto & field_x = fields[0];
+  same_layout &= field_x.name == "x";
+  same_layout &= field_x.offset == offset_x;
+  same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32;
+  same_layout &= field_x.count == 1;
+
+  const auto & field_y = fields[1];
+  same_layout &= field_y.name == "y";
+  same_layout &= field_y.offset == offset_y;
+  same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32;
+  same_layout &= field_y.count == 1;
+
+  const auto & field_z = fields[2];
+  same_layout &= field_z.name == "z";
+  same_layout &= field_z.offset == offset_z;
+  same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32;
+  same_layout &= field_z.count == 1;
+
+  return same_layout;
+}
+
+bool is_data_layout_compatible_with_point_xyz(const sensor_msgs::msg::PointCloud2 & input)
+{
+  return is_data_layout_compatible_with_point_xyz(input.fields);
+}
+
 }  // namespace autoware::pointcloud_preprocessor::utils