hesai_decoders. update point stamp to be relative to scan

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>
tier4 · Jun 20, 2023 · ced637e · ced637e
1 parent 39e48c6
commit ced637e
Show file tree

Hide file tree

Showing 9 changed files with 107 additions and 106 deletions.
diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_128_e4x_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_128_e4x_decoder.cpp
@@ -138,7 +138,9 @@ drivers::NebulaPoint Pandar128E4XDecoder::build_point(
   point.azimuth = block_azimuth_rad_[azimuth] + azimuth_offset_rad_[laser_id];
   point.distance = unit_distance;
   point.elevation = elevation_angle_rad_[laser_id];
-  point.time_stamp = unix_second + packet_.tail.timestamp_us - scan_timestamp_;
+  point.time_stamp = static_cast<uint32_t>((static_cast<double>(unix_second) +
+    static_cast<double>(packet_.tail.timestamp_us)/1000000 -
+    scan_timestamp_)*1000000000);
   out_distance = xyDistance;
   return point;
 }

diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_40_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_40_decoder.cpp
@@ -72,8 +72,6 @@ int Pandar40Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet
   }
 
   if (has_scanned_) {
-    auto unix_second = static_cast<double>(timegm(&packet_.t));  // sensor-time (ppt/gps)
-    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
     scan_pc_ = overflow_pc_;
     overflow_pc_.reset(new NebulaPointCloud);
     overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS);
