feat(hesai): implement mask-based pruning filter for Hesai

docs/filters.md

@@ -34,7 +34,7 @@ The following filter types are supported:
 
 | Filter Name            | Filter Type       | Hesai | Robosense | Velodyne |
 | ---------------------- | ----------------- | :---: | :-------: | :------: |
-| Downsample Mask Filter | `downsample_mask` |  ❌   |    ❌     |    ❌    |
+| Downsample Mask Filter | `downsample_mask` |  ✅   |    ❌     |    ❌    |
 
 Compatibility:  
 ✅: compatible  
@@ -87,11 +87,37 @@ The filter can be disabled by omitting the `downsample_mask` config item, or by
 ros2 param set /<vendor>_ros_wrapper_node point_filters.downsample_mask.path ""'
 ```
 
+The required resolution of the mask is sensor-dependent:
+
+| Sensor Model | Required Resolution |
+| ------------ | ------------------- |
+| Pandar40P    | 1800 x 40           |
+| Pandar64     | 1800 x 64           |
+| PandarXT16   | 2000 x 16           |
+| PandarXT32   | 2000 x 32           |
+| PandarXT32M  | 2000 x 32           |
+| PandarAT128  | 1200 x 128          |
+| PandarQT64   | 600 x 64            |
+| PandarQT128  | 900 x 128           |
+| Pandar128E4X | 3600 x 128          |
+
+<!-- prettier-ignore-start -->
+!!! warning
+    If the mask resolution does not match the required resolution, Nebula will not start (if configured on launch) or reject the setting (if set during runtime).
+<!-- prettier-ignore-end -->
+
 #### Behavior
 
+<!-- prettier-ignore-start -->
+!!! warning
+    Color spaces in image editors can be confusing and can lead to unexpected results (more/less downsampling than expected).
+    To work on monochrome images, use the `Grayscale` color mode and use the `Value` in the HSV color space to choose greyscale values.
+    For example, a `Value` (HSV) of 50% is equal to a downsampling factor of 2, but a `Luminosity` (LCh) of 50% is equal to a downsampling factor of 2.17.
+    Check the generated `_dithered.png` mask to see whether you are getting the expected results.
+<!-- prettier-ignore-end -->
+
 - Greyscale values are quantized to the nearest 10th (yielding 11 quantization levels in total)
 - Mask resolution is dictated by the sensor's maximum FoV, the number of channels (for rotational LiDARs) and the peak angular resolution:
   - For a 40-channel LiDAR with `360 deg` FoV and `0.1 deg` peak azimuth resolution, the mask has to be `(360 / 0.1, 40) = (3600, 40)` pixels
   - Currently, non-rotational LiDARs are not yet supported
-- Image editors like GIMP use perceptual color profiles, which can lead to unexpected results (more/less downsampling than expected). Check the generated `_dithered.png` mask to see if you are affected.
 - Dithering performed by Nebula is spatial only, meaning that it stays constant over time. Decoded points are checked against the nearest pixel in the dithered mask

docs/requirements.txt

@@ -5,7 +5,7 @@ markdown==3.3.7
 mdx_truly_sane_lists
 mkdocs-awesome-pages-plugin==2.9.1
 mkdocs-exclude
-mkdocs-macros-plugin
+mkdocs-macros-plugin>=1.2.0
 mkdocs-same-dir
 mkdocs-static-i18n
 mkdocs-video

mkdocs.yml

@@ -167,6 +167,7 @@ plugins:
       force_render_paths: |
         *
         !*_source.md
+        !class*.md
   - mkdoxy:
       projects:
         nebula_common:

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -24,6 +24,7 @@
 #include <fstream>
 #include <iostream>
 #include <map>
+#include <optional>
 #include <sstream>
 #include <stdexcept>
 #include <string>
@@ -50,6 +51,7 @@
   PtpTransportType ptp_transport_type;
   PtpSwitchType ptp_switch_type;
   uint8_t ptp_lock_threshold;
+  std::optional<std::string> downsample_mask_path;
 };
 /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator)
 /// @param os
@@ -60,7 +62,7 @@
   os << "Hesai Sensor Configuration:" << '\n';
   os << (LidarConfigurationBase)(arg) << '\n';
   os << "Multicast: "
-     << (arg.multicast_ip.empty() ? "disabled" : "enabled, group " + arg.multicast_ip) << '\n';
+     << (arg.multicast_ip.empty() ? "disabled" : "enabled, group: " + arg.multicast_ip) << '\n';
   os << "GNSS Port: " << arg.gnss_port << '\n';
   os << "Rotation Speed: " << arg.rotation_speed << '\n';
   os << "Sync Angle: " << arg.sync_angle << '\n';
@@ -73,7 +75,10 @@
   os << "PTP Domain: " << std::to_string(arg.ptp_domain) << '\n';
   os << "PTP Transport Type: " << arg.ptp_transport_type << '\n';
   os << "PTP Switch Type: " << arg.ptp_switch_type << '\n';
-  os << "PTP Lock Threshold: " << std::to_string(arg.ptp_lock_threshold);
+  os << "PTP Lock Threshold: " << std::to_string(arg.ptp_lock_threshold) << '\n';
+  os << "Downsample Filter: "
+     << (arg.downsample_mask_path ? "enabled, path: " + arg.downsample_mask_path.value()
+                                  : "disabled");
   return os;
 }
 

nebula_decoders/CMakeLists.txt

@@ -53,10 +53,12 @@ add_library(nebula_decoders_hesai SHARED
 )
 target_link_libraries(nebula_decoders_hesai PUBLIC
     ${pandar_msgs_TARGETS}
+    ${PNG_LIBRARIES}
 )
 
 target_include_directories(nebula_decoders_hesai PUBLIC
     ${pandar_msgs_INCLUDE_DIRS}
+    ${PNG_INCLUDE_DIRS}
 )
 
 # Velodyne

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

@@ -15,11 +15,13 @@
 #pragma once
 
 #include "nebula_decoders/nebula_decoders_common/angles.hpp"
+#include "nebula_decoders/nebula_decoders_common/point_filters/downsample_mask.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp"
 
 #include <nebula_common/hesai/hesai_common.hpp>
+#include <nebula_common/loggers/logger.hpp>
 #include <nebula_common/nebula_common.hpp>
 #include <nebula_common/point_types.hpp>
 #include <rclcpp/logging.hpp>
@@ -28,7 +30,9 @@
 #include <algorithm>
 #include <array>
 #include <cmath>
+#include <cstdint>
 #include <memory>
+#include <optional>
 #include <tuple>
 #include <utility>
 #include <vector>
@@ -46,7 +50,7 @@ class HesaiDecoder : public HesaiScanDecoder
     float scan_emit_angle;
   };
 
-protected:
+private:
   /// @brief Configuration for this decoder
   const std::shared_ptr<const drivers::HesaiSensorConfiguration> sensor_configuration_;
 
@@ -74,7 +78,7 @@ class HesaiDecoder : public HesaiScanDecoder
   ScanCutAngles scan_cut_angles_;
   uint32_t last_azimuth_ = 0;
 
-  rclcpp::Logger logger_;
+  std::shared_ptr<loggers::Logger> logger_;
 
   /// @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_;
@@ -83,15 +87,17 @@ class HesaiDecoder : public HesaiScanDecoder
   std::array<std::array<int, SensorT::packet_t::n_blocks>, SensorT::packet_t::max_returns>
     block_firing_offset_ns_;
 
+  std::optional<point_filters::DownsampleMaskFilter> mask_filter_;
+
   /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc.
   /// @param packet The incoming PandarPacket
   /// @return Whether the packet was parsed successfully
   bool parse_packet(const std::vector<uint8_t> & packet)
   {
     if (packet.size() < sizeof(typename SensorT::packet_t)) {
-      RCLCPP_ERROR_STREAM(
-        logger_, "Packet size mismatch: " << packet.size() << " | Expected at least: "
-                                          << sizeof(typename SensorT::packet_t));
+      NEBULA_LOG_STREAM(
+        logger_->error, "Packet size mismatch: " << packet.size() << " | Expected at least: "
+                                                 << sizeof(typename SensorT::packet_t));
       return false;
     }
     if (std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) {
@@ -100,7 +106,7 @@ class HesaiDecoder : public HesaiScanDecoder
       return true;
     }
 
-    RCLCPP_ERROR(logger_, "Packet memcopy failed");
+    logger_->error("Packet memcopy failed");
     return false;
   }
 
@@ -195,7 +201,7 @@ class HesaiDecoder : public HesaiScanDecoder
         uint64_t 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 = get_point_time_relative(
@@ -214,6 +220,10 @@ 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;
+
+        if (!mask_filter_ || !mask_filter_->excluded(point)) {
+          pc->emplace_back(point);
+        }
       }
     }
   }
@@ -246,24 +256,29 @@ class HesaiDecoder : public HesaiScanDecoder
   explicit HesaiDecoder(
     const std::shared_ptr<const HesaiSensorConfiguration> & sensor_configuration,
     const std::shared_ptr<const typename SensorT::angle_corrector_t::correction_data_t> &
-      correction_data)
+      correction_data,
+    const std::shared_ptr<loggers::Logger> & logger)
   : sensor_configuration_(sensor_configuration),
     angle_corrector_(
       correction_data, sensor_configuration_->cloud_min_angle,
       sensor_configuration_->cloud_max_angle, sensor_configuration_->cut_angle),
-    logger_(rclcpp::get_logger("HesaiDecoder"))
+    scan_cut_angles_(
+      {deg2rad(sensor_configuration_->cloud_min_angle),
+       deg2rad(sensor_configuration_->cloud_max_angle), deg2rad(sensor_configuration_->cut_angle)}),
+    logger_(logger)
   {
-    logger_.set_level(rclcpp::Logger::Level::Debug);
-
     decode_pc_ = std::make_shared<NebulaPointCloud>();
     output_pc_ = std::make_shared<NebulaPointCloud>();
 
+    if (sensor_configuration->downsample_mask_path) {
+      mask_filter_ = point_filters::DownsampleMaskFilter(
+        sensor_configuration->downsample_mask_path.value(), SensorT::fov_mdeg.azimuth,
+        SensorT::peak_resolution_mdeg.azimuth, SensorT::packet_t::n_channels,
+        logger_->child("Downsample Mask"), true);
+    }
+
     decode_pc_->reserve(SensorT::max_scan_buffer_points);
     output_pc_->reserve(SensorT::max_scan_buffer_points);
-
-    scan_cut_angles_ = {
-      deg2rad(sensor_configuration_->cloud_min_angle),
-      deg2rad(sensor_configuration_->cloud_max_angle), deg2rad(sensor_configuration_->cut_angle)};
   }
 
   int unpack(const std::vector<uint8_t> & packet) override

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp

@@ -220,6 +220,8 @@ class Pandar128E3X : public HesaiSensor<hesai_packet::Packet128E3X>
   static constexpr float min_range = 0.1;
   static constexpr float max_range = 230.0;
   static constexpr size_t max_scan_buffer_points = 691200;
+  static constexpr FieldOfView<int32_t, MilliDegrees> fov_mdeg{{0, 360'000}, {-25'000, 14'400}};
+  static constexpr AnglePair<int32_t, MilliDegrees> peak_resolution_mdeg{100, 125};
 
   int get_packet_relative_point_time_offset(
     uint32_t block_id, uint32_t channel_id, const packet_t & packet) override

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp

@@ -83,6 +83,8 @@ class Pandar128E4X : public HesaiSensor<hesai_packet::Packet128E4X>
   static constexpr float min_range = 0.1;
   static constexpr float max_range = 230.0;
   static constexpr size_t max_scan_buffer_points = 691200;
+  static constexpr FieldOfView<int32_t, MilliDegrees> fov_mdeg{{0, 360'000}, {-24'800, 14'400}};
+  static constexpr AnglePair<int32_t, MilliDegrees> peak_resolution_mdeg{100, 125};
 
   int get_packet_relative_point_time_offset(
     uint32_t block_id, uint32_t channel_id, const packet_t & packet) override

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp

@@ -14,6 +14,7 @@
 
 #pragma once
 
+#include "nebula_decoders/nebula_decoders_common/angles.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp"
 
@@ -74,6 +75,8 @@ class Pandar40 : public HesaiSensor<hesai_packet::Packet40P>
   static constexpr float min_range = 0.3f;
   static constexpr float max_range = 200.f;
   static constexpr size_t max_scan_buffer_points = 144000;
+  static constexpr FieldOfView<int32_t, MilliDegrees> fov_mdeg{{0, 360'000}, {-25'000, 15'000}};
+  static constexpr AnglePair<int32_t, MilliDegrees> peak_resolution_mdeg{200, 334};
 
   int get_packet_relative_point_time_offset(
     uint32_t block_id, uint32_t channel_id, const packet_t & packet) override

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp

@@ -54,6 +54,8 @@ class Pandar64 : public HesaiSensor<hesai_packet::Packet64>
   static constexpr float min_range = 0.3f;
   static constexpr float max_range = 200.f;
   static constexpr size_t max_scan_buffer_points = 230400;
+  static constexpr FieldOfView<int32_t, MilliDegrees> fov_mdeg{{0, 360'000}, {-25'000, 15'000}};
+  static constexpr AnglePair<int32_t, MilliDegrees> peak_resolution_mdeg{200, 167};
 
   int get_packet_relative_point_time_offset(
     uint32_t block_id, uint32_t channel_id, const packet_t & packet) override

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp

@@ -80,6 +80,9 @@ class PandarAT128
   static constexpr float min_range = 1.f;
   static constexpr float max_range = 180.0f;
   static constexpr size_t max_scan_buffer_points = 307200;
+  static constexpr FieldOfView<int32_t, MilliDegrees> fov_mdeg{
+    {30'000, 150'000}, {-12'500, 12'900}};
+  static constexpr AnglePair<int32_t, MilliDegrees> peak_resolution_mdeg{100, 200};
 
   int get_packet_relative_point_time_offset(
     uint32_t block_id, uint32_t channel_id, const packet_t & packet) override

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp

@@ -93,6 +93,8 @@ class PandarQT128 : public HesaiSensor<hesai_packet::PacketQT128C2X>
   static constexpr float min_range = 0.05;
   static constexpr float max_range = 50.0;
   static constexpr size_t max_scan_buffer_points = 172800;
+  static constexpr FieldOfView<int32_t, MilliDegrees> fov_mdeg{{0, 360'000}, {-52'630, 52'630}};
+  static constexpr AnglePair<int32_t, MilliDegrees> peak_resolution_mdeg{400, 100};
 
   int get_packet_relative_point_time_offset(
     uint32_t block_id, uint32_t channel_id, const packet_t & packet) override

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp

@@ -63,6 +63,8 @@ class PandarQT64 : public HesaiSensor<hesai_packet::PacketQT64>
   static constexpr float min_range = 0.1f;
   static constexpr float max_range = 60.f;
   static constexpr size_t max_scan_buffer_points = 76800;
+  static constexpr FieldOfView<int32_t, MilliDegrees> fov_mdeg{{0, 360'000}, {-52'100, 52'100}};
+  static constexpr AnglePair<int32_t, MilliDegrees> peak_resolution_mdeg{600, 1'450};
 
   int get_packet_relative_point_time_offset(
     uint32_t block_id, uint32_t channel_id, const packet_t & packet) override

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt16.hpp

@@ -49,6 +49,8 @@ class PandarXT16 : public HesaiSensor<hesai_packet::PacketXT16>
   static constexpr float min_range = 0.05f;
   static constexpr float max_range = 120.0f;
   static constexpr size_t max_scan_buffer_points = 128000;
+  static constexpr FieldOfView<int32_t, MilliDegrees> fov_mdeg{{0, 360'000}, {-15'000, 15'000}};
+  static constexpr AnglePair<int32_t, MilliDegrees> peak_resolution_mdeg{180, 1'000};
 
   int get_packet_relative_point_time_offset(
     uint32_t block_id, uint32_t channel_id, const packet_t & packet) override

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp

@@ -56,6 +56,8 @@ class PandarXT32 : public HesaiSensor<hesai_packet::PacketXT32>
   static constexpr float min_range = 0.05f;
   static constexpr float max_range = 120.0f;
   static constexpr size_t max_scan_buffer_points = 256000;
+  static constexpr FieldOfView<int32_t, MilliDegrees> fov_mdeg{{0, 360'000}, {-16'000, 15'000}};
+  static constexpr AnglePair<int32_t, MilliDegrees> peak_resolution_mdeg{180, 1'000};
 
   int get_packet_relative_point_time_offset(
     uint32_t block_id, uint32_t channel_id, const packet_t & packet) override

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp

@@ -46,6 +46,8 @@ class PandarXT32M : public HesaiSensor<hesai_packet::PacketXT32M2X>
   static constexpr float min_range = 0.5f;
   static constexpr float max_range = 300.0f;
   static constexpr size_t max_scan_buffer_points = 384000;
+  static constexpr FieldOfView<int32_t, MilliDegrees> fov_mdeg{{0, 360'000}, {-20'800, 19'500}};
+  static constexpr AnglePair<int32_t, MilliDegrees> peak_resolution_mdeg{180, 1'300};
 
   int get_packet_relative_point_time_offset(
     uint32_t block_id, uint32_t channel_id, const packet_t & packet) override

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp

@@ -20,6 +20,8 @@
 #include "nebula_common/point_types.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp"
 
+#include <nebula_common/loggers/logger.hpp>
+
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <memory>
@@ -34,6 +36,7 @@ class HesaiDriver
 private:
   /// @brief Current driver status
   Status driver_status_;
+  std::shared_ptr<loggers::Logger> logger_;
   /// @brief Decoder according to the model
   std::shared_ptr<HesaiScanDecoder> scan_decoder_;
 
@@ -52,7 +55,8 @@ class HesaiDriver
   explicit HesaiDriver(
     const std::shared_ptr<const drivers::HesaiSensorConfiguration> & sensor_configuration,
     const std::shared_ptr<const drivers::HesaiCalibrationConfigurationBase> &
-      calibration_configuration);
+      calibration_configuration,
+    const std::shared_ptr<loggers::Logger> & logger);
 
   /// @brief Get current status of this driver
   /// @return Current status