1. add a timestamp to each point in Livox pointcloud2 format; 2. revi…

…se the frame-segmentation logic

CHANGELOG.md

@@ -2,27 +2,35 @@
 
 All notable changes to this project will be documented in this file.
 
-## [1.1.3] - 2023-03-13
+## [1.2.0]
+### Added
+- Revise the frame segmentation logic.
+- (Notice!!!) Add Timestamp to each point in Livox pointcloud2 (PointXYZRTLT) format. The PointXYZRTL format has been updated to PointXYZRTLT format. Compatibility needs to be considered.
+### Fixed
+- Improve support for gPTP and GPS synchronizations.
+
+--- 
+## [1.1.3]
 ### Fixed
-- Improve performance when running in ROS2 Humble;
+- Improve performance when running in ROS2 Humble.
 
 --- 
-## [1.1.2] - 2023-02-15
+## [1.1.2]
 ### Changed
-- Change publish frequency range to [0.5Hz, 10 Hz];
+- Change publish frequency range to [0.5Hz, 10 Hz].
 ### Fixed
-- Fix a high CPU-usage problem;
+- Fix a high CPU-usage problem.
 
 --- 
-## [1.1.1] - 2023-01-09
+## [1.1.1]
 ### Added
 - Offer valid line-number info in the point cloud data of MID-360 Lidar.
 - Enable IMU by default.
 ### Changed
 - Update the README slightly.
 
 --- 
-## [1.0.0] - 2022-12-12
+## [1.0.0]
 ### Added
 - Support Mid-360 Lidar.
 - Support for Ubuntu 22.04 ROS2 humble.

README.md

@@ -129,46 +129,50 @@ All internal parameters of Livox_ros_driver2 are in the launch file. Below are d
 | ------------ | ------------------------------------------------------------ | ------- |
 | publish_freq | Set the frequency of point cloud publish <br>Floating-point data type, recommended values 5.0, 10.0, 20.0, 50.0, etc. The maximum publish frequency is 100.0 Hz.| 10.0    |
 | multi_topic  | If the LiDAR device has an independent topic to publish pointcloud data<br>0 -- All LiDAR devices use the same topic to publish pointcloud data<br>1 -- Each LiDAR device has its own topic to publish point cloud data | 0       |
-| xfer_format  | Set pointcloud format<br>0 -- Livox pointcloud2(PointXYZRTL) pointcloud format<br>1 -- Livox customized pointcloud format<br>2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (just for ROS) | 0       |
+| xfer_format  | Set pointcloud format<br>0 -- Livox pointcloud2(PointXYZRTLT) pointcloud format<br>1 -- Livox customized pointcloud format<br>2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (just for ROS) | 0       |
 
   **Note :**
 
   Other parameters not mentioned in this table are not suggested to be changed unless fully understood.
 
 &ensp;&ensp;&ensp;&ensp;***Livox_ros_driver2 pointcloud data detailed description :***
 
-1. Livox pointcloud2 (PointXYZRTL) point cloud format, as follows :
+1. Livox pointcloud2 (PointXYZRTLT) point cloud format, as follows :
 
 ```c
 float32 x               # X axis, unit:m
 float32 y               # Y axis, unit:m
 float32 z               # Z axis, unit:m
 float32 intensity       # the value is reflectivity, 0.0~255.0
-uint8 tag               # livox tag
-uint8 line              # laser number in lidar
+uint8   tag             # livox tag
+uint8   line            # laser number in lidar
+float64 timestamp       # Timestamp of point
 ```
