feat(hesai): software-based precise FoV limit & early publishing

.cspell.json

@@ -11,6 +11,8 @@
     "block_id",
     "Bpearl",
     "calib",
+    "centi",
+    "ddeg",
     "DHAVE",
     "Difop",
     "extrinsics",

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -19,13 +19,15 @@
 #include "nebula_common/nebula_status.hpp"
 #include "nebula_common/util/string_conversions.hpp"
 
+#include <algorithm>
 #include <cmath>
 #include <fstream>
 #include <iostream>
 #include <map>
 #include <sstream>
 #include <stdexcept>
 #include <string>
+#include <tuple>
 #include <vector>
 namespace nebula
 {
@@ -36,9 +38,10 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase
 {
   std::string multicast_ip;
   uint16_t gnss_port{};
-  double scan_phase{};
+  uint16_t sync_angle{};
+  double cut_angle{};
   double dual_return_distance_threshold{};
-  std::string calibration_path{};
+  std::string calibration_path;
   uint16_t rotation_speed;
   uint16_t cloud_min_angle;
   uint16_t cloud_max_angle;
@@ -58,11 +61,13 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con
   os << "Multicast: "
      << (arg.multicast_ip.empty() ? "disabled" : "enabled, group " + arg.multicast_ip) << '\n';
   os << "GNSS Port: " << arg.gnss_port << '\n';
-  os << "Scan Phase: " << arg.scan_phase << '\n';
   os << "Rotation Speed: " << arg.rotation_speed << '\n';
+  os << "Sync Angle: " << arg.sync_angle << '\n';
+  os << "Cut Angle: " << arg.cut_angle << '\n';
   os << "FoV Start: " << arg.cloud_min_angle << '\n';
   os << "FoV End: " << arg.cloud_max_angle << '\n';
   os << "Dual Return Distance Threshold: " << arg.dual_return_distance_threshold << '\n';
+  os << "Calibration Path: " << arg.calibration_path << '\n';
   os << "PTP Profile: " << arg.ptp_profile << '\n';
   os << "PTP Domain: " << std::to_string(arg.ptp_domain) << '\n';
   os << "PTP Transport Type: " << arg.ptp_transport_type << '\n';
@@ -76,6 +81,8 @@ struct HesaiCalibrationConfigurationBase : public CalibrationConfigurationBase
   virtual nebula::Status LoadFromFile(const std::string & calibration_file) = 0;
   virtual nebula::Status SaveToFileFromBytes(
     const std::string & calibration_file, const std::vector<uint8_t> & buf) = 0;
+
+  [[nodiscard]] virtual std::tuple<float, float> getFovPadding() const = 0;
 };
 
 /// @brief struct for Hesai calibration configuration
@@ -177,6 +184,19 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase
     ofs.close();
     return Status::OK;
   }
+
+  [[nodiscard]] std::tuple<float, float> getFovPadding() const override
+  {
+    float min = INFINITY;
+    float max = -INFINITY;
+
+    for (const auto & item : azimuth_offset_map) {
+      min = std::min(min, item.second);
+      max = std::max(max, item.second);
+    }
+
+    return {-max, -min};
+  }
 };
 
 /// @brief struct for Hesai correction configuration (for AT)
@@ -360,7 +380,7 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase
   /// @param ch The channel id
   /// @param azi The precision azimuth in (0.01 / 256) degree unit
   /// @return The azimuth adjustment in 0.01 degree unit
