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);
  }
 };