fix(hesai): cut scans correctly in edge cases

Author: mojomex

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp

@@ -52,7 +52,7 @@ class AngleCorrector
   /// @brief Returns whether the given block (azimuth) is the last in the current scan
   /// @param block_azimuth The current azimuth value in the sensor's angle resolution
   /// @return true if the current azimuth is the last in the current scan, false otherwise
-  virtual bool blockCompletesScan(uint32_t block_azimuth) = 0;
+  virtual bool blockCompletesScan(uint32_t block_azimuth, uint32_t last_azimuth) = 0;
 };
 
 }  // namespace drivers

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp

@@ -19,6 +19,7 @@
 
 #include <nebula_common/nebula_common.hpp>
 
+#include <algorithm>
 #include <cmath>
 #include <cstdint>
 #include <memory>
@@ -43,7 +44,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector<HesaiCalibrationCon
   std::array<std::array<float, ChannelN>, MAX_AZIMUTH_LEN> azimuth_cos_{};
   std::array<std::array<float, ChannelN>, MAX_AZIMUTH_LEN> azimuth_sin_{};
 
-  uint32_t scan_cut_block_azimuth_{};
+  const uint32_t scan_cut_block_azimuth_{};
 
 public:
   AngleCorrectorCalibrationBased(
@@ -78,20 +79,17 @@ class AngleCorrectorCalibrationBased : public AngleCorrector<HesaiCalibrationCon
       }
     }
 
-    auto scan_cut_block_azimuth = static_cast<uint32_t>(rad2deg(scan_cut_azimuth_rad) * 10.0);
-    while (true) {
-      auto block_azimuth_rad = block_azimuth_rad_[scan_cut_block_azimuth];
-      for (auto correction : azimuth_offset_rad_) {
-        if (block_azimuth_rad + correction < scan_cut_azimuth_rad) {
-          scan_cut_block_azimuth_++;
-          break;
-        }
-      }
+    auto min_azimuth_offset = -std::min(azimuth_offset_rad_);
 
-      break;
+    auto cut_azimuth = std::ceil(rad2deg(scan_cut_azimuth_rad + min_azimuth_offset) * AngleUnit);
+    if (cut_azimuth > MAX_AZIMUTH_LEN) {
+      cut_azimuth -= MAX_AZIMUTH_LEN;
     }
 
-    scan_cut_block_azimuth_ = scan_cut_block_azimuth;
+    scan_cut_block_azimuth_ = cut_azimuth;
+
+    std::cout << "Cut angle setting: " << rad2deg(scan_cut_azimuth_rad)
+              << " deg, calculated: " << scan_cut_block_azimuth_ / 100.0 << " deg" << std::endl;
   }
 
   CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override
@@ -108,9 +106,18 @@ class AngleCorrectorCalibrationBased : public AngleCorrector<HesaiCalibrationCon
       elevation_cos_[channel_id]};
   }
 
-  bool blockCompletesScan(uint32_t current_azimuth) override
+  bool blockCompletesScan(uint32_t current_azimuth, uint32_t last_azimuth) override
   {
-    return current_azimuth == scan_cut_block_azimuth_;
+    auto cut_azimuth = scan_cut_block_azimuth_;
+    if (cut_azimuth < last_azimuth) {
+      cut_azimuth += MAX_AZIMUTH_LEN;
+    }
+
+    if (current_azimuth < last_azimuth) {
+      current_azimuth += MAX_AZIMUTH_LEN;
+    }
+
+    return current_azimuth >= cut_azimuth && last_azimuth < cut_azimuth;
   }
 };
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp

@@ -17,7 +17,6 @@
 #include "nebula_common/hesai/hesai_common.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp"
 
-#include <algorithm>
 #include <cstdint>
 #include <memory>
 #include <stdexcept>
@@ -126,11 +125,20 @@ class AngleCorrectorCorrectionBased : public AngleCorrector<HesaiCorrection>
             cos_[azimuth], sin_[elevation], cos_[elevation]};
   }
 
-  bool blockCompletesScan(uint32_t block_azimuth) override
+  bool blockCompletesScan(uint32_t block_azimuth, uint32_t last_azimuth) override
   {
-    auto begin = cut_azimuths_.cbegin();
-    auto end = cut_azimuths_.cend();
-    return std::find(begin, end, block_azimuth) != end;
+    for (auto cut_azimuth : cut_azimuths_) {
+      if (cut_azimuth < last_azimuth) {
+        cut_azimuth += MAX_AZIMUTH_LENGTH;
+      }
+
+      auto current_azimuth = block_azimuth;
+      if (current_azimuth < last_azimuth) {
+        current_azimuth += MAX_AZIMUTH_LENGTH;
+      }
+
+      return current_azimuth >= cut_azimuth && last_azimuth < cut_azimuth;
+    }
   }
 };