-  int8_t getAzimuthAdjustV3(uint8_t ch, uint32_t azi) const
+  [[nodiscard]] int8_t getAzimuthAdjustV3(uint8_t ch, uint32_t azi) const
   {
     unsigned int i = std::floor(1.f * azi / STEP3);
     unsigned int l = azi - i * STEP3;
@@ -372,13 +392,23 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase
   /// @param ch The channel id
   /// @param azi The precision azimuth in (0.01 / 256) degree unit
   /// @return The elevation adjustment in 0.01 degree unit
-  int8_t getElevationAdjustV3(uint8_t ch, uint32_t azi) const
+  [[nodiscard]] int8_t getElevationAdjustV3(uint8_t ch, uint32_t azi) const
   {
     unsigned int i = std::floor(1.f * azi / STEP3);
     unsigned int l = azi - i * STEP3;
     float k = 1.f * l / STEP3;
     return round((1 - k) * elevationOffset[ch * 180 + i] + k * elevationOffset[ch * 180 + i + 1]);
   }
+
+  [[nodiscard]] std::tuple<float, float> getFovPadding() const override
+  {
+    // TODO(mojomex): calculate instead of hard-coding
+    // The reason this is tricky is that an upper bound over all azimuth/elevation combinations has
+    // to be found. For other sensors, this is only a function of elevation, so the search space is
+    // tiny compared to AT128. We should be able to find an upper bound of `getAzimuthAdjustV3` but
+    // I have not invested the time for now.
+    return {-5, 5};
+  }
 };
 
 /*

nebula_decoders/include/nebula_decoders/nebula_decoders_common/angles.hpp

@@ -0,0 +1,50 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <cmath>
+namespace nebula::drivers
+{
+
+/**
+ * @brief Tests if `angle` is in the region of the circle defined by `start_angle` and `end_angle`.
+ * Notably, `end_angle` can be smaller than `start_angle`, in which case the region passes over the
+ * 360/0 deg bound. This function is unit-independent (but all angles have to have the same unit),
+ * so degrees, radians, and arbitrary scale factors can be used.
+ */
+template <typename T>
+bool angle_is_between(
+  T start_angle, T end_angle, T angle, bool start_inclusive = true, bool end_inclusive = true)
+{
+  if (!start_inclusive && angle == start_angle) return false;
+  if (!end_inclusive && angle == end_angle) return false;
+
+  return (start_angle <= angle && angle <= end_angle) ||
+         ((end_angle < start_angle) && (angle <= end_angle || start_angle <= angle));
+}
+
+/**
+ * @brief Normalizes an angle to the interval [0; max_angle]. This function is unit-independent.
+ * `max_angle` is 360 for degrees, 2 * M_PI for radians, and the corresponding scaled value for
+ * scaled units such as centi-degrees (36000).
+ */
+template <typename T>
+T normalize_angle(T angle, T max_angle)
+{
+  T factor = std::floor((1.0 * angle) / max_angle);
+  return angle - (factor * max_angle);
+}
+
+}  // namespace nebula::drivers

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

@@ -47,15 +47,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;
+  virtual bool passedEmitAngle(uint32_t last_azimuth, uint32_t current_azimuth) = 0;
+  virtual bool passedTimestampResetAngle(uint32_t last_azimuth, uint32_t current_azimuth) = 0;
+  virtual bool isInsideFoV(uint32_t last_azimuth, uint32_t current_azimuth) = 0;
+  virtual bool isInsideOverlap(uint32_t last_azimuth, uint32_t current_azimuth) = 0;
 };
 
 }  // namespace nebula::drivers

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

@@ -15,10 +15,18 @@
 #pragma once
 
 #include "nebula_common/hesai/hesai_common.hpp"
+#include "nebula_decoders/nebula_decoders_common/angles.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp"
 
+#include <nebula_common/nebula_common.hpp>
+
+#include <algorithm>
+#include <cmath>
 #include <cstdint>
 #include <memory>