@@ -94,12 +92,15 @@ int Pandar40Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet
   }
 
   for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) {
-    auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
     int current_phase =
       (static_cast<int>(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000;
     if (current_phase > last_phase_ && !has_scanned_) {
+      auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
       *scan_pc_ += *block_pc;
     } else {
+      auto unix_second = static_cast<double>(timegm(&packet_.t));  // sensor-time (ppt/gps)
+      scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
+      auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
       *overflow_pc_ += *block_pc;
       has_scanned_ = true;
     }
@@ -132,22 +133,17 @@ drivers::NebulaPoint Pandar40Decoder::build_point(
   point.return_type = return_type;
 
   if (scan_timestamp_ < 0) {  // invalid timestamp
-    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
+    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.;
   }
   auto offset = dual_return
-                  ? (static_cast<double>(
-                       block_time_offset_dual_return_[block_id] + firing_time_offset_[unit_id]) /
-                     1000000.0f)
-                  : (static_cast<double>(
-                       block_time_offset_single_return_[block_id] + firing_time_offset_[unit_id]) /
-                     1000000.0f);
-  auto point_stamp =
-    (unix_second + offset + static_cast<double>(packet_.usec) / 1000000.f - scan_timestamp_);
-  if (point_stamp < 0) {
-    point.time_stamp = 0;
-  } else {
-    point.time_stamp = static_cast<uint32_t>(point_stamp * 10e9);
-  }
+                  ? static_cast<double>(
+                       block_time_offset_dual_return_[block_id] + firing_time_offset_[unit_id]) / 1000000.
+                  : static_cast<double>(
+                       block_time_offset_single_return_[block_id] + firing_time_offset_[unit_id]) / 1000000.;
+  auto point_stamp =  unix_second + (static_cast<double>(packet_.usec) / 1000000.0) - offset - scan_timestamp_;
+  if (point_stamp < 0)
+    point_stamp = 0;
+  point.time_stamp = static_cast<uint32_t>(point_stamp*1000000000);
 
   return point;
 }

diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_64_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_64_decoder.cpp
@@ -70,8 +70,6 @@ int Pandar64Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet
 
   if (has_scanned_) {
     scan_pc_ = overflow_pc_;
-    auto unix_second = static_cast<double>(timegm(&packet_.t));  // sensor-time (ppt/gps)
-    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
     overflow_pc_.reset(new NebulaPointCloud);
     overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS);
     has_scanned_ = false;
@@ -90,12 +88,15 @@ int Pandar64Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet
     }
   }
   for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) {
-    auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
     int current_phase =
       (static_cast<int>(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000;
     if (current_phase > last_phase_ && !has_scanned_) {
+      auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
       *scan_pc_ += *block_pc;
     } else {
+      auto unix_second = static_cast<double>(timegm(&packet_.t));  // sensor-time (ppt/gps)
+      scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
+      auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
       *overflow_pc_ += *block_pc;
       has_scanned_ = true;
     }
@@ -127,21 +128,17 @@ drivers::NebulaPoint Pandar64Decoder::build_point(
   point.elevation = elevation_angle_rad_[unit_id];
   point.return_type = return_type;
   if (scan_timestamp_ < 0) {  // invalid timestamp
-    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
-  }
-  auto offset =
-    dual_return
-      ? (static_cast<double>(block_time_offset_dual_[block_id] + firing_time_offset_[unit_id]) /
-         1000000.0f)
-      : (static_cast<double>(block_time_offset_single_[block_id] + firing_time_offset_[unit_id]) /
-         1000000.0f);
-  auto point_stamp =
-    (unix_second + offset + static_cast<double>(packet_.usec) / 1000000.f - scan_timestamp_);
-  if (point_stamp < 0) {
-    point.time_stamp = 0;
-  } else {
-    point.time_stamp = static_cast<uint32_t>(point_stamp * 10e9);
+    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.;
   }
+  auto offset = dual_return
+                ? static_cast<double>(
+                    block_time_offset_dual_[block_id] + firing_time_offset_[unit_id]) / 1000000.0
+                : static_cast<double>(
+                    block_time_offset_single_[block_id] + firing_time_offset_[unit_id]) / 1000000.;
+  auto point_stamp =  unix_second + (static_cast<double>(packet_.usec) / 1000000.0) - offset - scan_timestamp_;
+  if (point_stamp < 0)
+    point_stamp = 0;
+  point.time_stamp = static_cast<uint32_t>(point_stamp*1000000000);
 
   return point;
 }

diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_at_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_at_decoder.cpp
@@ -76,8 +76,6 @@ int PandarATDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet
 
   if (has_scanned_) {
     scan_pc_ = overflow_pc_;
-
-    scan_timestamp_ = packet_.unix_second + static_cast<double>(packet_.usec) / 1000000.f;
     overflow_pc_.reset(new NebulaPointCloud);
     has_scanned_ = false;
   }
@@ -87,7 +85,6 @@ int PandarATDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet
       packet_.blocks[block_id].azimuth * LIDAR_AZIMUTH_UNIT +
       packet_.blocks[block_id].fine_azimuth);
 
-    auto block_pc = convert(block_id);
     //*
     int count = 0, field = 0;
     while (count < correction_configuration_->frameNumber &&
@@ -103,9 +100,12 @@ int PandarATDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet
       if (max_azimuth_ < azimuth) {
         max_azimuth_ = azimuth;
       }
+      scan_timestamp_ = packet_.unix_second + static_cast<double>(packet_.usec) / 1000000.f;
+      auto block_pc = convert(block_id);
       *overflow_pc_ += *block_pc;
       has_scanned_ = true;
     } else {
+      auto block_pc = convert(block_id);
       *scan_pc_ += *block_pc;
     }
     last_azimuth_ = azimuth;
@@ -177,10 +177,13 @@ void PandarATDecoder::CalcXTPointXYZIT(
       point.z = static_cast<float>(unit.distance * m_sin_elevation_map_[elevation]);
     }
     if (scan_timestamp_ < 0) {  // invalid timestamp
-      scan_timestamp_ = packet_.unix_second + static_cast<double>(packet_.usec) / 1000000.f;
+      scan_timestamp_ = packet_.unix_second + static_cast<double>(packet_.usec) / 1000000.;
     }
 
-    point.time_stamp = channel_firing_ns[i];
+    point.time_stamp = static_cast<uint32_t>(
+      (packet_.unix_second +
+      static_cast<double>(packet_.usec) / 1000000. -
+      scan_timestamp_)*1000000000);
 
     switch (packet_.return_mode) {
       case STRONGEST_RETURN:

diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_128_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_128_decoder.cpp
@@ -82,8 +82,6 @@ int PandarQT128Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_pac
 
   if (has_scanned_) {
     scan_pc_ = overflow_pc_;
-    auto unix_second = static_cast<double>(timegm(&packet_.t));  // sensor-time (ppt/gps)
-    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
     overflow_pc_.reset(new NebulaPointCloud);
     overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS);
     has_scanned_ = false;
@@ -94,8 +92,19 @@ int PandarQT128Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_pac
   drivers::NebulaPointCloudPtr block_pc(new NebulaPointCloud);
   int current_phase;
   int cnt2;
+  bool accumulating;
   if (dual_return) {
     for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += 2) {
+
+      current_phase =
+        (static_cast<int>(packet_.blocks[block_id + 1].azimuth) - scan_phase_ + 36000) % 36000;
+      if (current_phase > last_phase_ && !has_scanned_) {
+        accumulating = true;
+      }
+      else {
+        auto unix_second = static_cast<double>(timegm(&packet_.t));  // sensor-time (ppt/gps)
+        scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
+      }
       auto block1_pt = convert(block_id);
       auto block2_pt = convert(block_id + 1);
       size_t block1size = block1_pt->points.size();
@@ -113,19 +122,22 @@ int PandarQT128Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_pac
           block_pc->points.emplace_back(block1_pt->points[i]);
         }
       }
-      current_phase =
-        (static_cast<int>(packet_.blocks[block_id + 1].azimuth) - scan_phase_ + 36000) % 36000;
+
     }
   } else  // single
   {
     for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id++) {
-      block_pc = convert(block_id);
-      *block_pc += *block_pc;
+
       current_phase =
         (static_cast<int>(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000;
+      if (current_phase > last_phase_ && !has_scanned_) {
+        accumulating = true;
+      }
+      block_pc = convert(block_id);
+      *block_pc += *block_pc;
     }
   }
-  if (current_phase > last_phase_ && !has_scanned_) {
+  if (accumulating) {
     *scan_pc_ += *block_pc;
   } else {
     *overflow_pc_ += *block_pc;
@@ -159,21 +171,17 @@ drivers::NebulaPoint PandarQT128Decoder::build_point(
     point.return_type = first_return_type_;
   }
   if (scan_timestamp_ < 0) {  // invalid timestamp
-    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
-  }
-  auto offset =
-    dual_return
-      ? (static_cast<double>(block_time_offset_dual_[block_id] + firing_time_offset1_[unit_id]) /
-         1000000.0f)
-      : (static_cast<double>(block_time_offset_single_[block_id] + firing_time_offset2_[unit_id]) /
-         1000000.0f);
-  auto point_stamp =
-    (unix_second + offset + static_cast<double>(packet_.usec) / 1000000.f - scan_timestamp_);
-  if (point_stamp < 0) {
-    point.time_stamp = 0;
-  } else {
-    point.time_stamp = static_cast<uint32_t>(point_stamp * 10e9);
+    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.;
   }
+  auto offset = dual_return
+                ? static_cast<double>(
+                    block_time_offset_dual_[block_id] + firing_time_offset2_[unit_id]) / 1000000.0
+                : static_cast<double>(
+                    block_time_offset_single_[block_id] + firing_time_offset1_[unit_id]) / 1000000.;
+  auto point_stamp =  unix_second + (static_cast<double>(packet_.usec) / 1000000.0) - offset - scan_timestamp_;
+  if (point_stamp < 0)
+    point_stamp = 0;
+  point.time_stamp = static_cast<uint32_t>(point_stamp*1000000000);
 
   return point;
 }

diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_64_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_64_decoder.cpp
@@ -70,7 +70,6 @@ int PandarQT64Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_pack
 
   if (has_scanned_) {
     scan_pc_ = overflow_pc_;
-    scan_timestamp_ = std::numeric_limits<uint32_t>::max();
     overflow_pc_.reset(new NebulaPointCloud);
     overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS);
     has_scanned_ = false;
@@ -90,12 +89,15 @@ int PandarQT64Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_pack
   }
 
   for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) {
-    auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
     int current_phase =
       (static_cast<int>(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000;
     if (current_phase > last_phase_ && !has_scanned_) {
+      auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
       *scan_pc_ += *block_pc;
     } else {
+      auto unix_second = static_cast<double>(timegm(&packet_.t));  // sensor-time (ppt/gps)
+      scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
+      auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
       *overflow_pc_ += *block_pc;
       has_scanned_ = true;
     }
@@ -127,21 +129,17 @@ drivers::NebulaPoint PandarQT64Decoder::build_point(
   point.elevation = elevation_angle_rad_[unit_id];
   point.return_type = return_type;
   if (scan_timestamp_ < 0) {  // invalid timestamp
-    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
-  }
-  auto offset =
-    dual_return
-      ? (static_cast<double>(block_time_offset_dual_[block_id] + firing_offset_[unit_id]) /
-         1000000.0f)
-      : (static_cast<double>(block_time_offset_single_[block_id] + firing_offset_[unit_id]) /
-         1000000.0f);
-  auto point_stamp =
-    (unix_second + offset + static_cast<double>(packet_.usec) / 1000000.f - scan_timestamp_);
-  if (point_stamp < 0) {
-    point.time_stamp = 0;
-  } else {
-    point.time_stamp = static_cast<uint32_t>(point_stamp * 10e9);
+    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.;
   }
+  auto offset = dual_return
+                ? static_cast<double>(
+                    block_time_offset_dual_[block_id] + firing_offset_[unit_id]) / 1000000.
+                : static_cast<double>(
+                    block_time_offset_single_[block_id] + firing_offset_[unit_id]) / 1000000.;
+  auto point_stamp =  unix_second + (static_cast<double>(packet_.usec) / 1000000.0) - offset - scan_timestamp_;
+  if (point_stamp < 0)
+    point_stamp = 0;
+  point.time_stamp = static_cast<uint32_t>(point_stamp*1000000000);
 
   return point;
 }

diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_xt_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_xt_decoder.cpp
@@ -63,8 +63,6 @@ int PandarXTDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet
 
   if (has_scanned_) {
     scan_pc_ = overflow_pc_;
-    auto unix_second = static_cast<double>(timegm(&packet_.t));  // sensor-time (ppt/gps)
-    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
     overflow_pc_.reset(new NebulaPointCloud);
     overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS);
     has_scanned_ = false;
@@ -74,12 +72,15 @@ int PandarXTDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet
   auto step = dual_return ? 2 : 1;
 
   for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) {
-    auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
     int current_phase =
       (static_cast<int>(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000;
     if (current_phase > last_phase_ && !has_scanned_) {
+      auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
       *scan_pc_ += *block_pc;
     } else {
+      auto unix_second = static_cast<double>(timegm(&packet_.t));  // sensor-time (ppt/gps)
+      scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
+      auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
       *overflow_pc_ += *block_pc;
       has_scanned_ = true;
     }
@@ -108,18 +109,14 @@ drivers::NebulaPoint PandarXTDecoder::build_point(int block_id, int unit_id, uin
   point.elevation = elevation_angle_rad_[unit_id];
   point.return_type = return_type;
   if (scan_timestamp_ < 0) {  // invalid timestamp
-    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
-  }
-  auto offset =
-    (static_cast<double>(block_offset_dual_return_[block_id] + firing_time_offset_[unit_id]) /
-     1000000.0f);
-  auto point_stamp =
-    (unix_second + offset + static_cast<double>(packet_.usec) / 1000000.f - scan_timestamp_);
-  if (point_stamp < 0) {
-    point.time_stamp = 0;
-  } else {
-    point.time_stamp = static_cast<uint32_t>(point_stamp * 10e9);
+    scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.;
   }
+  auto offset = static_cast<double>(
+                  block_time_offset_single_return_[block_id] + firing_time_offset_[unit_id]) / 1000000.0;
+  auto point_stamp =  unix_second + (static_cast<double>(packet_.usec) / 1000000.0) - offset - scan_timestamp_;
+  if (point_stamp < 0)
+    point_stamp = 0;
+  point.time_stamp = static_cast<uint32_t>(point_stamp*1000000000);
   return point;
 }
 
@@ -194,18 +191,14 @@ drivers::NebulaPointCloudPtr PandarXTDecoder::convert_dual(size_t block_id)
       point.distance = unit.distance;
 
       if (scan_timestamp_ < 0) {  // invalid timestamp
-        scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
-      }
-      auto offset =
-        (static_cast<double>(block_offset_dual_return_[block_id] + firing_time_offset_[unit_id]) /
-         1000000.0f);
-      auto point_stamp =
-        (unix_second + offset + static_cast<double>(packet_.usec) / 1000000.f - scan_timestamp_);
-      if (point_stamp < 0) {
-        point.time_stamp = 0;
-      } else {
-        point.time_stamp = static_cast<uint32_t>(point_stamp * 10e9);
+        scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.;
       }
+      auto offset = static_cast<double>(
+                        block_offset_dual_return_[block_id] + firing_time_offset_[unit_id]) / 1000000.0;
+      auto point_stamp =  unix_second + (static_cast<double>(packet_.usec) / 1000000.0) - offset - scan_timestamp_;
+      if (point_stamp < 0)
+        point_stamp = 0;
+      point.time_stamp = static_cast<uint32_t>(point_stamp*1000000000);
 
       if (identical_flg) {
         point.return_type = static_cast<uint8_t>(nebula::drivers::ReturnType::IDENTICAL);