WIP

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

@@ -78,20 +78,12 @@ 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;
-        }
-      }
-
-      break;
-    }
+    auto min_azimuth_offset = std::min(azimuth_offset_rad_);
+    
 
     scan_cut_block_azimuth_ = scan_cut_block_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 +100,13 @@ 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_;
+    if (current_azimuth < last_azimuth) {
+      current_azimuth += MAX_AZIMUTH_LEN;
+    }
+
+    return current_azimuth >= scan_cut_block_azimuth_ && last_azimuth < scan_cut_block_azimuth_;
   }
 };
 

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

@@ -126,7 +126,7 @@ 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();

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

@@ -237,7 +237,7 @@ class HesaiDecoder : public HesaiScanDecoder
       return static_cast<uint32_t>(sensor_configuration_->scan_phase * 10);
     }
 
-    return sensor_configuration_->cloud_max_angle;
+    return sensor_configuration_->cloud_max_angle * 10;
   }
 
 public:
@@ -281,7 +281,7 @@ class HesaiDecoder : public HesaiScanDecoder
       convertReturns(block_id, n_returns);
 
       auto block_azimuth = packet_.body.blocks[block_id].get_azimuth();
-      bool scan_completed = angle_corrector_.blockCompletesScan(block_azimuth);
+      bool scan_completed = angle_corrector_.blockCompletesScan(block_azimuth, last_phase_);
 
       if (scan_completed) {
         std::swap(decode_pc_, output_pc_);

nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp

@@ -505,14 +505,16 @@ HesaiLidarRangeAll HesaiHwInterface::GetLidarRange()
 Status HesaiHwInterface::checkAndSetLidarRange(
   const HesaiCalibrationConfigurationBase & calibration)
 {
-  int cloud_min = sensor_configuration_->cloud_min_angle;
-  int cloud_max = sensor_configuration_->cloud_max_angle;
+  int cloud_min = sensor_configuration_->cloud_min_angle * 10;
+  int cloud_max = sensor_configuration_->cloud_max_angle * 10;
+
+  std::cout << "Starting with HW FoV of " << cloud_min << "~" << cloud_max << std::endl;
 
   // Only oversize the FoV if it is not already the full 360deg
-  if (cloud_max - cloud_min < 3600) {
+  if (cloud_min != 0 || cloud_max != 3600) {
     auto padding = calibration.getFovPadding();
-    cloud_min += static_cast<int>(std::get<0>(padding) * 10);
-    cloud_max += static_cast<int>(std::get<1>(padding) * 10);
+    cloud_min += floor(std::get<0>(padding) * 10);
+    cloud_max += ceil(std::get<1>(padding) * 10);
   }
 
   auto clamp = [](int x) {
@@ -521,6 +523,8 @@ Status HesaiHwInterface::checkAndSetLidarRange(
     return x;
   };
 
+  std::cout << "Setting HW FoV to " << cloud_min << "~" << cloud_max << std::endl;
+
   return SetLidarRange(clamp(cloud_min), clamp(cloud_max));
 }
 

nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml

@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    host_ip: 255.255.255.255
+    host_ip: 192.168.1.10
     sensor_ip: 192.168.1.201
     data_port: 2368
     gnss_port: 10110
@@ -11,8 +11,8 @@
     diag_span: 1000
     min_range: 0.3
     max_range: 300.0
-    cloud_min_angle: 0
-    cloud_max_angle: 360
+    cloud_min_angle: 90
+    cloud_max_angle: 270
     scan_phase: 0.0
     sensor_model: Pandar128E4X
     calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv