fix(hesai): workaround for PCL bug to fix pointcloud size fields when…

… filters are enabled

Signed-off-by: Max SCHMELLER <[email protected]>

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp

@@ -195,7 +195,7 @@ class HesaiDecoder : public HesaiScanDecoder
         auto scan_timestamp_ns =
           in_current_scan ? decode_scan_timestamp_ns_ : output_scan_timestamp_ns_;
 
-        NebulaPoint & point = pc->emplace_back();
+        NebulaPoint point;
         point.distance = distance;
         point.intensity = unit.reflectivity;
         point.time_stamp = getPointTimeRelative(
@@ -214,12 +214,17 @@ class HesaiDecoder : public HesaiScanDecoder
         // The driver wrapper converts to degrees, expects radians
         point.azimuth = corrected_angle_data.azimuth_rad;
         point.elevation = corrected_angle_data.elevation_rad;
-        for (const auto & filter : sensor_configuration_->point_filters) {
-          if (filter->excluded(point)) {
-            pc->points.pop_back();
-            break;
-          }
+
+        const auto & filters = sensor_configuration_->point_filters;
+        bool excluded = std::any_of(filters.begin(), filters.end(), [&](const auto & filter) {
+          return filter->excluded(point);
+        });
+
+        if (excluded) {
+          continue;
         }
+
+        pc->emplace_back(point);
       }
     }
   }