chore(lidar/livox): using sys time if time not synchronized or diff is exceeded

modules/drivers/lidar/livox/component/livox_component.cpp

@@ -16,6 +16,7 @@
 
 #include "modules/drivers/lidar/livox/component/livox_component.h"
 
+#include <iomanip>
 #include <string>
 #include <utility>
 
@@ -25,16 +26,60 @@ namespace lidar {
 
 static LivoxLidarComponent* g_livox_component = nullptr;
 
-uint64_t GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp,
-                               uint8_t size) {
+uint64_t LivoxLidarComponent::GetEthPacketTimestamp(uint8_t handle,
+                                                     uint8_t timestamp_type,
+                                                     uint8_t* time_stamp,
+                                                     uint8_t size) {
   LdsStamp time;
   memcpy(time.stamp_bytes, time_stamp, size);
+  uint64_t packet_timestamp = time.stamp;
+  uint64_t system_time_ns = cyber::Time::Now().ToNanosecond();
+
+  // Default is use_lidar_clock=true, check if explicitly disabled
+  bool use_lidar_clock = config_.has_use_lidar_clock() ?
+                         config_.use_lidar_clock() : true;
+
+  // If use_lidar_clock is false, always use system time
+  // Only warn once since this is the user's explicit configuration
+  if (!use_lidar_clock) {
+    if (!warned_not_use_lidar_clock_) {
+      AWARN << "[Livox Timestamp] use_lidar_clock is DISABLED. "
+            << "Using system time (data arrival time) instead of lidar clock. "
+            << "This means measurement_time will NOT reflect laser scan time!";
+      warned_not_use_lidar_clock_ = true;
+    }
+    return system_time_ns;
+  }
 
-  if (timestamp_type == kTimestampTypeGptpOrPtp ||
-      timestamp_type == kTimestampTypeGps) {
-    return time.stamp;
+  // use_lidar_clock=true: Try to use packet timestamp (laser scan time)
+  double max_diff_s = config_.has_max_timestamp_diff_s() ?
+                       config_.max_timestamp_diff_s() : 10.0;
+  uint64_t max_diff_ns = static_cast<uint64_t>(max_diff_s * 1e9);
+
+  // Calculate absolute difference
+  uint64_t diff_ns;
+  if (packet_timestamp > system_time_ns) {
+    diff_ns = packet_timestamp - system_time_ns;
+  } else {
+    diff_ns = system_time_ns - packet_timestamp;
   }
-  return cyber::Time::Now().ToNanosecond();
+
+  // If diff is too large, packet time is likely not synchronized
+  if (diff_ns > max_diff_ns) {
+    double packet_time_s = packet_timestamp / 1e9;
+    double system_time_s = system_time_ns / 1e9;
+    double diff_s = static_cast<double>(diff_ns) / 1e9;
+    AWARN_EVERY(10000) << std::fixed << std::setprecision(3)
+          << "[Livox Ts] Handle=" << static_cast<int>(handle)
+          << " type=" << static_cast<int>(timestamp_type)
+          << " pkt_ts=" << packet_time_s
+          << " sys_ts=" << system_time_s
+          << " diff=" << diff_s << "s > " << max_diff_s << "s, using sys time";
+    return system_time_ns;
+  }
+
+  // Use packet timestamp (lidar clock / laser scan time)
+  return packet_timestamp;
 }
 
 void LivoxLidarComponent::BinaryDataProcess(const unsigned char* data,
@@ -98,14 +143,14 @@ void LivoxLidarComponent::PointCloudCallback(uint8_t handle,
     return;
   }
 
-  AINFO << boost::format(
-               "point cloud handle: %d, data_num: %d, data_type: %d\n") %
-               static_cast<int>(handle) % data_num %
-               static_cast<int>(data->data_type);
+  ADEBUG << boost::format(
+              "point cloud handle: %d, data_num: %d, data_type: %d") %
+              static_cast<int>(handle) % data_num %
+              static_cast<int>(data->data_type);
 
   size_t byte_size = g_livox_component->GetEthPacketByteSize(data, data_num);
-  uint64_t pkt_timestamp = GetEthPacketTimestamp(
-      data->timestamp_type, data->timestamp, sizeof(data->timestamp));
+  uint64_t pkt_timestamp = g_livox_component->GetEthPacketTimestamp(
+      handle, data->timestamp_type, data->timestamp, sizeof(data->timestamp));
 
   g_livox_component->BinaryDataProcess(data->data, data->data_type, data_num,
                                        pkt_timestamp);

modules/drivers/lidar/livox/component/livox_component.h

@@ -49,9 +49,6 @@ typedef union {
   int64_t stamp;
 } LdsStamp;
 
-uint64_t GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp,
-                               uint8_t size);
-
 class LivoxLidarComponent final : public LidarComponentBase<livox::LivoxScan> {
  public:
   void BinaryDataProcess(const unsigned char* data, int data_type,
@@ -68,13 +65,19 @@ class LivoxLidarComponent final : public LidarComponentBase<livox::LivoxScan> {
   static void OnDeviceStateUpdate(const DeviceInfo* device, DeviceEvent event);
   static void OnLidarStartSamplingCb(livox_status status, uint8_t handle,
                                      uint8_t response, void* client_data);
+
+ private:
+  uint64_t GetEthPacketTimestamp(uint8_t handle, uint8_t timestamp_type,
+                                 uint8_t* time_stamp, uint8_t size);
+
   livox::Config config_;
   std::deque<PointXYZIT> integral_queue_;
   uint64_t last_pointcloud_pub_timestamp_{0};
   double pointcloud_freq_ = {10.0};
   double integral_time_ = {0.4};
   uint8_t lidar_handle_ = {0};
   bool sdk_initialized_ = {false};
+  bool warned_not_use_lidar_clock_ = {false};
 };
 CYBER_REGISTER_COMPONENT(LivoxLidarComponent)
 }  // namespace lidar

modules/drivers/lidar/livox/proto/livox.proto

@@ -9,6 +9,27 @@ message Config {
   optional string broadcast_code = 2;
   optional double integral_time = 3 [default = 0.1];
   optional bool enable_sdk_console_log = 4 [default = false];
+  // Use lidar clock (packet timestamp) instead of system time
+  // When true, use lidar hardware timestamp (laser scan time)
+  // When false, use system time (data arrival time)
+  // Default: true (use lidar clock for accurate laser scan time)
+  optional bool use_lidar_clock = 5 [default = true];
+
+  // Max allowed diff between packet timestamp and system time (seconds)
+  // If the diff exceeds this value and use_lidar_clock is true,
+  // fall back to system time and warn about time sync issue
+  // This helps detect when lidar hardware time is not synchronized
+  //
+  // Livox timestamp sync modes (timestamp_type in packet):
+  //   0 = NoSync    - No sync signal mode (may have time drift)
+  //   1 = PTP       - 1588v2.0 PTP sync mode
+  //   2 = Reserved  - Reserved for future use
+  //   3 = PPS+GPS   - PPS (Pulse Per Second) + GPS sync mode
+  //   4 = PPS       - PPS only sync mode
+  //   5 = Unknown   - Unknown sync mode
+  //
+  // Default: 10.0 seconds
+  optional double max_timestamp_diff_s = 6 [default = 10.0];
 }
 
 message LivoxScan {