fix(nebula_decoders_hesai) unify pointcloud order and filtering of all sensors

.cspell.json

@@ -3,27 +3,28 @@
   "language": "en",
   "allowCompoundWords": true,
   "words": [
+    "adctp",
+    "Adctp",
+    "AT",
     "block_id",
-    "UDP_SEQ",
-    "STD_COUT",
-    "PANDARQT",
-    "PANDARXT",
-    "PANDARAT",
+    "gprmc",
+    "HESAI",
+    "nohup",
+    "nproc",
     "pandar",
     "PANDAR",
-    "AT",
-    "XT",
-    "QT",
-    "XTM",
+    "PANDARAT",
+    "PANDARQT",
+    "PANDARXT",
     "Pdelay",
-    "gprmc",
-    "Vccint",
-    "vccint",
-    "adctp",
-    "Adctp",
+    "QT",
+    "schedutil",
+    "STD_COUT",
     "stds",
-    "nproc",
-    "nohup",
-    "schedutil"
+    "UDP_SEQ",
+    "vccint",
+    "Vccint",
+    "XT",
+    "XTM"
   ]
 }

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 << ", DualReturnThreshold: " << arg.dual_return_distance_threshold;
   return os;
 }
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp

@@ -29,7 +29,7 @@ constexpr size_t INFO_SIZE =
 constexpr size_t UTC_TIME = 6;
 constexpr size_t PACKET_SIZE = BLOCK_SIZE * BLOCKS_PER_PACKET + INFO_SIZE + UTC_TIME;
 constexpr size_t SEQ_NUM_SIZE = 4;
-constexpr float LASER_RETURN_TO_DISTANCE_RATE = 0.004;
+constexpr double LASER_RETURN_TO_DISTANCE_RATE = 0.004;
 constexpr uint32_t STRONGEST_RETURN = 0x37;
 constexpr uint32_t LAST_RETURN = 0x38;
 constexpr uint32_t DUAL_RETURN = 0x39;

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40_decoder.hpp

@@ -64,7 +64,6 @@ class Pandar40Decoder : public HesaiScanDecoder
   std::array<float, MAX_AZIMUTH_STEPS> block_azimuth_rad_{};
 
   std::array<float, LASER_COUNT> firing_time_offset_{};
-  std::array<size_t, LASER_COUNT> vertical_laser_firing_order_{};
 
   std::array<float, BLOCKS_PER_PACKET> block_time_offset_single_return_{};
   std::array<float, BLOCKS_PER_PACKET> block_time_offset_dual_return_{};

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt.hpp

@@ -45,6 +45,8 @@ constexpr size_t PACKET_SIZE = HEAD_SIZE + BODY_SIZE + PACKET_TAIL_SIZE;
 constexpr uint32_t STRONGEST_RETURN = 0x37;
 constexpr uint32_t LAST_RETURN = 0x38;
 constexpr uint32_t DUAL_RETURN = 0x39;
+constexpr uint32_t DUAL_RETURN_B = 0x3B;
+constexpr uint32_t DUAL_RETURN_C = 0x3C;
 constexpr uint32_t MAX_AZIMUTH_STEPS = 360 * 100;  // Unit: 0.01°
 
 struct Header

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xtm_decoder.hpp

@@ -109,9 +109,6 @@ class PandarXTMDecoder : public HesaiScanDecoder
   std::vector<float> sin_elevation_angle_;
   std::vector<float> cos_elevation_angle_;
 
-  std::vector<float> sin_azimuth_angle_;
-  std::vector<float> cos_azimuth_angle_;
-
   Packet packet_{};
 
   uint16_t last_azimuth_;

nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_128_e4x_decoder.cpp

@@ -199,7 +199,7 @@ drivers::NebulaPointCloudPtr Pandar128E4XDecoder::convert_dual([[maybe_unused]]
       packet_.body.block_02[i], i, packet_.body.azimuth_2, current_unit_unix_second_, pt2_distance);
     block1_pt.return_type = first_return_type_;
     block_pc->points.emplace_back(block1_pt);
