Timestamp update

nebula_common/include/nebula_common/nebula_common.hpp

@@ -487,8 +487,8 @@ inline ReturnMode ReturnModeFromString(const std::string & return_mode)
 [[maybe_unused]] pcl::PointCloud<PointXYZIR>::Ptr convertPointXYZIRADTToPointXYZIR(
   const pcl::PointCloud<PointXYZIRADT>::ConstPtr & input_pointcloud);
 
-[[maybe_unused]] pcl::PointCloud<PointXYZIR>::Ptr convertPointXYZICAETRToPointXYZIR(
-  const pcl::PointCloud<PointXYZICAETR>::ConstPtr & input_pointcloud);
+[[maybe_unused]] pcl::PointCloud<PointXYZIR>::Ptr convertPointXYZIRCAEDTToPointXYZIR(
+  const pcl::PointCloud<PointXYZIRCAEDT>::ConstPtr & input_pointcloud);
 
 }  // namespace drivers
 }  // namespace nebula

nebula_common/include/nebula_common/point_types.hpp

@@ -29,16 +29,17 @@ struct PointXYZICATR
  * This point type is not using PCL_ADD_POINT4D to avoid the addition of a 32-bit dummy word.
  * The fields are ordered to meet the SSE alignment.
  */
-struct PointXYZICAETR
+struct PointXYZIRCAEDT
 {
   float x;
   float y;
   float z;
-  float azimuth;
-  float elevation;
-  std::uint8_t return_type;
   std::uint8_t intensity;
+  std::uint8_t return_type;
   std::uint16_t channel;
+  float azimuth;
+  float elevation;
+  float distance;
   std::uint32_t time_stamp;
 };
 
@@ -54,7 +55,7 @@ struct PointXYZIRADT
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 } EIGEN_ALIGN16;
 
-using NebulaPoint = PointXYZICAETR;
+using NebulaPoint = PointXYZIRCAEDT;
 using NebulaPointPtr = std::shared_ptr<NebulaPoint>;
 using NebulaPointCloud = pcl::PointCloud<NebulaPoint>;
 using NebulaPointCloudPtr = pcl::PointCloud<NebulaPoint>::Ptr;
@@ -80,9 +81,10 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(
     std::uint32_t, time_stamp, time_stamp)(std::uint8_t, return_type, return_type))
 
 POINT_CLOUD_REGISTER_POINT_STRUCT(
-  nebula::drivers::PointXYZICAETR,
-  (float, x, x)(float, y, y)(float, z, z)(float, azimuth, azimuth)(float, elevation, elevation)(
-    std::uint8_t, return_type, return_type)(std::uint8_t, intensity, intensity)(
-    std::uint16_t, channel, channel)(std::uint32_t, time_stamp, time_stamp))
+  nebula::drivers::PointXYZIRCAEDT,
+  (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(
+    std::uint8_t, return_type,
+    return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)(
+    float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp))
 
 #endif

nebula_common/src/nebula_common.cpp

@@ -25,8 +25,8 @@ namespace drivers
   return output_pointcloud;
 }
 
-pcl::PointCloud<PointXYZIR>::Ptr convertPointXYZICAETRToPointXYZIR(
-  const pcl::PointCloud<PointXYZICAETR>::ConstPtr & input_pointcloud)
+pcl::PointCloud<PointXYZIR>::Ptr convertPointXYZIRCAEDTToPointXYZIR(
+  const pcl::PointCloud<PointXYZIRCAEDT>::ConstPtr & input_pointcloud)
 {
   pcl::PointCloud<PointXYZIR>::Ptr output_pointcloud(new pcl::PointCloud<PointXYZIR>);
   output_pointcloud->reserve(input_pointcloud->points.size());

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp

@@ -26,8 +26,7 @@ class HesaiScanDecoder
   int last_phase_{};
   bool has_scanned_{};
   double dual_return_distance_threshold_{};
-  uint32_t first_timestamp_{};
-  uint32_t first_timestamp_tmp{};
+  double scan_timestamp_{};
 
   /// @brief SensorConfiguration for this decoder
   std::shared_ptr<drivers::HesaiSensorConfiguration> sensor_configuration_;

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt_128_decoder.hpp

@@ -46,7 +46,7 @@ class PandarQT128Decoder : public HesaiScanDecoder
   /// @param unix_second Packet time
   /// @return Point cloud
   drivers::NebulaPoint build_point(
-    size_t block_id, size_t unit_id, bool dual_return, const uint32_t & unix_second);
+    size_t block_id, size_t unit_id, bool dual_return, const double & unix_second);
   /// @brief Convert to point cloud
   /// @param block_id target block
   /// @return Point cloud

nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_128_e4x_decoder.cpp

@@ -34,7 +34,7 @@ Pandar128E4XDecoder::Pandar128E4XDecoder(
 
   last_phase_ = 0;
   has_scanned_ = false;
-  first_timestamp_ = 0;
+  scan_timestamp_ = 0;
 
   scan_pc_.reset(new NebulaPointCloud);
   scan_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS);
@@ -46,7 +46,7 @@ bool Pandar128E4XDecoder::hasScanned() { return has_scanned_; }
 
 std::tuple<drivers::NebulaPointCloudPtr, double> Pandar128E4XDecoder::get_pointcloud()
 {
-  return std::make_tuple(scan_pc_, first_timestamp_);
+  return std::make_tuple(scan_pc_, scan_timestamp_);
 }
 
 bool Pandar128E4XDecoder::parsePacket(const pandar_msgs::msg::PandarPacket & raw_packet)
@@ -104,7 +104,7 @@ void Pandar128E4XDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_p
     overflow_pc_.reset(new NebulaPointCloud);
     overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS);
     has_scanned_ = false;
-    first_timestamp_ = current_unit_unix_second_;
+    scan_timestamp_ = current_unit_unix_second_;
   }
 
   bool dual_return = is_dual_return();
@@ -126,18 +126,18 @@ drivers::NebulaPoint Pandar128E4XDecoder::build_point(
   const uint32_t & unix_second, float & out_distance)
 {
   NebulaPoint point{};
-
-  float xyDistance =
-    static_cast<float>(block.distance) * DISTANCE_UNIT * cos_elevation_angle_[laser_id];
+  auto unit_distance = block.distance * DISTANCE_UNIT;
+  float xyDistance = unit_distance * cos_elevation_angle_[laser_id];
 
   point.x = xyDistance * sinf(azimuth_offset_rad_[laser_id] + block_azimuth_rad_[azimuth]);
   point.y = xyDistance * cosf(azimuth_offset_rad_[laser_id] + block_azimuth_rad_[azimuth]);
   point.z = static_cast<float>(block.distance) * DISTANCE_UNIT * sin_elevation_angle_[laser_id];
   point.intensity = block.reflectivity;
   point.channel = laser_id;
   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 - first_timestamp_;
+  point.time_stamp = unix_second + packet_.tail.timestamp_us - scan_timestamp_;
   out_distance = xyDistance;
   return point;
 }

nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_40_decoder.cpp

@@ -50,8 +50,7 @@ Pandar40Decoder::Pandar40Decoder(
   dual_return_distance_threshold_ = sensor_configuration_->dual_return_distance_threshold;
   last_phase_ = 0;
   has_scanned_ = false;
-  first_timestamp_tmp = std::numeric_limits<uint32_t>::max();
-  first_timestamp_ = first_timestamp_tmp;
+  scan_timestamp_ = -1;
 
   scan_pc_.reset(new NebulaPointCloud);
   scan_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS);
