feat(hesai): publish on cloud_max_angle

Author: mojomex

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

@@ -49,15 +49,10 @@ class AngleCorrector
   /// @return The corrected angles (azimuth, elevation) in radians and their sin/cos values
   virtual CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) = 0;
 
-  /// @brief Returns true if the current azimuth lies in a different (new) scan compared to the last
-  /// azimuth
-  /// @param current_azimuth The current azimuth value in the sensor's angle resolution
-  /// @param last_azimuth The last azimuth in the sensor's angle resolution
-  /// @param sync_azimuth The azimuth set in the sensor configuration, for which the
-  /// timestamp is aligned to the full second
-  /// @return true if the current azimuth is in a different scan than the last one, false otherwise
-  virtual bool hasScanned(
-    uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0;
+  /// @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;
 };
 
 }  // namespace drivers

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

@@ -17,6 +17,9 @@
 #include "nebula_common/hesai/hesai_common.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp"
 
+#include <nebula_common/nebula_common.hpp>
+
+#include <cmath>
 #include <cstdint>
 #include <memory>
 
@@ -40,9 +43,12 @@ 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_{};
+
 public:
   AngleCorrectorCalibrationBased(
-    const std::shared_ptr<const HesaiCalibrationConfiguration> & sensor_calibration)
+    const std::shared_ptr<const HesaiCalibrationConfiguration> & sensor_calibration,
+    float scan_cut_azimuth_rad)
   {
     if (sensor_calibration == nullptr) {
       throw std::runtime_error(
@@ -71,6 +77,21 @@ class AngleCorrectorCalibrationBased : public AngleCorrector<HesaiCalibrationCon
         azimuth_sin_[block_azimuth][channel_id] = sinf(precision_azimuth);
       }
     }
+
+    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;
+        }
+      }
+
+      break;
+    }
+
+    scan_cut_block_azimuth_ = scan_cut_block_azimuth;
   }
 
   CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override
@@ -87,15 +108,9 @@ class AngleCorrectorCalibrationBased : public AngleCorrector<HesaiCalibrationCon
       elevation_cos_[channel_id]};
   }
 
-  bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) override
+  bool blockCompletesScan(uint32_t current_azimuth) override
   {
-    // Cut the scan when the azimuth passes over the sync_azimuth
-    uint32_t current_diff_from_sync =
-      (MAX_AZIMUTH_LEN + current_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN;
-    uint32_t last_diff_from_sync =
-      (MAX_AZIMUTH_LEN + last_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN;
-
-    return current_diff_from_sync < last_diff_from_sync;
+    return current_azimuth == scan_cut_block_azimuth_;
   }
 };
 

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

@@ -17,8 +17,12 @@
 #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>
+#include <string>
+#include <vector>
 
 namespace nebula
 {
@@ -36,6 +40,8 @@ class AngleCorrectorCorrectionBased : public AngleCorrector<HesaiCorrection>
   std::array<float, MAX_AZIMUTH_LENGTH> cos_{};
   std::array<float, MAX_AZIMUTH_LENGTH> sin_{};
 
+  std::vector<uint32_t> cut_azimuths_;
+
   /// @brief For a given azimuth value, find its corresponding output field
   /// @param azimuth The azimuth to get the field for
   /// @return The correct output field, as specified in @ref HesaiCorrection
@@ -57,7 +63,7 @@ class AngleCorrectorCorrectionBased : public AngleCorrector<HesaiCorrection>
 
 public:
   explicit AngleCorrectorCorrectionBased(
-    const std::shared_ptr<const HesaiCorrection> & sensor_correction)
+    const std::shared_ptr<const HesaiCorrection> & sensor_correction, float scan_cut_azimuth_rad)
   : correction_(sensor_correction), logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased"))
   {
     if (sensor_correction == nullptr) {
@@ -72,6 +78,31 @@ class AngleCorrectorCorrectionBased : public AngleCorrector<HesaiCorrection>
       cos_[i] = cosf(rad);
       sin_[i] = sinf(rad);
     }
+
+    for (size_t field_id = 0; field_id < correction_->frameNumber; ++field_id) {
+      auto start = correction_->startFrame[field_id];
+      auto end = correction_->endFrame[field_id];
+      auto raw_azimuth = start;
+      for (; raw_azimuth != end; raw_azimuth = (raw_azimuth + 1) % MAX_AZIMUTH_LENGTH) {
+        for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) {
+          auto corrected_azimuth = getCorrectedAngleData(raw_azimuth, channel_id).azimuth_rad;
+          if (corrected_azimuth < scan_cut_azimuth_rad) {
+            break;  // Not all channels are past the cut azimuth, search at next raw azimuth
+          }
+        }
+
+        // All channels are past the cut azimuth, add this raw_azimuths to the cut azimuths
+        break;
+      }
+
+      cut_azimuths_.push_back(raw_azimuth);
+    }
+
+    if (cut_azimuths_.size() != correction_->frameNumber) {
+      throw std::runtime_error(
+        "Sensor has " + std::to_string(correction_->frameNumber) +
+        " fields but calculation resulted in " + std::to_string(cut_azimuths_.size()));
+    }
   }
 
   CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override
