fixed: a timestamp problem in CustomMsg format

Livox-SDK · May 12, 2023 · bf4d062 · bf4d062
1 parent 4629d1a
commit bf4d062
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 10 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -2,6 +2,10 @@
 
 All notable changes to this project will be documented in this file.
 
+## [1.2.1]
+### Fixed
+- Fix offset time error regarding CustomMsg format message publishment.
+
 ## [1.2.0]
 ### Added
 - Revise the frame segmentation logic.

diff --git a/src/comm/pub_handler.cpp b/src/comm/pub_handler.cpp
@@ -167,18 +167,16 @@ void PubHandler::CheckTimer() {
       continue;
     }
 
+    frame_.base_time[frame_.lidar_num] = process_handler.second->GetLidarBaseTime();
     uint32_t id = process_handler.first;
     points_[id].clear();
     process_handler.second->GetLidarPointClouds(points_[id]);
     if (points_[id].empty()) {
       continue;
     }
-
-    frame_.base_time[frame_.lidar_num] = process_handler.second->GetLidarBaseTime();
     PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num];
     lidar_point.lidar_type = LidarProtoType::kLivoxLidarType;  // TODO:
     lidar_point.handle = id;
-
     lidar_point.points_num = points_[id].size();
     lidar_point.points = points_[id].data();
     frame_.lidar_num++;
@@ -205,7 +203,6 @@ void PubHandler::RawDataProcess() {
       raw_data = raw_packet_queue_.front();
       raw_packet_queue_.pop_front();
     }
-    // 这里是来一个包判断一个雷达
     uint32_t id = 0;
     GetLidarId(raw_data.lidar_type, raw_data.handle, id);
     if (lidar_process_handlers_.find(id) == lidar_process_handlers_.end()) {
@@ -232,8 +229,7 @@ uint64_t PubHandler::GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time
   LdsStamp time;
   memcpy(time.stamp_bytes, time_stamp, size);
 
-  if (timestamp_type == kTimestampTypeNoSync ||
-      timestamp_type == kTimestampTypeGptpOrPtp ||
+  if (timestamp_type == kTimestampTypeGptpOrPtp ||
       timestamp_type == kTimestampTypeGps) {
     return time.stamp;
   }

diff --git a/src/lddc.cpp b/src/lddc.cpp
@@ -307,7 +307,7 @@ void Lddc::InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint
   cloud.is_dense     = true;
 
   if (!pkg.points.empty()) {
-    timestamp = pkg.base_time + pkg.points[0].offset_time;
+    timestamp = pkg.base_time;
   }
 
   #ifdef BUILDING_ROS1
@@ -362,7 +362,7 @@ void Lddc::InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t
 
   uint64_t timestamp = 0;
   if (!pkg.points.empty()) {
-    timestamp = pkg.base_time + pkg.points[0].offset_time;
+    timestamp = pkg.base_time;
   }
   livox_msg.timebase = timestamp;
 
@@ -392,7 +392,7 @@ void Lddc::FillPointsToCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg)
     point.reflectivity = points[i].intensity;
     point.tag = points[i].tag;
     point.line = points[i].line;
-    point.offset_time = static_cast<uint32_t>(points[i].offset_time);
+    point.offset_time = static_cast<uint32_t>(points[i].offset_time - pkg.base_time);
 
     livox_msg.points.push_back(std::move(point));
   }
@@ -423,7 +423,7 @@ void Lddc::InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& tim
   cloud.width = pkg.points_num;
 
   if (!pkg.points.empty()) {
-    timestamp = pkg.base_time + pkg.points[0].offset_time;
+    timestamp = pkg.base_time;
   }
   cloud.header.stamp = timestamp / 1000.0;  // to pcl ros time stamp
 #elif defined BUILDING_ROS2