refactor(hesai_decoder): remove redundant arguments for correction/ca…

…libration

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

@@ -23,19 +23,11 @@ struct CorrectedAngleData
 
 /// @brief Handles angle correction for given azimuth/channel combinations, as well as trigonometry
 /// lookup tables
+template <typename CorrectionDataT>
 class AngleCorrector
 {
-protected:
-  const std::shared_ptr<const HesaiCalibrationConfiguration> sensor_calibration_;
-  const std::shared_ptr<const HesaiCorrection> sensor_correction_;
-
 public:
-  AngleCorrector(
-    const std::shared_ptr<const HesaiCalibrationConfiguration> & sensor_calibration,
-    const std::shared_ptr<const HesaiCorrection> & sensor_correction)
-  : sensor_calibration_(sensor_calibration), sensor_correction_(sensor_correction)
-  {
-  }
+  using correction_data_t = CorrectionDataT;
 
   /// @brief Get the corrected azimuth and elevation for a given block and channel, along with their
   /// sin/cos values.

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

@@ -11,7 +11,7 @@ namespace drivers
 {
 
 template <size_t ChannelN, size_t AngleUnit>
-class AngleCorrectorCalibrationBased : public AngleCorrector
+class AngleCorrectorCalibrationBased : public AngleCorrector<HesaiCalibrationConfiguration>
 {
 private:
   static constexpr size_t MAX_AZIMUTH_LEN = 360 * AngleUnit;
@@ -27,9 +27,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector
 
 public:
   AngleCorrectorCalibrationBased(
-    const std::shared_ptr<const HesaiCalibrationConfiguration> & sensor_calibration,
-    const std::shared_ptr<const HesaiCorrection> & sensor_correction)
-  : AngleCorrector(sensor_calibration, sensor_correction)
+    const std::shared_ptr<const HesaiCalibrationConfiguration> & sensor_calibration)
   {
     if (sensor_calibration == nullptr) {
       throw std::runtime_error(

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

@@ -13,10 +13,11 @@ namespace drivers
 {
 
 template <size_t ChannelN, size_t AngleUnit>
-class AngleCorrectorCorrectionBased : public AngleCorrector
+class AngleCorrectorCorrectionBased : public AngleCorrector<HesaiCorrection>
 {
 private:
   static constexpr size_t MAX_AZIMUTH_LENGTH = 360 * AngleUnit;
+  const std::shared_ptr<const HesaiCorrection> correction_;
   rclcpp::Logger logger_;
 
   std::array<float, MAX_AZIMUTH_LENGTH> cos_{};
@@ -31,26 +32,25 @@ class AngleCorrectorCorrectionBased : public AngleCorrector
     // * none of the startFrames are defined as > 360 deg (< 0 not possible since they are unsigned)
     // * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg etc.)
     // These assumptions hold for AT128E2X.
-    int field = sensor_correction_->frameNumber - 1;
-    for (size_t i = 0; i < sensor_correction_->frameNumber; ++i) {
-      if (azimuth < sensor_correction_->startFrame[i]) return field;
+    int field = correction_->frameNumber - 1;
+    for (size_t i = 0; i < correction_->frameNumber; ++i) {
+      if (azimuth < correction_->startFrame[i]) return field;
       field = i;
     }
 
-    // This is never reached if sensor_correction_ is correct
+    // This is never reached if correction_ is correct
     return field;
   }
 
 public:
   AngleCorrectorCorrectionBased(
-    const std::shared_ptr<const HesaiCalibrationConfiguration> & sensor_calibration,
     const std::shared_ptr<const HesaiCorrection> & sensor_correction)
-  : AngleCorrector(sensor_calibration, sensor_correction),
+  : correction_(sensor_correction),
     logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased"))
   {
     if (sensor_correction == nullptr) {
       throw std::runtime_error(
-        "Cannot instantiate AngleCorrectorCalibrationBased without calibration data");
+        "Cannot instantiate AngleCorrectorCorrectionBased without correction data");
     }
 
     logger_.set_level(rclcpp::Logger::Level::Debug);
@@ -64,17 +64,16 @@ class AngleCorrectorCorrectionBased : public AngleCorrector
 
   CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override
   {
-    const auto & correction = AngleCorrector::sensor_correction_;
     int field = findField(block_azimuth);
 
     auto elevation =
-      correction->elevation[channel_id] +
-      correction->getElevationAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100);
+      correction_->elevation[channel_id] +
+      correction_->getElevationAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100);
     elevation = (MAX_AZIMUTH_LENGTH + elevation) % MAX_AZIMUTH_LENGTH;
 
-    auto azimuth = (block_azimuth + MAX_AZIMUTH_LENGTH - correction->startFrame[field]) * 2 -
-                   correction->azimuth[channel_id] +
-                   correction->getAzimuthAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100);
+    auto azimuth = (block_azimuth + MAX_AZIMUTH_LENGTH - correction_->startFrame[field]) * 2 -
+                   correction_->azimuth[channel_id] +
+                   correction_->getAzimuthAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100);
     azimuth = (MAX_AZIMUTH_LENGTH + azimuth) % MAX_AZIMUTH_LENGTH;
 
     float azimuth_rad = 2.f * azimuth * M_PI / MAX_AZIMUTH_LENGTH;

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

@@ -204,16 +204,12 @@ class HesaiDecoder : public HesaiScanDecoder
 public:
   /// @brief Constructor
   /// @param sensor_configuration SensorConfiguration for this decoder
-  /// @param calibration_configuration Calibration for this decoder (can be nullptr if
-  /// correction_configuration is set)
-  /// @param correction_configuration Correction for this decoder (can be nullptr if
-  /// calibration_configuration is set)
+  /// @param correction_data Calibration data for this decoder
   explicit HesaiDecoder(
     const std::shared_ptr<const HesaiSensorConfiguration> & sensor_configuration,
-    const std::shared_ptr<const HesaiCalibrationConfiguration> & calibration_configuration,
-    const std::shared_ptr<const HesaiCorrection> & correction_configuration)
+    const std::shared_ptr<const typename SensorT::angle_corrector_t::correction_data_t> & correction_data)
   : sensor_configuration_(sensor_configuration),
-    angle_corrector_(calibration_configuration, correction_configuration),
+    angle_corrector_(correction_data),
     logger_(rclcpp::get_logger("HesaiDecoder"))
   {
     logger_.set_level(rclcpp::Logger::Level::Debug);

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp

@@ -16,6 +16,7 @@
 #include <iostream>
 #include <stdexcept>
 #include <string>
+#include <memory>
 
 namespace nebula
 {
@@ -30,15 +31,22 @@ class HesaiDriver
   /// @brief Decoder according to the model
   std::shared_ptr<HesaiScanDecoder> scan_decoder_;
 
+  template <typename SensorT>
+  std::shared_ptr<HesaiScanDecoder> InitializeDecoder(
+    const std::shared_ptr<const drivers::HesaiSensorConfiguration> & sensor_configuration,
+    const std::shared_ptr<const drivers::HesaiCalibrationConfigurationBase> &
+      calibration_configuration);
+
 public:
   HesaiDriver() = delete;
   /// @brief Constructor
   /// @param sensor_configuration SensorConfiguration for this driver
-  /// @param calibration_configuration CalibrationConfiguration for this driver (either HesaiCalibrationConfiguration
-  /// for sensors other than AT128 or HesaiCorrection for AT128)
+  /// @param calibration_configuration CalibrationConfiguration for this driver (either
+  /// HesaiCalibrationConfiguration for sensors other than AT128 or HesaiCorrection for AT128)
   explicit HesaiDriver(
-      const std::shared_ptr<const drivers::HesaiSensorConfiguration>& sensor_configuration,
-      const std::shared_ptr<const drivers::HesaiCalibrationConfigurationBase>& calibration_configuration = nullptr);
+    const std::shared_ptr<const drivers::HesaiSensorConfiguration> & sensor_configuration,
+    const std::shared_ptr<const drivers::HesaiCalibrationConfigurationBase> &
+      calibration_configuration);
 
   /// @brief Get current status of this driver
   /// @return Current status
@@ -47,12 +55,14 @@ class HesaiDriver
   /// @brief Setting CalibrationConfiguration (not used)
   /// @param calibration_configuration
   /// @return Resulting status
-  Status SetCalibrationConfiguration(const HesaiCalibrationConfigurationBase& calibration_configuration);
+  Status SetCalibrationConfiguration(
+    const HesaiCalibrationConfigurationBase & calibration_configuration);
 
   /// @brief Convert PandarScan message to point cloud
   /// @param pandar_scan Message
   /// @return tuple of Point cloud and timestamp
-  std::tuple<drivers::NebulaPointCloudPtr, double> ParseCloudPacket(const std::vector<uint8_t>& packet);
+  std::tuple<drivers::NebulaPointCloudPtr, double> ParseCloudPacket(
+    const std::vector<uint8_t> & packet);
 };
 
 }  // namespace drivers

nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp

@@ -17,64 +17,62 @@ namespace nebula
 {
 namespace drivers
 {
-HesaiDriver::HesaiDriver(const std::shared_ptr<const HesaiSensorConfiguration>& sensor_configuration,
-                         const std::shared_ptr<const HesaiCalibrationConfigurationBase>& calibration_configuration)
+HesaiDriver::HesaiDriver(
+  const std::shared_ptr<const HesaiSensorConfiguration> & sensor_configuration,
+  const std::shared_ptr<const HesaiCalibrationConfigurationBase> & calibration_data)
 {
   // initialize proper parser from cloud config's model and echo mode
   driver_status_ = nebula::Status::OK;
 
-  std::shared_ptr<const HesaiCalibrationConfiguration> calibration = nullptr;
-  std::shared_ptr<const HesaiCorrection> correction = nullptr;
-
-  if (sensor_configuration->sensor_model == SensorModel::HESAI_PANDARAT128)
-  {
-    correction = std::static_pointer_cast<const HesaiCorrection>(calibration_configuration);
-  }
-  else
-  {
-    calibration = std::static_pointer_cast<const HesaiCalibrationConfiguration>(calibration_configuration);
-  }
-
-  switch (sensor_configuration->sensor_model)
-  {
-    case SensorModel::UNKNOWN:
-      driver_status_ = nebula::Status::INVALID_SENSOR_MODEL;
-      break;
+  switch (sensor_configuration->sensor_model) {
     case SensorModel::HESAI_PANDAR64:
-      scan_decoder_.reset(new HesaiDecoder<Pandar64>(sensor_configuration, calibration, correction));
+      scan_decoder_ = InitializeDecoder<Pandar64>(sensor_configuration, calibration_data);
       break;
     case SensorModel::HESAI_PANDAR40P:
     case SensorModel::HESAI_PANDAR40M:
-      scan_decoder_.reset(new HesaiDecoder<Pandar40>(sensor_configuration, calibration, correction));
+      scan_decoder_ = InitializeDecoder<Pandar40>(sensor_configuration, calibration_data);
       break;
     case SensorModel::HESAI_PANDARQT64:
-      scan_decoder_.reset(new HesaiDecoder<PandarQT64>(sensor_configuration, calibration, correction));
+      scan_decoder_ = InitializeDecoder<PandarQT64>(sensor_configuration, calibration_data);
       break;
     case SensorModel::HESAI_PANDARQT128:
-      scan_decoder_.reset(new HesaiDecoder<PandarQT128>(sensor_configuration, calibration, correction));
+      scan_decoder_ = InitializeDecoder<PandarQT128>(sensor_configuration, calibration_data);
       break;
     case SensorModel::HESAI_PANDARXT32:
-      scan_decoder_.reset(new HesaiDecoder<PandarXT32>(sensor_configuration, calibration, correction));
+      scan_decoder_ = InitializeDecoder<PandarXT32>(sensor_configuration, calibration_data);
       break;
     case SensorModel::HESAI_PANDARXT32M:
-      scan_decoder_.reset(new HesaiDecoder<PandarXT32M>(sensor_configuration, calibration, correction));
+      scan_decoder_ = InitializeDecoder<PandarXT32M>(sensor_configuration, calibration_data);
       break;
     case SensorModel::HESAI_PANDARAT128:
-      scan_decoder_.reset(new HesaiDecoder<PandarAT128>(sensor_configuration, calibration, correction));
+      scan_decoder_ = InitializeDecoder<PandarAT128>(sensor_configuration, calibration_data);
       break;
     case SensorModel::HESAI_PANDAR128_E3X:
-      scan_decoder_.reset(new HesaiDecoder<Pandar128E3X>(sensor_configuration, calibration, correction));
+      scan_decoder_ = InitializeDecoder<Pandar128E3X>(sensor_configuration, calibration_data);
       break;
     case SensorModel::HESAI_PANDAR128_E4X:
-      scan_decoder_.reset(new HesaiDecoder<Pandar128E4X>(sensor_configuration, calibration, correction));
+      scan_decoder_ = InitializeDecoder<Pandar128E4X>(sensor_configuration, calibration_data);
       break;
+    case SensorModel::UNKNOWN:
+      driver_status_ = nebula::Status::INVALID_SENSOR_MODEL;
+      throw std::runtime_error("Invalid sensor model.");
     default:
       driver_status_ = nebula::Status::NOT_INITIALIZED;
       throw std::runtime_error("Driver not Implemented for selected sensor.");
-      break;
   }
 }
 
+template <typename SensorT>
+std::shared_ptr<HesaiScanDecoder> HesaiDriver::InitializeDecoder(
+  const std::shared_ptr<const drivers::HesaiSensorConfiguration> & sensor_configuration,
+  const std::shared_ptr<const drivers::HesaiCalibrationConfigurationBase> &
+    calibration_configuration)
+{
+  using CalibT = typename SensorT::angle_corrector_t::correction_data_t;
+  return std::make_shared<HesaiDecoder<SensorT>>(
+    sensor_configuration, std::dynamic_pointer_cast<const CalibT>(calibration_configuration));
+}
+
 std::tuple<drivers::NebulaPointCloudPtr, double> HesaiDriver::ParseCloudPacket(
   const std::vector<uint8_t> & packet)
 {