feat(hesai): add filtered pointcloud counter function

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

@@ -22,9 +22,12 @@
 #include <nebula_common/hesai/hesai_common.hpp>
 #include <nebula_common/nebula_common.hpp>
 #include <nebula_common/point_types.hpp>
+#include <nlohmann/json.hpp>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <sys/types.h>
+
 #include <algorithm>
 #include <array>
 #include <cmath>
@@ -36,6 +39,96 @@
 namespace nebula::drivers
 {
 
+struct HesaiDecodeFilteredInfo
+{
+  uint16_t distance_filtered_count = 0;
+  uint16_t fov_filtered_count = 0;
+  uint16_t timestamp_filtered_count = 0;
+  uint16_t invalid_point_count = 0;
+  uint16_t multiple_return_point_count = 0;
+  uint16_t mutliple_return_point_count = 0;
+  uint16_t total_kept_point_count = 0;
+  uint16_t invalid_packet_count = 0;
+  float cloud_distance_min_m = 0;
+  float cloud_distance_max_m = 0;
+  float cloud_azimuth_min_rad = 0;
+  float cloud_azimuth_max_rad = 0;
+  uint64_t packet_timestamp_min_ns = 0;
+  uint64_t packet_timestamp_max_ns = 0;
+
+  void clear()
+  {
+    distance_filtered_count = 0;
+    fov_filtered_count = 0;
+    timestamp_filtered_count = 0;
+    invalid_point_count = 0;
+    multiple_return_point_count = 0;
+    mutliple_return_point_count = 0;
+    total_kept_point_count = 0;
+    invalid_packet_count = 0;
+    cloud_distance_min_m = 0;
+    cloud_distance_max_m = 0;
+    cloud_azimuth_min_rad = 0;
+    cloud_azimuth_max_rad = 0;
+    packet_timestamp_min_ns = 0;
+    packet_timestamp_max_ns = 0;
+  }
+
+  [[nodiscard]] nlohmann::ordered_json to_json() const
+  {
+    nlohmann::json distance_j;
+    distance_j["filter"] = "distance";
+    distance_j["distance_filtered_count"] = distance_filtered_count;
+    distance_j["cloud_distance_min_m"] = cloud_distance_min_m;
+    distance_j["cloud_distance_max_m"] = cloud_distance_max_m;
+    nlohmann::json fov_j;
+    fov_j["filter"] = "fov";
+    fov_j["fov_filtered_count"] = fov_filtered_count;
+    fov_j["cloud_azimuth_min_rad"] = cloud_azimuth_min_rad;
+    fov_j["cloud_azimuth_max_rad"] = cloud_azimuth_max_rad;
+    nlohmann::json timestamp_j;
+    timestamp_j["filter"] = "timestamp";
+    timestamp_j["timestamp_filtered_count"] = timestamp_filtered_count;
+    timestamp_j["packet_timestamp_min_ns"] = packet_timestamp_min_ns;
+    timestamp_j["packet_timestamp_max_ns"] = packet_timestamp_max_ns;
+    nlohmann::json invalid_j;
+    invalid_j["filter"] = "invalid";
+    invalid_j["invalid_point_count"] = invalid_point_count;
+    invalid_j["invalid_packet_count"] = invalid_packet_count;
+    nlohmann::json identical_j;
+    identical_j["filter"] = "identical";
+    identical_j["multiple_return_point_count"] = multiple_return_point_count;
+    nlohmann::json multiple_j;
+    multiple_j["filter"] = "multiple";
+    multiple_j["mutliple_return_point_count"] = mutliple_return_point_count;
+
+    nlohmann::json j;
+    j["filter_pipeline"] = nlohmann::json::array({
+      distance_j,
+      fov_j,
+      timestamp_j,
+      invalid_j,
+      identical_j,
+      multiple_j,
+    });
+    j["total_kept_point_count"] = total_kept_point_count;
+
+    return j;
+  }
+
+  void get_minmax_info(const NebulaPoint & point)
+  {
+    cloud_azimuth_min_rad = std::min(cloud_azimuth_min_rad, point.azimuth);
+    cloud_azimuth_max_rad = std::max(cloud_azimuth_max_rad, point.azimuth);
+    packet_timestamp_min_ns =
+      std::min(packet_timestamp_min_ns, static_cast<uint64_t>(point.time_stamp));
+    packet_timestamp_max_ns =
+      std::max(packet_timestamp_max_ns, static_cast<uint64_t>(point.time_stamp));
+    cloud_distance_min_m = std::min(cloud_distance_min_m, point.distance);
+    cloud_distance_max_m = std::max(cloud_distance_max_m, point.distance);
+  }
+};
+
 template <typename SensorT>
 class HesaiDecoder : public HesaiScanDecoder
 {
@@ -76,6 +169,9 @@
 
   rclcpp::Logger logger_;
 
+  // filtered pointcloud counter
+  HesaiDecodeFilteredInfo decode_filtered_info_;
+
   /// @brief For each channel, its firing offset relative to the block in nanoseconds
   std::array<int, SensorT::packet_t::n_channels> channel_firing_offset_ns_;
   /// @brief For each return mode, the firing offset of each block relative to its packet in
@@ -129,6 +225,7 @@
         auto & unit = *return_units[block_offset];
 
         if (unit.distance == 0) {
+          decode_filtered_info_.invalid_point_count++;
           continue;
         }
 
@@ -138,6 +235,7 @@
           distance < SensorT::min_range || SensorT::max_range < distance ||
           distance < sensor_configuration_->min_range ||
           sensor_configuration_->max_range < distance) {
+          decode_filtered_info_.distance_filtered_count++;
           continue;
         }
 
@@ -147,6 +245,7 @@
 
         // Keep only last of multiple identical points
         if (return_type == ReturnType::IDENTICAL && block_offset != n_blocks - 1) {
+          decode_filtered_info_.multiple_return_point_count++;
           continue;
         }
 
@@ -168,6 +267,7 @@
           }
 
           if (is_below_multi_return_threshold) {
+            decode_filtered_info_.mutliple_return_point_count++;
             continue;
           }
         }
@@ -178,6 +278,7 @@
 
         bool in_fov = angle_is_between(scan_cut_angles_.fov_min, scan_cut_angles_.fov_max, azimuth);
         if (!in_fov) {
+          decode_filtered_info_.fov_filtered_count++;
           continue;
         }
 
@@ -214,6 +315,9 @@
         // The driver wrapper converts to degrees, expects radians
         point.azimuth = corrected_angle_data.azimuth_rad;
         point.elevation = corrected_angle_data.elevation_rad;
+
+        decode_filtered_info_.get_minmax_info(point);
+        decode_filtered_info_.total_kept_point_count++;
       }
     }
   }
@@ -269,6 +373,7 @@
   int unpack(const std::vector<uint8_t> & packet) override
   {
     if (!parse_packet(packet)) {
+      decode_filtered_info_.invalid_packet_count++;
       return -1;
     }
 
@@ -320,6 +425,16 @@
         std::swap(decode_pc_, output_pc_);
         std::swap(decode_scan_timestamp_ns_, output_scan_timestamp_ns_);
         has_scanned_ = true;
+        nlohmann::ordered_json j = decode_filtered_info_.to_json();
+        std::cout << "=======================" << std::endl;
+        for (const auto & [key, value] : j.items()) {
+          std::cout << key << ": " << std::endl;
+          for (const auto & [k, v] : value.items()) {
+            std::cout << k << ": " << v << std::endl;
+          }
+        }
+        std::cout << "=======================" << std::endl;
+        decode_filtered_info_.clear();
       }
 
       last_azimuth_ = block_azimuth;