Refactor unit tests, add debug counters to HesaiDecoder

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -31,7 +31,7 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con
 {
   os << (SensorConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port
      << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed
-     << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle;
+     << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold;
   return os;
 }
 

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

@@ -22,7 +22,7 @@ class AngleCorrector
   {
     std::array<float, LookupTableSize> lut{};
     for (uint32_t i = 0; i < LookupTableSize; i++) {
-      float angle_rad = i / static_cast<float>(LookupTableSize) * 2 * M_PI;
+      float angle_rad = i / static_cast<float>(LookupTableSize) * 2.f * M_PI;
       lut[i] = is_cos ? cosf(angle_rad) : sinf(angle_rad);
     }
     return lut;

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

@@ -54,24 +54,23 @@ class AngleCorrectorCalibrationBased : public AngleCorrector<1000 * 360>
   std::tuple<uint32_t, uint32_t, float, float> getCorrectedAzimuthAndElevation(
     uint32_t block_azimuth, uint32_t channel_id) override
   {
-    int32_t corrected_azimuth = block_azimuth * INTERNAL_RESOLUTION_PER_DEG / AngleUnit;
+    int32_t corrected_azimuth = block_azimuth * (INTERNAL_RESOLUTION_PER_DEG / AngleUnit);
     corrected_azimuth += azimuth_offset_[channel_id];
-    if (corrected_azimuth < 0) {
-      corrected_azimuth += INTERNAL_MAX_AZIMUTH_LENGTH;
-    } else if (corrected_azimuth >= INTERNAL_MAX_AZIMUTH_LENGTH) {
-      corrected_azimuth -= INTERNAL_MAX_AZIMUTH_LENGTH;
-    }
+    corrected_azimuth = (corrected_azimuth + INTERNAL_MAX_AZIMUTH_LENGTH) % INTERNAL_MAX_AZIMUTH_LENGTH;
 
     int32_t corrected_elevation = elevation_angle_[channel_id];
-    if (corrected_elevation < 0) {
-      corrected_elevation += INTERNAL_MAX_AZIMUTH_LENGTH;
-    } else if (corrected_elevation >= INTERNAL_MAX_AZIMUTH_LENGTH) {
-      corrected_elevation -= INTERNAL_MAX_AZIMUTH_LENGTH;
+    corrected_elevation = (corrected_elevation + INTERNAL_MAX_AZIMUTH_LENGTH) % INTERNAL_MAX_AZIMUTH_LENGTH;
+
+    float azimuth_rad = block_azimuth_rad_[block_azimuth] + azimuth_offset_rad_[channel_id];
+    if (azimuth_rad > 2 * M_PI) {
+      azimuth_rad -= 2 * M_PI;
+    } else if (azimuth_rad < 0) {
+      azimuth_rad += 2 * M_PI;
     }
 
     return std::make_tuple(
       static_cast<uint32_t>(corrected_azimuth), static_cast<uint32_t>(corrected_elevation),
-      block_azimuth_rad_[block_azimuth] + azimuth_offset_rad_[channel_id],
+      azimuth_rad,
       elevation_angle_rad_[channel_id]);
   }
 

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

@@ -12,6 +12,37 @@ namespace nebula
 {
 namespace drivers
 {
+
+struct DebugCounters
+{
+  uint64_t points_excluded_distance_zero;
+  uint64_t points_excluded_distance_range;
+  uint64_t points_excluded_multi_return_identical;
+  uint64_t points_excluded_multi_return_close;
+  uint64_t points_appended;
+  float min_azimuth;
+  float max_azimuth;
+  size_t min_azimuth_idx;
+  size_t max_azimuth_idx;
+
+  DebugCounters()
+  {
+    reset();
+  }
+
+  void reset() {
+    points_excluded_distance_zero = 0;
+    points_excluded_distance_range = 0;
+    points_excluded_multi_return_identical = 0;
+    points_excluded_multi_return_close = 0;
+    points_appended = 0;
+    min_azimuth = MAXFLOAT;
+    max_azimuth = -MAXFLOAT;
+    min_azimuth_idx = -1UL;
+    max_azimuth_idx = 0;
+  }
+};
+
 template <typename SensorT>
 class HesaiDecoder : public HesaiScanDecoder
 {
@@ -40,6 +71,7 @@ class HesaiDecoder : public HesaiScanDecoder
   bool has_scanned_;
 
   rclcpp::Logger logger_;
+  DebugCounters debug_counters_;
 
   /// @brief For each channel, its firing offset relative to the block in nanoseconds
   std::array<int, SensorT::packet_t::N_CHANNELS> channel_firing_offset_ns_;
@@ -79,33 +111,62 @@ class HesaiDecoder : public HesaiScanDecoder
     uint64_t packet_timestamp_ns = hesai_packet::get_timestamp_ns(packet_);
     uint32_t raw_azimuth = packet_.body.blocks[start_block_id].get_azimuth();
 
+    for (size_t block_id = start_block_id + 1; block_id < start_block_id + n_blocks; ++block_id) {
+      if (raw_azimuth != packet_.body.blocks[block_id].get_azimuth()) {
+        RCLCPP_ERROR(
+          logger_, "Azimuth mismatch for %d-return: %d != %d", n_blocks, raw_azimuth,
+          packet_.body.blocks[block_id].get_azimuth());
+        return;
+      }
+    }
+
     for (size_t channel_id = 0; channel_id < SensorT::packet_t::N_CHANNELS; ++channel_id) {
-      typename SensorT::packet_t::body_t::block_t::unit_t * return_units[n_blocks];
+      std::vector<typename SensorT::packet_t::body_t::block_t::unit_t *> return_units(n_blocks);
 
-      for (size_t block_id = start_block_id; block_id < start_block_id + n_blocks; ++block_id) {
-        return_units[block_id - start_block_id] = &packet_.body.blocks[block_id].units[channel_id];
+      for (size_t block_offset = 0; block_offset < n_blocks; ++block_offset) {
+        return_units[block_offset] = &packet_.body.blocks[block_offset + start_block_id].units[channel_id];
       }
 
-      for (size_t block_id = start_block_id; block_id < start_block_id + n_blocks; ++block_id) {
-        auto & unit = *return_units[block_id - start_block_id];
+      for (size_t block_offset = 0; block_offset < n_blocks; ++block_offset) {
+        auto & unit = *return_units[block_offset];
+
+        if (unit.distance == 0) {
+          debug_counters_.points_excluded_distance_zero++;
+          continue;
+        }
 
         auto distance = getDistance(unit);
-        if (
-          distance < sensor_configuration_->min_range ||
-          distance > sensor_configuration_->max_range) {
+
+        if (distance < SensorT::MIN_RANGE || distance > SensorT::MAX_RANGE) {
+          debug_counters_.points_excluded_distance_range++;
           continue;
         }
 
-        // TODO(mojomex) handle dual_return_distance_threshold_
+        auto return_type = sensor_.getReturnType(
+          static_cast<hesai_packet::return_mode::ReturnMode>(packet_.tail.return_mode),
+          block_offset, return_units);
+
+
+        // Keep only first of multiple identical points
+        if (return_type == ReturnType::IDENTICAL && block_offset != 0) {
+          debug_counters_.points_excluded_multi_return_identical++;
+          continue;
+        }
+
+        // Keep only first (if any) of multiple points that are too close
+        for (size_t return_idx = 0; return_idx < block_offset; ++return_idx) {
+          if (fabsf(getDistance(*return_units[return_idx]) - distance) < sensor_configuration_->dual_return_distance_threshold) {
+            debug_counters_.points_excluded_multi_return_close++;
+            continue;
+          }
+        }
 
         NebulaPoint point;
         point.distance = distance;
         point.intensity = unit.reflectivity;
         // TODO(mojomex) add header offset to scan offset correction
-        point.time_stamp = getPointTimeRelative(packet_timestamp_ns, block_id, channel_id);
+        point.time_stamp = getPointTimeRelative(packet_timestamp_ns, block_offset + start_block_id, channel_id);
 
-        auto return_type =
-          sensor_.getReturnType(static_cast<hesai_packet::return_mode::ReturnMode>(packet_.tail.return_mode), block_id - start_block_id, return_units);
         point.return_type = static_cast<uint8_t>(return_type);
         point.channel = channel_id;
 
@@ -115,10 +176,27 @@ class HesaiDecoder : public HesaiScanDecoder
         point.x = xyDistance * angle_corrector_.sin_map_[azimuth_idx];
         point.y = xyDistance * angle_corrector_.cos_map_[azimuth_idx];
         point.z = distance * angle_corrector_.sin_map_[elevation_idx];
+
+        // The driver wrapper converts to degrees, expects radians
         point.azimuth = azimuth_rad;
         point.elevation = elevation_rad;
 
+        if (point.azimuth < debug_counters_.min_azimuth) {
+          debug_counters_.min_azimuth = point.azimuth;
+        }
+        if (point.azimuth > debug_counters_.max_azimuth) {
+          debug_counters_.max_azimuth = point.azimuth;
+        }
+
+        if (azimuth_idx < debug_counters_.min_azimuth_idx) {
+          debug_counters_.min_azimuth_idx = azimuth_idx;
+        }
+        if (azimuth_idx > debug_counters_.max_azimuth_idx) {
+          debug_counters_.max_azimuth_idx = azimuth_idx;
+        }
+
         // NDDUMP(_(point.x), _(point.y), _(point.z));
+        debug_counters_.points_appended++;
         decode_pc_->emplace_back(point);
       }
     }
@@ -132,10 +210,10 @@ class HesaiDecoder : public HesaiScanDecoder
     return angle_corrector_.hasScanned(current_phase, last_phase_);
   }
 
-  /// @brief Get the distance of the given unit in millimeters
+  /// @brief Get the distance of the given unit in meters
   float getDistance(typename SensorT::packet_t::body_t::block_t::unit_t & unit)
   {
-    return unit.distance * hesai_packet::get_dis_unit(packet_) / 1000.f;
+    return unit.distance * hesai_packet::get_dis_unit(packet_);
   }
 
   /// @brief Get timestamp of point in nanoseconds, relative to scan timestamp. Includes firing time
@@ -145,7 +223,8 @@ class HesaiDecoder : public HesaiScanDecoder
   /// @param channel_id The channel index of the point
   uint32_t getPointTimeRelative(uint64_t packet_timestamp_ns, size_t block_id, size_t channel_id)
   {
-    auto point_to_packet_offset_ns = sensor_.getPacketRelativePointTimeOffset(block_id, channel_id, packet_);
+    auto point_to_packet_offset_ns =
+      sensor_.getPacketRelativePointTimeOffset(block_id, channel_id, packet_);
     auto packet_to_scan_offset_ns = static_cast<uint32_t>(packet_timestamp_ns - scan_timestamp_ns_);
     return packet_to_scan_offset_ns + point_to_packet_offset_ns;
   }
@@ -166,10 +245,12 @@ class HesaiDecoder : public HesaiScanDecoder
     logger_(rclcpp::get_logger("HesaiDecoder"))
   {
     logger_.set_level(rclcpp::Logger::Level::Debug);
-    RCLCPP_DEBUG(logger_, "Hi from HesaiDecoder");
+    RCLCPP_INFO_STREAM(logger_, sensor_configuration_);
 
     decode_pc_.reset(new NebulaPointCloud);
     output_pc_.reset(new NebulaPointCloud);
+
+    // TODO(mojomex) reserve n_points_per_scan * max_reurns points in buffers
   }
 
   int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) override
@@ -195,23 +276,39 @@ class HesaiDecoder : public HesaiScanDecoder
     for (size_t block_id = 0; block_id < SensorT::packet_t::N_BLOCKS; block_id += n_returns) {
       // FIXME(mojomex) respect scan phase
       current_azimuth =
-        packet_.body.blocks[block_id].get_azimuth() -
-        static_cast<int>(
-          sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS);
+        (360 * SensorT::packet_t::DEGREE_SUBDIVISIONS +
+         packet_.body.blocks[block_id].get_azimuth() -
+         static_cast<int>(
+           sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS)) %
+        (360 * SensorT::packet_t::DEGREE_SUBDIVISIONS);
 
       bool scan_completed = checkScanCompleted(current_azimuth);
       if (scan_completed) {
+        {
+          RCLCPP_DEBUG_STREAM(
+            logger_,
+            "decode: " << decode_pc_->size() << " | output: " << output_pc_->size()
+                       << " | points_excl_zero: " << debug_counters_.points_excluded_distance_zero
+                       << " | points_excl_range: " << debug_counters_.points_excluded_distance_range
+                       << " | points_excl_identical: " << debug_counters_.points_excluded_multi_return_identical
+                       << " | points_excl_close: " << debug_counters_.points_excluded_multi_return_close
+                       << " | points_appended: " << debug_counters_.points_appended
+                       << " | min_azimuth: " << debug_counters_.min_azimuth
+                       << " | max_azimuth: " << debug_counters_.max_azimuth
+                       << " | min_azimuth_idx: " << debug_counters_.min_azimuth_idx
+                       << " | max_azimuth_idx: " << debug_counters_.max_azimuth_idx
+                       << " | current_azimuth: " << current_azimuth
+                       << " | last_phase: " << last_phase_);
+          debug_counters_.reset();
+        }
+
         std::swap(decode_pc_, output_pc_);
         decode_pc_->clear();
         has_scanned_ = true;
       }
 
       convertReturns(block_id, n_returns);
       last_phase_ = current_azimuth;
-      // NDDUMP(
-      //   _(n_returns), _(block_id), _(last_phase_), _(current_azimuth), _(scan_completed),
-      //   _(decode_pc_->size()),
-      //   _(output_pc_->size()));
     }
 
     return last_phase_;

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp

@@ -221,14 +221,15 @@ uint64_t get_timestamp_ns(const PacketT & packet)
   return packet.tail.date_time.get_seconds() * 1000000000 + packet.tail.timestamp * 1000;
 }
 
-/// @brief Get the distance unit of the given packet type in millimeters
+/// @brief Get the distance unit of the given packet type in meters. Distance values in the packet, multiplied by this value, yield the distance in meters.
 /// @tparam PacketT The packet type
 /// @param packet The packet to get the distance unit from
-/// @return The distance unit in millimeters
+/// @return The distance unit in meters
 template <typename PacketT>
-uint8_t get_dis_unit(const PacketT & packet)
+float get_dis_unit(const PacketT & packet)
 {
-  return packet.header.dis_unit;
+  // Packets define distance unit in millimeters, convert to meters here
+  return packet.header.dis_unit / 1000.f;
 }
 
 }  // namespace hesai_packet

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp

@@ -55,12 +55,12 @@ class HesaiSensor
   /// @return The return type of the point
   virtual ReturnType getReturnType(
     hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx,
-    typename packet_t::body_t::block_t::unit_t ** return_units)
+    std::vector<typename packet_t::body_t::block_t::unit_t *> return_units)
   {
-    unsigned int n_returns = hesai_packet::get_n_returns(return_mode);
+    unsigned int n_returns = return_units.size();
 
     const auto is_strongest = [&]() {
-      for (int i = 0; i < n_returns; ++i) {
+      for (unsigned int i = 0; i < n_returns; ++i) {
         if (i == return_idx) {
           continue;
         }
@@ -74,7 +74,7 @@ class HesaiSensor
     };
 
     const auto is_duplicate = [&]() {
-      for (int i = 0; i < n_returns; ++i) {
+      for (unsigned int i = 0; i < n_returns; ++i) {
         if (i == return_idx) {
           continue;
         }
@@ -89,7 +89,7 @@ class HesaiSensor
       return false;
     };
 
-    if (n_returns != 1 && return_idx != 0 && is_duplicate()) {
+    if (n_returns != 1 && is_duplicate()) {
       return ReturnType::IDENTICAL;
     }
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp

@@ -204,6 +204,9 @@ class Pandar128E3X : public HesaiSensor<hesai_packet::Packet128E3X>
     -1,    -1, 34634, -1, -1, 1541, -1,    29319};
 
 public:
+  static constexpr float MIN_RANGE = 0.1;
+  static constexpr float MAX_RANGE = 230.0;
+
   int getPacketRelativePointTimeOffset(
     uint32_t block_id, uint32_t channel_id, const packet_t & packet)
   {
@@ -213,7 +216,7 @@ class Pandar128E3X : public HesaiSensor<hesai_packet::Packet128E3X>
     int channel_offset_ns;
     bool is_hires_mode = packet.tail.operational_state == OperationalState::HIGH_RESOLUTION;
     bool is_nearfield = (hesai_packet::get_dis_unit(packet) *
-                         packet.body.blocks[block_id].units[channel_id].distance / 1000.f) <= 2.85f;
+                         packet.body.blocks[block_id].units[channel_id].distance) <= 2.85f;
     auto azimuth_state = packet.tail.geAzimuthState(block_id);
 
     if (is_hires_mode && azimuth_state == 0 && !is_nearfield)
@@ -241,14 +244,15 @@ class Pandar128E3X : public HesaiSensor<hesai_packet::Packet128E3X>
     else if (!is_hires_mode && azimuth_state == 1 && is_nearfield)
       channel_offset_ns = standard_as1_near_offset_ns_[channel_id];
     else
-      throw std::runtime_error("Invalid combination of operational state and azimuth state and nearfield firing");
+      throw std::runtime_error(
+        "Invalid combination of operational state and azimuth state and nearfield firing");
 
     return block_offset_ns + channel_offset_ns;
   }
 
   ReturnType getReturnType(
     hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx,
-    typename packet_t::body_t::block_t::unit_t ** return_units) override
+    std::vector<typename packet_t::body_t::block_t::unit_t *> return_units) override
   {
     auto return_type = HesaiSensor<packet_t>::getReturnType(return_mode, return_idx, return_units);
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp

@@ -63,6 +63,9 @@ class Pandar128E4X : public HesaiSensor<hesai_packet::Packet128E4X>
     -1,    -1,    -1,    21980, 15446, 8912,  2378,  -1,    -1,    -1,    -1};
 
 public:
+  static constexpr float MIN_RANGE = 0.1;
+  static constexpr float MAX_RANGE = 230.0;
+
   int getPacketRelativePointTimeOffset(
     uint32_t block_id, uint32_t channel_id, const packet_t & packet)
   {
@@ -93,7 +96,7 @@ class Pandar128E4X : public HesaiSensor<hesai_packet::Packet128E4X>
 
   ReturnType getReturnType(
     hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx,
-    typename packet_t::body_t::block_t::unit_t ** return_units) override
+    std::vector<typename packet_t::body_t::block_t::unit_t *> return_units) override
   {
     auto return_type = HesaiSensor<packet_t>::getReturnType(return_mode, return_idx, return_units);