-    if (fabsf(pt1_distance - pt2_distance) > dual_return_distance_threshold_) {
+    if (fabsf(pt1_distance - pt2_distance) >= dual_return_distance_threshold_) {
       block2_pt.return_type = second_return_type_;
       block_pc->points.emplace_back(block2_pt);
     }

nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_40_decoder.cpp

@@ -18,10 +18,6 @@ Pandar40Decoder::Pandar40Decoder(
   sensor_configuration_ = sensor_configuration;
   sensor_calibration_ = calibration_configuration;
 
-  vertical_laser_firing_order_ = {7,  19, 14, 26, 6,  18, 4,  32, 36, 0,  10, 22, 17, 29,
-                                  9,  21, 5,  33, 37, 1,  13, 25, 20, 30, 12, 8,  24, 34,
-                                  38, 2,  16, 28, 23, 31, 15, 11, 27, 35, 39, 3};
-
   firing_time_offset_ = {42.22, 28.47, 16.04, 3.62,  45.49, 31.74, 47.46, 54.67, 20.62, 33.71,
                          40.91, 8.19,  20.62, 27.16, 50.73, 8.19,  14.74, 36.98, 45.49, 52.7,
                          23.89, 31.74, 38.95, 11.47, 18.65, 25.19, 48.76, 6.23,  12.77, 35.01,
@@ -152,7 +148,13 @@ drivers::NebulaPointCloudPtr Pandar40Decoder::convert(size_t block_id)
 {
   drivers::NebulaPointCloudPtr block_pc(new NebulaPointCloud);
 
-  for (auto unit_id : vertical_laser_firing_order_) {
+  for (size_t unit_id = 0; unit_id < LASER_COUNT; ++unit_id) {
+    const auto & unit = packet_.blocks[block_id].units[unit_id];
+
+    if (unit.distance < MIN_RANGE || unit.distance > MAX_RANGE) {
+      continue;
+    }
+
     block_pc->points.emplace_back(build_point(
       block_id, unit_id,
       (packet_.return_mode == STRONGEST_RETURN)
@@ -179,7 +181,7 @@ drivers::NebulaPointCloudPtr Pandar40Decoder::convert_dual(size_t block_id)
   const auto & odd_block = packet_.blocks[odd_block_id];
   //  auto sensor_return_mode = sensor_configuration_->return_mode;
 
-  for (auto unit_id : vertical_laser_firing_order_) {
+  for (size_t unit_id = 0; unit_id < LASER_COUNT; ++unit_id) {
     const auto & even_unit = even_block.units[unit_id];
     const auto & odd_unit = odd_block.units[unit_id];
 
@@ -192,38 +194,38 @@ drivers::NebulaPointCloudPtr Pandar40Decoder::convert_dual(size_t block_id)
     // If the two returns are too close, only return the last one
     if (
       (abs(even_unit.distance - odd_unit.distance) < dual_return_distance_threshold_) &&
-      even_usable) {
+      odd_usable) {
       block_pc->emplace_back(build_point(
-        even_block_id, unit_id,
+        odd_block_id, unit_id,
         static_cast<uint8_t>(drivers::ReturnType::IDENTICAL)));  // drivers::ReturnMode::DUAL_ONLY
     } else if (even_unit.intensity >= odd_unit.intensity) {
       // Strongest return is in even block when it is also the last
+      if (even_usable) {
+        block_pc->emplace_back(build_point(
+          even_block_id, unit_id,
+          static_cast<uint8_t>(
+            drivers::ReturnType::STRONGEST)));  // drivers::ReturnMode::DUAL_STRONGEST_LAST
+      }
       if (odd_usable) {
         block_pc->emplace_back(build_point(
           odd_block_id, unit_id,
           static_cast<uint8_t>(
             drivers::ReturnType::FIRST_WEAK)));  // drivers::ReturnMode::DUAL_WEAK_FIRST
       }
+    } else {
       if (even_usable) {
         block_pc->emplace_back(build_point(
           even_block_id, unit_id,
           static_cast<uint8_t>(
-            drivers::ReturnType::STRONGEST)));  // drivers::ReturnMode::DUAL_STRONGEST_LAST
+            drivers::ReturnType::LAST_WEAK)));  // drivers::ReturnMode::DUAL_WEAK_LAST
       }
-    } else {
       // Normally, strongest return is in odd block and last return is in even block
       if (odd_usable) {
         block_pc->emplace_back(build_point(
           odd_block_id, unit_id,
           static_cast<uint8_t>(
             drivers::ReturnType::STRONGEST)));  // drivers::ReturnMode::DUAL_STRONGEST_FIRST
       }
-      if (even_usable) {
-        block_pc->emplace_back(build_point(
-          even_block_id, unit_id,
-          static_cast<uint8_t>(
-            drivers::ReturnType::LAST_WEAK)));  // drivers::ReturnMode::DUAL_WEAK_LAST
-      }
     }
     //    }
   }

nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_64_decoder.cpp

@@ -151,7 +151,7 @@ drivers::NebulaPointCloudPtr Pandar64Decoder::convert(size_t block_id)
   for (size_t unit_id = 0; unit_id < LASER_COUNT; ++unit_id) {
     const auto & unit = block.units[unit_id];
     // skip invalid points
-    if (unit.distance <= MIN_RANGE || unit.distance > MAX_RANGE) {
+    if (unit.distance < MIN_RANGE || unit.distance > MAX_RANGE) {
       continue;
     }
     block_pc->points.emplace_back(build_point(

nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_at_decoder.cpp

@@ -132,12 +132,22 @@ void PandarATDecoder::CalcXTPointXYZIT(
       continue;
     }
 
+    // Filter out pairs of close points in dual return mode (keep the odd block_id)
+    bool is_dual_return = packet_.return_mode == DUAL_RETURN ||
+                          packet_.return_mode == DUAL_RETURN_B ||
+                          packet_.return_mode == DUAL_RETURN_C;
+    if (is_dual_return && block_id == 0) {
+      if (std::abs(another_unit.distance - unit.distance) < dual_return_distance_threshold_) {
+        continue;
+      }
+    }
+
     point.intensity = unit.intensity;
     auto another_intensity = another_unit.intensity;
 
     bool identical_flg = false;
     if (point.intensity == another_intensity && unit.distance == another_unit.distance) {
-      if (0 < block_id) {
+      if (block_id == 0) {
         continue;
       }
       identical_flg = true;

nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_128_decoder.cpp

@@ -113,7 +113,7 @@ int PandarQT128Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_pac
         if (
           fabsf(
             packet_.blocks[block_id + 1].units[i].distance -
-            packet_.blocks[block_id].units[i].distance) > dual_return_distance_threshold_) {
+            packet_.blocks[block_id].units[i].distance) >= dual_return_distance_threshold_) {
           block_pc->points.emplace_back(block1_pt->points[i]);
           block_pc->points.emplace_back(block2_pt->points[i]);
           cnt2++;

nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_64_decoder.cpp

@@ -152,7 +152,7 @@ drivers::NebulaPointCloudPtr PandarQT64Decoder::convert(size_t block_id)
   for (size_t unit_id = 0; unit_id < LASER_COUNT; ++unit_id) {
     const auto & unit = block.units[unit_id];
     // skip invalid points
-    if (unit.distance <= MIN_RANGE || unit.distance > MAX_RANGE) {
+    if (unit.distance < MIN_RANGE || unit.distance > MAX_RANGE) {
       continue;
     }
 
@@ -185,8 +185,8 @@ drivers::NebulaPointCloudPtr PandarQT64Decoder::convert_dual(size_t block_id)
     const auto & even_unit = even_block.units[unit_id];
     const auto & odd_unit = odd_block.units[unit_id];
 
-    bool even_usable = !(even_unit.distance <= MIN_RANGE || even_unit.distance > MAX_RANGE);
-    bool odd_usable = !(odd_unit.distance <= MIN_RANGE || odd_unit.distance > MAX_RANGE);
+    bool even_usable = !(even_unit.distance < MIN_RANGE || even_unit.distance > MAX_RANGE);
+    bool odd_usable = !(odd_unit.distance < MIN_RANGE || odd_unit.distance > MAX_RANGE);
 
     // If the two returns are too close, only return the last one
     if (

nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_xt_decoder.cpp

@@ -68,7 +68,8 @@ int PandarXTDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet
     has_scanned_ = false;
   }
 
-  bool dual_return = packet_.return_mode == DUAL_RETURN;
+  bool dual_return = packet_.return_mode == DUAL_RETURN || packet_.return_mode == DUAL_RETURN_B ||
+                     packet_.return_mode == DUAL_RETURN_C;
   auto step = dual_return ? 2 : 1;
 
   for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) {
@@ -158,29 +159,39 @@ drivers::NebulaPointCloudPtr PandarXTDecoder::convert_dual(size_t block_id)
 
   auto unix_second = static_cast<double>(timegm(&packet_.t));  // sensor-time (ppt/gps)
 
-  auto head =
-    block_id + ((sensor_configuration_->return_mode == drivers::ReturnMode::FIRST) ? 1 : 0);
-  auto tail =
-    block_id + ((sensor_configuration_->return_mode == drivers::ReturnMode::LAST) ? 1 : 2);
+  auto head = block_id;
+  auto tail = block_id + 1;
 
   for (size_t unit_id = 0; unit_id < LASER_COUNT; ++unit_id) {
-    for (size_t i = head; i < tail; ++i) {
+    for (size_t i = head; i <= tail; ++i) {
       NebulaPoint point{};
       const auto & block = packet_.blocks[i];
       const auto & unit = block.units[unit_id];
-      const auto & another_block = packet_.blocks[(i + 1) % 2];
+      const auto & another_block = packet_.blocks[i == head ? tail : head];
       const auto & another_unit = another_block.units[unit_id];
       // skip invalid points
-      if (unit.distance <= MIN_RANGE || unit.distance > MAX_RANGE) {
+      if (unit.distance < MIN_RANGE || unit.distance > MAX_RANGE) {
         continue;
       }
+
       point.intensity = unit.intensity;
       auto another_intensity = another_unit.intensity;
       bool identical_flg = false;
-      if (point.intensity == another_intensity && unit.distance == another_unit.distance) {
+      if (unit.intensity == another_intensity && unit.distance == another_unit.distance) {
         identical_flg = true;
       }
 
+      if (i == head && identical_flg) {
+        continue;
+      }
+
+      // filter out dual return points with similar distance, keep the odd block_id
+      if (
+        i == head &&
+        std::abs(unit.distance - another_unit.distance) < dual_return_distance_threshold_) {
+        continue;
+      }
+
       float xyDistance = unit.distance * cosf(elevation_angle_rad_[unit_id]);
 
       point.x = xyDistance * sinf(azimuth_offset_rad_[unit_id] + block_azimuth_rad_[block.azimuth]);