@@ -63,7 +62,7 @@ bool Pandar40Decoder::hasScanned() { return has_scanned_; }
 
 std::tuple<drivers::NebulaPointCloudPtr, double> Pandar40Decoder::get_pointcloud()
 {
-  return std::make_tuple(scan_pc_, first_timestamp_);
+  return std::make_tuple(scan_pc_, scan_timestamp_);
 }
 
 void Pandar40Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet)
@@ -73,9 +72,9 @@ void Pandar40Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packe
   }
 
   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_;
-    first_timestamp_ = first_timestamp_tmp;
-    first_timestamp_tmp = std::numeric_limits<uint32_t>::max();
     overflow_pc_.reset(new NebulaPointCloud);
     overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS);
     has_scanned_ = false;
@@ -114,9 +113,7 @@ drivers::NebulaPoint Pandar40Decoder::build_point(
   const auto & block = packet_.blocks[block_id];
   const auto & unit = block.units[unit_id];
   auto unix_second = static_cast<double>(timegm(&packet_.t));
-  if (unix_second < first_timestamp_tmp) {
-    first_timestamp_tmp = unix_second;
-  }
+
   bool dual_return = (packet_.return_mode == DUAL_RETURN);
   NebulaPoint point{};
 
@@ -128,18 +125,28 @@ drivers::NebulaPoint Pandar40Decoder::build_point(
 
   point.intensity = unit.intensity;
   point.channel = unit_id;
-  point.azimuth = block_azimuth_rad_[block_id] + azimuth_offset_rad_[unit_id];
+  point.azimuth = block_azimuth_rad_[block.azimuth] + azimuth_offset_rad_[unit_id];
+  point.distance = unit.distance;
   point.elevation = elevation_angle_rad_[unit_id];
   point.return_type = return_type;
-  point.time_stamp = (static_cast<double>(packet_.usec)) / 1000000.0;
-
-  point.time_stamp -=
-    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);
+
+  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_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);
+  }
 
   return point;
 }

nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_64_decoder.cpp

@@ -48,8 +48,7 @@ Pandar64Decoder::Pandar64Decoder(
 
   last_phase_ = 0;
   has_scanned_ = false;
-  first_timestamp_tmp = std::numeric_limits<uint32_t>::max();
-  first_timestamp_ = first_timestamp_tmp;
+  scan_timestamp_ = -1;
   scan_pc_.reset(new NebulaPointCloud);
   scan_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS);
   overflow_pc_.reset(new NebulaPointCloud);
@@ -60,7 +59,7 @@ bool Pandar64Decoder::hasScanned() { return has_scanned_; }
 
 std::tuple<drivers::NebulaPointCloudPtr, double> Pandar64Decoder::get_pointcloud()
 {
-  return std::make_tuple(scan_pc_, first_timestamp_);
+  return std::make_tuple(scan_pc_, scan_timestamp_);
 }
 
 void Pandar64Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet)
@@ -71,8 +70,8 @@ void Pandar64Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packe
 
   if (has_scanned_) {
     scan_pc_ = overflow_pc_;
-    first_timestamp_ = first_timestamp_tmp;
-    first_timestamp_tmp = std::numeric_limits<uint32_t>::max();
+    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;
@@ -110,9 +109,7 @@ drivers::NebulaPoint Pandar64Decoder::build_point(
   const auto & block = packet_.blocks[block_id];
   const auto & unit = block.units[unit_id];
   auto unix_second = static_cast<double>(timegm(&packet_.t));
-  if (unix_second < first_timestamp_tmp) {
-    first_timestamp_tmp = unix_second;
-  }
+
   bool dual_return = (packet_.return_mode == DUAL_RETURN);
   NebulaPoint point{};
 
@@ -124,16 +121,26 @@ drivers::NebulaPoint Pandar64Decoder::build_point(
 
   point.intensity = unit.intensity;
   point.channel = unit_id;
-  point.azimuth = block_azimuth_rad_[block_id] + azimuth_offset_rad_[unit_id];
+  point.azimuth = block_azimuth_rad_[block.azimuth] + azimuth_offset_rad_[unit_id];
+  point.distance = unit.distance;
   point.elevation = elevation_angle_rad_[unit_id];
   point.return_type = return_type;
-  point.time_stamp = (static_cast<double>(packet_.usec)) / 1000000.0;
-  point.time_stamp +=
+  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);
+  }
 
   return point;
 }