+  **Note :**
+
+  The number of points in the frame may be different, but each point provides a timestamp.
 
 2. Livox customized data package format, as follows :
 
 ```c
-std_msgs/Header header    # ROS standard message header
-uint64 timebase           # The time of first point
-uint32 point_num          # Total number of pointclouds
-uint8  lidar_id           # Lidar device id number
-uint8[3]  rsvd            # Reserved use
-CustomPoint[] points      # Pointcloud data
+std_msgs/Header header     # ROS standard message header
+uint64          timebase   # The time of first point
+uint32          point_num  # Total number of pointclouds
+uint8           lidar_id   # Lidar device id number
+uint8[3]        rsvd       # Reserved use
+CustomPoint[]   points     # Pointcloud data
 ```
 
 &ensp;&ensp;&ensp;&ensp;Customized Point Cloud (CustomPoint) format in the above customized data package :
 
 ```c
-uint32 offset_time      # offset time relative to the base time
+uint32  offset_time     # offset time relative to the base time
 float32 x               # X axis, unit:m
 float32 y               # Y axis, unit:m
 float32 z               # Z axis, unit:m
-uint8 reflectivity      # reflectivity, 0~255
-uint8 tag               # livox tag
-uint8 line              # laser number in lidar
+uint8   reflectivity    # reflectivity, 0~255
+uint8   tag             # livox tag
+uint8   line            # laser number in lidar
 ```
 
 3. The standard pointcloud2 (pcl :: PointXYZI) format in the PCL library (only ROS can publish):

src/comm/comm.h

@@ -63,6 +63,8 @@ const int64_t kMaxPacketTimeGap = 1700000;
 /**< the threshold of device disconect */
 const int64_t kDeviceDisconnectThreshold = 1000000000;
 const uint32_t kNsPerSecond = 1000000000; /**< 1s  = 1000000000ns */
+const uint32_t kNsTolerantFrameTimeDeviation = 1000000; /**< 1ms  = 1000000ns */
+const uint32_t kRatioOfMsToNs = 1000000; /**< 1ms  = 1000000ns */
 
 const int kPathStrMinSize = 4;   /**< Must more than 4 char */
 const int kPathStrMaxSize = 256; /**< Must less than 256 char */