@@ -95,17 +126,11 @@ class AngleCorrectorCorrectionBased : public AngleCorrector<HesaiCorrection>
             cos_[azimuth], sin_[elevation], cos_[elevation]};
   }
 
-  bool hasScanned(
-    uint32_t current_azimuth, uint32_t last_azimuth, uint32_t /*sync_azimuth*/) override
+  bool blockCompletesScan(uint32_t block_azimuth) override
   {
-    // For AT128, the scan is always cut at the beginning of the field:
-    // If we would cut at `sync_azimuth`, the points left of it would be
-    // from the previous field and therefore significantly older than the
-    // points right of it.
-    // This also means that the pointcloud timestamp is only at top of second
-    // if the `sync_azimuth` aligns with the beginning of the field (e.g. 30deg for AT128).
-    // The absolute point time for points at `sync_azimuth` is still at top of second.
-    return findField(current_azimuth) != findField(last_azimuth);
+    auto begin = cut_azimuths_.cbegin();
+    auto end = cut_azimuths_.cend();
+    return std::find(begin, end, block_azimuth) != end;
   }
 };
 

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

@@ -202,20 +202,6 @@ class HesaiDecoder : public HesaiScanDecoder
     }
   }
 
-  /// @brief Checks whether the last processed block was the last block of a scan
-  /// @param current_phase The azimuth of the last processed block
-  /// @param sync_phase The azimuth set in the sensor configuration, for which the
-  /// timestamp is aligned to the full second
-  /// @return Whether the scan has completed
-  bool checkScanCompleted(uint32_t current_phase, uint32_t sync_phase)
-  {
-    if (last_phase_ == -1) {
-      return false;
-    }
-
-    return angle_corrector_.hasScanned(current_phase, last_phase_, sync_phase);
-  }
-
   /// @brief Get the distance of the given unit in meters
   float getDistance(const typename SensorT::packet_t::body_t::block_t::unit_t & unit)
   {
@@ -236,6 +222,24 @@ class HesaiDecoder : public HesaiScanDecoder
     return packet_to_scan_offset_ns + point_to_packet_offset_ns;
   }
 
+  /**
+   * @brief Return the appropriate scan cut azimuth based on whether the configured FoV is 360
+   * degrees (use scan phase for cutting) or less (use max angle for cutting).
+   *
+   * @param config The sensor configuration to use
+   * @return uint32_t The scan cut azimuth in 10ths of degrees
+   */
+  [[nodiscard]] uint32_t getCutAngle() const
+  {
+    if (
+      sensor_configuration_->cloud_min_angle == 0 &&
+      sensor_configuration_->cloud_max_angle == 3600) {
+      return static_cast<uint32_t>(sensor_configuration_->scan_phase * 10);
+    }
+
+    return sensor_configuration_->cloud_max_angle;
+  }
+
 public:
   /// @brief Constructor
   /// @param sensor_configuration SensorConfiguration for this decoder
@@ -245,14 +249,14 @@ class HesaiDecoder : public HesaiScanDecoder
     const std::shared_ptr<const typename SensorT::angle_corrector_t::correction_data_t> &
       correction_data)
   : sensor_configuration_(sensor_configuration),
-    angle_corrector_(correction_data),
+    angle_corrector_(correction_data, deg2rad(getCutAngle() / 10.0)),
     logger_(rclcpp::get_logger("HesaiDecoder"))
   {
     logger_.set_level(rclcpp::Logger::Level::Debug);
     RCLCPP_INFO_STREAM(logger_, *sensor_configuration_);
 
-    decode_pc_.reset(new NebulaPointCloud);
-    output_pc_.reset(new NebulaPointCloud);
+    decode_pc_ = std::make_shared<NebulaPointCloud>();
+    output_pc_ = std::make_shared<NebulaPointCloud>();
 
     decode_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS);
     output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS);
@@ -273,14 +277,11 @@ class HesaiDecoder : public HesaiScanDecoder
     }
 
     const size_t n_returns = hesai_packet::get_n_returns(packet_.tail.return_mode);
-    uint32_t current_azimuth;
-
     for (size_t block_id = 0; block_id < SensorT::packet_t::N_BLOCKS; block_id += n_returns) {
-      current_azimuth = packet_.body.blocks[block_id].get_azimuth();
+      convertReturns(block_id, n_returns);
 
-      bool scan_completed = checkScanCompleted(
-        current_azimuth,
-        sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS);
+      auto block_azimuth = packet_.body.blocks[block_id].get_azimuth();
+      bool scan_completed = angle_corrector_.blockCompletesScan(block_azimuth);
 
       if (scan_completed) {
         std::swap(decode_pc_, output_pc_);
@@ -295,8 +296,7 @@ class HesaiDecoder : public HesaiScanDecoder
                                     sensor_.getEarliestPointTimeOffsetForBlock(block_id, packet_);
       }
 
-      convertReturns(block_id, n_returns);
-      last_phase_ = current_azimuth;
+      last_phase_ = block_azimuth;
     }
 
     return last_phase_;