@@ -146,7 +153,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 <= 0.1 || unit.distance > 200.0) {
+    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

@@ -50,8 +50,7 @@ PandarATDecoder::PandarATDecoder(
 
   last_phase_ = 0;
   has_scanned_ = false;
-  first_timestamp_tmp = std::numeric_limits<uint32_t>::max();
-  first_timestamp_ = first_timestamp_tmp;
+  scan_timestamp_ = -1;
   last_field_ = -1;
 
   scan_pc_.reset(new NebulaPointCloud);
@@ -62,7 +61,7 @@ bool PandarATDecoder::hasScanned() { return has_scanned_; }
 
 std::tuple<drivers::NebulaPointCloudPtr, double> PandarATDecoder::get_pointcloud()
 {
-  return std::make_tuple(scan_pc_, first_timestamp_);
+  return std::make_tuple(scan_pc_, scan_timestamp_);
 }
 
 void PandarATDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet)
@@ -74,8 +73,8 @@ void PandarATDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packe
 
   if (has_scanned_) {
     scan_pc_ = overflow_pc_;
-    first_timestamp_ = first_timestamp_tmp;
-    first_timestamp_tmp = std::numeric_limits<uint32_t>::max();
+    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);
     has_scanned_ = false;
   }
@@ -104,7 +103,6 @@ void PandarATDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packe
     }
     last_azimuth_ = Azimuth;
     last_field_ = field;
-    last_timestamp_ = packet_.usec;
   }
 }
 
@@ -163,7 +161,9 @@ void PandarATDecoder::CalcXTPointXYZIT(
                      correction_configuration_->azimuth[i] +
                      correction_configuration_->getAzimuthAdjustV3(i, Azimuth) * LIDAR_AZIMUTH_UNIT;
       azimuth = (MAX_AZI_LEN + azimuth) % MAX_AZI_LEN;
-      point.azimuth = azimuth / 3600.f;
+      point.azimuth = deg2rad(azimuth / 3600.f);
+      point.distance = unit.distance;
+      point.elevation = elevation;
       {
         float xyDistance = unit.distance * m_cos_elevation_map_[elevation];
         point.x = static_cast<float>(xyDistance * m_sin_azimuth_map_[azimuth]);
@@ -179,6 +179,7 @@ void PandarATDecoder::CalcXTPointXYZIT(
         Azimuth + MAX_AZI_LEN - (azimuth_offset_[i] * 100 * LIDAR_AZIMUTH_UNIT) / 2);
       azimuth = (MAX_AZI_LEN + azimuth) % MAX_AZI_LEN;
       point.azimuth = azimuth / 3600.f;
+      point.distance = unit.distance;
       point.elevation = elevation;
 
       {
@@ -188,13 +189,18 @@ void PandarATDecoder::CalcXTPointXYZIT(
         point.z = static_cast<float>(unit.distance * m_sin_elevation_map_[elevation]);
       }
     }
-
-    double unix_second = packet_.unix_second;
-    if (unix_second < first_timestamp_tmp) {
-      first_timestamp_tmp = unix_second;
+    auto unix_second = static_cast<double>(timegm(&packet_.t));
+    if (scan_timestamp_ < 0) {  // invalid timestamp
+      scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
     }
-    point.time_stamp = (static_cast<double>(packet_.usec)) / 1000000.0;
-    //    point.return_type = packet_.return_mode;
+    auto point_stamp =
+      (unix_second + 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);
+    }
+
     switch (packet_.return_mode) {
       case STRONGEST_RETURN:
         point.return_type = static_cast<uint8_t>(nebula::drivers::ReturnType::STRONGEST);