+#include <optional>
+#include <ostream>
+#include <utility>
 
 namespace nebula::drivers
 {
@@ -27,38 +35,96 @@ template <size_t ChannelN, size_t AngleUnit>
 class AngleCorrectorCalibrationBased : public AngleCorrector<HesaiCalibrationConfiguration>
 {
 private:
-  static constexpr size_t MAX_AZIMUTH_LEN = 360 * AngleUnit;
+  static constexpr size_t MAX_AZIMUTH = 360 * AngleUnit;
 
   std::array<float, ChannelN> elevation_angle_rad_{};
   std::array<float, ChannelN> azimuth_offset_rad_{};
-  std::array<float, MAX_AZIMUTH_LEN> block_azimuth_rad_{};
+  std::array<float, MAX_AZIMUTH> block_azimuth_rad_{};
 
   std::array<float, ChannelN> elevation_cos_{};
   std::array<float, ChannelN> elevation_sin_{};
-  std::array<std::array<float, ChannelN>, MAX_AZIMUTH_LEN> azimuth_cos_{};
-  std::array<std::array<float, ChannelN>, MAX_AZIMUTH_LEN> azimuth_sin_{};
+  std::array<std::array<float, ChannelN>, MAX_AZIMUTH> azimuth_cos_{};
+  std::array<std::array<float, ChannelN>, MAX_AZIMUTH> azimuth_sin_{};
 
 public:
+  uint32_t emit_angle_raw_;
+  uint32_t timestamp_reset_angle_raw_;
+  uint32_t fov_start_raw_;
+  uint32_t fov_end_raw_;
+
+  bool is_360_;
+
   explicit AngleCorrectorCalibrationBased(
-    const std::shared_ptr<const HesaiCalibrationConfiguration> & sensor_calibration)
+    const std::shared_ptr<const HesaiCalibrationConfiguration> & sensor_calibration,
+    double fov_start_azimuth_deg, double fov_end_azimuth_deg, double scan_cut_azimuth_deg)
   {
     if (sensor_calibration == nullptr) {
       throw std::runtime_error(
         "Cannot instantiate AngleCorrectorCalibrationBased without calibration data");
     }
 
+    // ////////////////////////////////////////
+    // Elevation lookup tables
+    // ////////////////////////////////////////
+
+    int32_t correction_min = INT32_MAX;
+    int32_t correction_max = INT32_MIN;
+
+    auto round_away_from_zero = [](float value) {
+      return (value < 0) ? std::floor(value) : std::ceil(value);
+    };
+
     for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) {
       float elevation_angle_deg = sensor_calibration->elev_angle_map.at(channel_id);
       float azimuth_offset_deg = sensor_calibration->azimuth_offset_map.at(channel_id);
 
+      int32_t azimuth_offset_raw = round_away_from_zero(azimuth_offset_deg * AngleUnit);
+      correction_min = std::min(correction_min, azimuth_offset_raw);
+      correction_max = std::max(correction_max, azimuth_offset_raw);
+
       elevation_angle_rad_[channel_id] = deg2rad(elevation_angle_deg);
       azimuth_offset_rad_[channel_id] = deg2rad(azimuth_offset_deg);
 
       elevation_cos_[channel_id] = cosf(elevation_angle_rad_[channel_id]);
       elevation_sin_[channel_id] = sinf(elevation_angle_rad_[channel_id]);
     }
 
-    for (size_t block_azimuth = 0; block_azimuth < MAX_AZIMUTH_LEN; block_azimuth++) {
+    // ////////////////////////////////////////
+    // Raw azimuth threshold angles
+    // ////////////////////////////////////////
+
+    int32_t emit_angle_raw = std::ceil(scan_cut_azimuth_deg * AngleUnit);
+    emit_angle_raw -= correction_min;
+    emit_angle_raw_ = normalize_angle<int32_t>(emit_angle_raw, MAX_AZIMUTH);
+
+    int32_t fov_start_raw = std::floor(fov_start_azimuth_deg * AngleUnit);
+    fov_start_raw -= correction_max;
+    fov_start_raw_ = normalize_angle<int32_t>(fov_start_raw, MAX_AZIMUTH);
+
+    int32_t fov_end_raw = std::ceil(fov_end_azimuth_deg * AngleUnit);
+    fov_end_raw -= correction_min;
+    fov_end_raw_ = normalize_angle<int32_t>(fov_end_raw, MAX_AZIMUTH);
+
+    // Reset timestamp on FoV start if FoV < 360 deg and scan is cut at FoV end.
+    // Otherwise, reset timestamp on publish
+    is_360_ =
+      normalize_angle(fov_start_azimuth_deg, 360.) == normalize_angle(fov_end_azimuth_deg, 360.);
+    bool reset_timestamp_on_publish = is_360_ || (normalize_angle(fov_end_azimuth_deg, 360.) !=
+                                                  normalize_angle(scan_cut_azimuth_deg, 360.));
+
+    if (reset_timestamp_on_publish) {
+      int32_t timestamp_reset_angle_raw = std::floor(scan_cut_azimuth_deg * AngleUnit);
+      timestamp_reset_angle_raw -= correction_max;
+      timestamp_reset_angle_raw_ = normalize_angle<int32_t>(timestamp_reset_angle_raw, MAX_AZIMUTH);
+    } else {
+      timestamp_reset_angle_raw_ = fov_start_raw_;
+    }
+
+    // ////////////////////////////////////////
+    // Azimuth lookup tables
+    // ////////////////////////////////////////
+
+    for (size_t block_azimuth = 0; block_azimuth < MAX_AZIMUTH; block_azimuth++) {
       block_azimuth_rad_[block_azimuth] = deg2rad(block_azimuth / static_cast<double>(AngleUnit));
 
       for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) {
@@ -74,6 +140,8 @@ class AngleCorrectorCalibrationBased : public AngleCorrector<HesaiCalibrationCon
   CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override
   {
     float azimuth_rad = block_azimuth_rad_[block_azimuth] + azimuth_offset_rad_[channel_id];
+    azimuth_rad = normalize_angle(azimuth_rad, M_PIf * 2);
+
     float elevation_rad = elevation_angle_rad_[channel_id];
 
     return {
@@ -85,15 +153,27 @@ 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 passedEmitAngle(uint32_t last_azimuth, 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 angle_is_between(last_azimuth, current_azimuth, emit_angle_raw_, false);
+  }
+
+  bool passedTimestampResetAngle(uint32_t last_azimuth, uint32_t current_azimuth) override
+  {
+    return angle_is_between(last_azimuth, current_azimuth, timestamp_reset_angle_raw_, false);
+  }
 
-    return current_diff_from_sync < last_diff_from_sync;
+  bool isInsideFoV(uint32_t last_azimuth, uint32_t current_azimuth) override
+  {
+    if (is_360_) return true;
+    return angle_is_between(fov_start_raw_, fov_end_raw_, current_azimuth) ||
+           angle_is_between(timestamp_reset_angle_raw_, emit_angle_raw_, last_azimuth);
+  }
+
+  bool isInsideOverlap(uint32_t last_azimuth, uint32_t current_azimuth) override
+  {
+    return angle_is_between(timestamp_reset_angle_raw_, emit_angle_raw_, current_azimuth) ||
+           angle_is_between(timestamp_reset_angle_raw_, emit_angle_raw_, last_azimuth);
   }
 };