@@ -92,11 +94,8 @@ typedef enum {
 /** Timestamp sync mode define. */
 typedef enum {
   kTimestampTypeNoSync = 0, /**< No sync signal mode. */
-  kTimestampTypePtp = 1,    /**< 1588v2.0 PTP sync mode. */
-  kTimestampTypeRsvd = 2,   /**< Reserved use. */
-  kTimestampTypePpsGps = 3, /**< pps+gps sync mode. */
-  kTimestampTypePps = 4,    /**< pps only sync mode. */
-  kTimestampTypeUnknown = 5 /**< Unknown mode. */
+  kTimestampTypeGptpOrPtp = 1,    /**< gPTP or PTP sync mode */
+  kTimestampTypeGps = 2   /**< GPS sync mode. */
 } TimestampType;
 
 /** Lidar connect state */
@@ -155,7 +154,8 @@ typedef struct {
   float reflectivity; /**< Reflectivity   */
   uint8_t tag;        /**< Livox point tag   */
   uint8_t line;       /**< Laser line id     */
-} LivoxPointXyzrtl;
+  double timestamp;   /**< Timestamp of point*/
+} LivoxPointXyzrtlt;
 
 typedef struct {
   float x;
@@ -175,7 +175,7 @@ typedef struct {
 } PointPacket;
 
 typedef struct {
-  uint64_t base_time {};
+  uint64_t base_time[kMaxSourceLidar] {};
   uint8_t lidar_num {};
   PointPacket lidar_point[kMaxSourceLidar] {};
 } PointFrame;

src/comm/pub_handler.cpp

@@ -24,6 +24,7 @@
 
 #include "pub_handler.h"
 
+#include <cstdlib>
 #include <chrono>
 #include <iostream>
 #include <limits>
@@ -61,6 +62,8 @@ void PubHandler::RequestExit() {
 
 void PubHandler::SetPointCloudConfig(const double publish_freq) {
   publish_interval_ = (kNsPerSecond / (publish_freq * 10)) * 10;
+  publish_interval_tolerance_ = publish_interval_ - kNsTolerantFrameTimeDeviation;
+  publish_interval_ms_ = publish_interval_ / kRatioOfMsToNs;
   if (!point_process_thread_) {
     point_process_thread_ = std::make_shared<std::thread>(&PubHandler::RawDataProcess, this);
   }
@@ -129,8 +132,8 @@ void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t d
   packet.data_type = data->data_type;
   packet.point_num = data->dot_num;
   packet.point_interval = data->time_interval * 100 / data->dot_num;  //ns
-  packet.time_stamp = GetDirectEthPacketTimestamp(data->time_type,
-                                                  data->timestamp, sizeof(data->timestamp));
+  packet.time_stamp = GetEthPacketTimestamp(data->time_type,
+                                            data->timestamp, sizeof(data->timestamp));
   uint32_t length = data->length - sizeof(LivoxLidarEthernetPacket) + 1;
   packet.raw_data.insert(packet.raw_data.end(), data->data, data->data + length);
   {
@@ -143,25 +146,35 @@ void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t d
 }
 
 void PubHandler::PublishPointCloud() {
-  frame_.base_time = std::numeric_limits<uint64_t>::max();
-  frame_.lidar_num = 0;
+  //publish point
+  if (points_callback_) {
+    points_callback_(&frame_, pub_client_data_);
+  }
+  return;
+}
+
+void PubHandler::CheckTimer() {
+  uint8_t lidar_number = lidar_process_handlers_.size();
 
-  // Calculate Base Time
   for (auto &process_handler : lidar_process_handlers_) {
-    uint64_t base_time = process_handler.second->GetLidarBaseTime();
-    if (base_time != 0 && base_time < frame_.base_time) {
-      frame_.base_time = base_time;
+    uint64_t recent_time_ms = process_handler.second->GetRecentTimeStamp() / kRatioOfMsToNs;
+    if ((recent_time_ms % publish_interval_ms_ != 0) || recent_time_ms == 0) {
+      continue;
     }
-  }
-  // Get Lidar Point
-  for (auto &process_handler : lidar_process_handlers_) {
-    process_handler.second->SetLidarOffsetTime(frame_.base_time);
+
+    uint64_t diff = process_handler.second->GetRecentTimeStamp() - process_handler.second->GetLidarBaseTime();
+    if (diff < publish_interval_tolerance_) {
+      continue;
+    }
+
     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;
@@ -170,27 +183,11 @@ void PubHandler::PublishPointCloud() {
     lidar_point.points = points_[id].data();
     frame_.lidar_num++;
   }
-  //publish point
-  if (points_callback_) {
-    points_callback_(&frame_, pub_client_data_);
-  }
-  return;
-}
 
-void PubHandler::CheckTimer() {
-  auto now_time = std::chrono::high_resolution_clock::now();
-  //First Set
-  if (last_pub_time_ == std::chrono::high_resolution_clock::time_point()) {
-    last_pub_time_ = now_time;
-    return;
-  } else if (now_time - last_pub_time_ <
-      std::chrono::nanoseconds(publish_interval_)) {
-    return;
+  if (frame_.lidar_num == lidar_number) {
+    PublishPointCloud();
+    frame_.lidar_num = 0;
   }
-  last_pub_time_ += std::chrono::nanoseconds(publish_interval_);
-
-  PublishPointCloud();
-
   return;
 }
 
@@ -200,15 +197,15 @@ void PubHandler::RawDataProcess() {
     {
       std::unique_lock<std::mutex> lock(packet_mutex_);
       if (raw_packet_queue_.empty()) {
-        packet_condition_.wait_for(lock, std::chrono::milliseconds(500))  ;
+        packet_condition_.wait_for(lock, std::chrono::milliseconds(500));
         if (raw_packet_queue_.empty()) {
           continue;
         } 
       }
       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()) {
@@ -235,37 +232,12 @@ uint64_t PubHandler::GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time
   LdsStamp time;
   memcpy(time.stamp_bytes, time_stamp, size);
 
-  if (timestamp_type == kTimestampTypePtp) {
+  if (timestamp_type == kTimestampTypeNoSync ||
+      timestamp_type == kTimestampTypeGptpOrPtp ||
+      timestamp_type == kTimestampTypeGps) {
     return time.stamp;
-  } else if (timestamp_type == kTimestampTypePpsGps) {
-    struct tm time_utc;
-    time_utc.tm_isdst = 0;
-    time_utc.tm_year = time_stamp[0] + 100; // map 2000 to 1990
-    time_utc.tm_mon  = time_stamp[1] - 1;   // map 1~12 to 0~11
-    time_utc.tm_mday = time_stamp[2];
-    time_utc.tm_hour = time_stamp[3];
-    time_utc.tm_min = 0;
-    time_utc.tm_sec = 0;
-
-#ifdef WIN32
-    uint64_t time_epoch = _mkgmtime(&time_utc);
-#else
-    uint64_t time_epoch = timegm(&time_utc); // no timezone
-#endif
-    time_epoch = time_epoch * 1000000 + time.stamp_word.high; // to us
-    time_epoch = time_epoch * 1000;                           // to ns
-
-    return time_epoch;
   }
-  return std::chrono::high_resolution_clock::now().time_since_epoch().count();
-}
 
-inline uint64_t PubHandler::GetDirectEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size) {
-  LdsStamp time;
-  memcpy(time.stamp_bytes, time_stamp, size);
-  if (timestamp_type == kTimestampTypePtp) {
-    return time.stamp;
-  }
   return std::chrono::high_resolution_clock::now().time_since_epoch().count();
 }
 
@@ -280,24 +252,25 @@ uint64_t LidarPubHandler::GetLidarBaseTime() {
   return points_clouds_.at(0).offset_time;
 }
 
-void LidarPubHandler::SetLidarOffsetTime(uint64_t base_time) {
-  for (auto& point_cloud : points_clouds_) {
-    point_cloud.offset_time -= base_time;
-  }
-}
-
 void LidarPubHandler::GetLidarPointClouds(std::vector<PointXyzlt>& points_clouds) {
   std::lock_guard<std::mutex> lock(mutex_);
   points_clouds.swap(points_clouds_);
 }
 
+uint64_t LidarPubHandler::GetRecentTimeStamp() {
+  if (points_clouds_.empty()) {
+    return 0;
+  }
+  return points_clouds_.back().offset_time;
+}
+
 uint32_t LidarPubHandler::GetLidarPointCloudsSize() {
   std::lock_guard<std::mutex> lock(mutex_);
   return points_clouds_.size();
 }
 
+//convert to standard format and extrinsic compensate
 void LidarPubHandler::PointCloudProcess(RawPacket & pkt) {
-   //convert to standard format and extrinsic compensate
   if (pkt.lidar_type == LidarProtoType::kLivoxLidarType) {
     LivoxLidarPointCloudProcess(pkt);
   } else {

src/comm/pub_handler.h

@@ -50,10 +50,9 @@ class LidarPubHandler {
   void SetLidarsExtParam(LidarExtParameter param);
   void GetLidarPointClouds(std::vector<PointXyzlt>& points_clouds);
 
+  uint64_t GetRecentTimeStamp();
   uint32_t GetLidarPointCloudsSize();
-
   uint64_t GetLidarBaseTime();
-  void SetLidarOffsetTime(uint64_t base_time);
 
  private:
   void LivoxLidarPointCloudProcess(RawPacket & pkt);
@@ -108,7 +107,6 @@ class PubHandler {
 
   static bool GetLidarId(LidarProtoType lidar_type, uint32_t handle, uint32_t& id);
   static uint64_t GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size);
-  static uint64_t GetDirectEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size);
 
   PointCloudsCallback points_callback_;
   void* pub_client_data_ = nullptr;
@@ -121,7 +119,9 @@ class PubHandler {
   std::deque<RawPacket> raw_packet_queue_;
 
   //pub config
-  uint64_t publish_interval_ = 100000000; //100 ms
+  uint64_t publish_interval_ = 100000000; //100 ns
+  uint64_t publish_interval_tolerance_ = 100000000; //100 ns
+  uint64_t publish_interval_ms_ = 100; //100 ms
   TimePoint last_pub_time_;
 
   std::map<uint32_t, std::unique_ptr<LidarPubHandler>> lidar_process_handlers_;