From ad004c8a602f90ad96a5afb11bb00c396f68d234 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?=
 <mehmeteminbasoglu@hotmail.com>
Date: Wed, 6 Dec 2023 13:41:57 +0300
Subject: [PATCH] fix(robosense): create corrected channel numbers

---
 .../nebula_common/robosense/robosense_common.hpp     | 12 ++++++++++++
 .../decoders/angle_corrector.hpp                     |  1 +
 .../decoders/angle_corrector_calibration_based.hpp   |  3 ++-
 .../decoders/robosense_decoder.hpp                   |  2 +-
 .../src/robosense/robosense_decoder_ros_wrapper.cpp  |  1 +
 5 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/nebula_common/include/nebula_common/robosense/robosense_common.hpp b/nebula_common/include/nebula_common/robosense/robosense_common.hpp
index 209dba00b..e92302f2d 100644
--- a/nebula_common/include/nebula_common/robosense/robosense_common.hpp
+++ b/nebula_common/include/nebula_common/robosense/robosense_common.hpp
@@ -69,6 +69,7 @@ struct ChannelCorrection
 {
   float azimuth{NAN};
   float elevation{NAN};
+  uint16_t channel{};
 
   [[nodiscard]] bool has_value() const { return !std::isnan(azimuth) && !std::isnan(elevation); }
 };
@@ -184,6 +185,17 @@ struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase
   {
     return calibration[channel_id];
   }
+
+  void CreateCorrectedChannels()
+  {
+    for(auto& correction : calibration) {
+      uint16_t channel = 0;
+      for(const auto& compare:calibration) {
+        if(compare.elevation < correction.elevation) ++channel;
+      }
+      correction.channel = channel;
+    }
+  }
 };
 
 }  // namespace drivers
diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp
index 0c95613c7..60b345861 100644
--- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp
+++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp
@@ -20,6 +20,7 @@ struct CorrectedAngleData
   float cos_azimuth;
   float sin_elevation;
   float cos_elevation;
+  uint16_t corrected_channel_id;
 };
 
 /// @brief Handles angle correction for given azimuth/channel combinations, as well as trigonometry
diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp
index 30aeb696a..99bd0c02f 100644
--- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp
+++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp
@@ -71,7 +71,8 @@ class AngleCorrectorCalibrationBased : public AngleCorrector
       azimuth_sin_[block_azimuth][channel_id],
       azimuth_cos_[block_azimuth][channel_id],
       elevation_sin_[channel_id],
-      elevation_cos_[channel_id]};
+      elevation_cos_[channel_id],
+      sensor_calibration_->calibration[channel_id].channel};
   }
 
   bool hasScanned(int current_azimuth, int last_azimuth) override
diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp
index ba478491f..79630bc23 100644
--- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp
+++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp
@@ -133,9 +133,9 @@ class RobosenseDecoder : public RobosenseScanDecoder
           getPointTimeRelative(packet_timestamp_ns, block_offset + start_block_id, channel_id);
 
         point.return_type = static_cast<uint8_t>(return_type);
-        point.channel = channel_id;
 
         auto corrected_angle_data = angle_corrector_.getCorrectedAngleData(raw_azimuth, channel_id);
+        point.channel = corrected_angle_data.corrected_channel_id;
 
         // The raw_azimuth and channel are only used as indices, sin/cos functions use the precise
         // corrected angles
diff --git a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp
index 858e9e198..d779e3bc6 100644
--- a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp
+++ b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp
@@ -143,6 +143,7 @@ void RobosenseDriverRosWrapper::ReceiveInfoMsgCallback(
   sensor_cfg_ptr_->return_mode = info_driver_ptr_->GetReturnMode();
   sensor_cfg_ptr_->use_sensor_time = info_driver_ptr_->GetSyncStatus();
   *calibration_cfg_ptr_ = info_driver_ptr_->GetSensorCalibration();
+  calibration_cfg_ptr_->CreateCorrectedChannels();
 
   RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << *sensor_cfg_ptr_);