fix(robosense): create corrected channel numbers

tier4 · Dec 6, 2023 · ad004c8 · ad004c8
1 parent 3538b5d
commit ad004c8
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 2 deletions.
diff --git 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/...a_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp b/...a_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/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp b/.../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/...decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp b/...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
@@ -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_);