feature:add offset time for every point

fix:fix directory tree bug in the license file
change:update custom msg format in README.md

README.md

@@ -1,12 +1,12 @@
-# livox ros driver
+# Livox ROS Driver
 
 ### Run livox ros driver
 
 livox_ros_driver is a new ros package under the Livox-SDK/Livox-SDK-ROS/src directory, which is designed to gradually become the standard driver package for livox devices in the ros environment. The driver offers users a wealth of options:
 
-1. Publish pointcloud2 format point cloud and automatically load rviz；
+1. Publish pointcloud2 format point cloud and automatically load rviz.
 
-for example：
+for example:
 
 ```
 roslaunch livox_ros_driver livox_lidar_rviz.launch bd_list:="broadcast_code1&broadcast_code2&broadcast_code3"
@@ -18,9 +18,9 @@ or
 roslaunch livox_ros_driver livox_hub_rviz.launch bd_list:="hub_broadcast_code" 
 ```
 
-2. Publish pointcloud2 format point cloud only；
+2. Publish pointcloud2 format point cloud only.
 
-for example：
+for example:
 
 ```
 roslaunch livox_ros_driver livox_lidar.launch bd_list:="broadcast_code1&broadcast_code2&broadcast_code3"
@@ -34,9 +34,9 @@ roslaunch livox_ros_driver livox_hub.launch bd_list:="hub_broadcast_code"
 
 
 
-3. Publish livox custom format point cloud；
+3. Publish livox custom format point cloud.
 
-for example：
+for example:
 
 ```
 roslaunch livox_ros_driver livox_lidar_msg.launch bd_list:="broadcast_code1&broadcast_code2&broadcast_code3"
@@ -48,12 +48,11 @@ or
 roslaunch livox_ros_driver livox_hub_msg.launch bd_list:="hub_broadcast_code"
 ```
 
-livox custom msg format：
+livox custom msg format:
 
 ```
 Header header             # ROS standard message header
 uint64 timebase           # The time of first point
-uint32 timestep           # Time interval between adjacent point clouds
 uint32 point_num          # Total number of pointclouds
 uint8  lidar_id           # Lidar device id number
 uint8[3]  rsvd            # Reserved use
@@ -61,6 +60,7 @@ CustomPoint[] points      # Pointcloud data
 ```
 pointcloud format:
 ```
+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

livox_ros_driver/livox_hub.cpp

@@ -49,6 +49,7 @@
 
 const uint64_t kPacketTimeGap      = 1000000;     // 1ms = 1000000ns
 const uint64_t kMaxPacketTimeGap   = 1700000;     // the threshold of packet continuous
+const uint64_t kDeviceDisconnectThreshold = 1000000000;     // the threshold of device disconect
 const uint64_t kNsPerSecond        = 1000000000;  // 1s  = 1000000000ns
 const uint32_t kPublishIntervalMs  = 50;         // unit:ms
 
@@ -156,12 +157,20 @@ void InitStoragePacketPool(void) {
   }
 }
 
-uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
+static void QueueProPop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
   uint32_t mask = queue->mask;
   uint32_t rd_idx = queue->rd_idx & mask;
 
   memcpy(storage_packet, queue->storage_packet[rd_idx], sizeof(StoragePacket));
+}
+
+static void QueuePopUpdate(StoragePacketQueue* queue) {
   queue->rd_idx++;
+}
+
+uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
+  QueueProPop(queue, storage_packet);
+  QueuePopUpdate(queue);
 
   return 1;
 }
@@ -236,6 +245,15 @@ static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) {
   }
 }
 
+static uint32_t GetPointInterval(uint32_t device_type) {
+  if ((kDeviceTypeLidarTele == device_type) || \
+      (kDeviceTypeLidarHorizon == device_type)) {
+    return 4167; // 4167 ns
+  } else {
+    return 10000; // ns
+  }
+}
+
 /* for pointcloud convert process */
 static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \
                                    uint8_t handle) {
@@ -251,7 +269,7 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu
 
   StoragePacket storage_packet;
   while (published_packet <  packet_num) {
-    QueuePop(queue, &storage_packet);
+    QueueProPop(queue, &storage_packet);
     LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
     LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
 
@@ -278,6 +296,7 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu
       cloud->points.push_back(point);
     }
 
+    QueuePopUpdate(queue);
     last_timestamp = timestamp;
     ++published_packet;
   }
@@ -294,6 +313,8 @@ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t pack
   uint64_t timestamp = 0;
   uint64_t last_timestamp = 0;
   uint32_t published_packet = 0;
+  uint32_t point_interval = GetPointInterval(lidars[handle].info.type);
+  uint32_t packet_offset_time = 0; // ns
 
   /* init livox custom msg */
   livox_ros_driver::CustomMsg livox_msg;
@@ -303,30 +324,33 @@ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t pack
   ++msg_seq;
   livox_msg.header.stamp = ros::Time::now();
   livox_msg.timebase = 0;
-  livox_msg.timestep = 10000; // 10us = 10^4ns;
   livox_msg.point_num = 0;
   livox_msg.lidar_id  = handle;
 
   StoragePacket storage_packet;
   while (published_packet <  packet_num) {
-    QueuePop(queue, &storage_packet);
+    QueueProPop(queue, &storage_packet);
     LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
     LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
 
     timestamp = GetStoragePacketTimestamp(&storage_packet);
     if (published_packet && \
-        ((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
+        ((timestamp - last_timestamp) > kDeviceDisconnectThreshold)) {
       ROS_INFO("packet loss : %ld", timestamp);
       break;
     }
     if (!livox_msg.timebase) {
       livox_msg.timebase = timestamp; // to us
-      ROS_DEBUG("[%d]:%ld", handle, livox_msg.timebase);
+      packet_offset_time = 0;         // first packet
+      ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval);
+    } else {
+      packet_offset_time = (uint32_t)(timestamp - livox_msg.timebase);
     }
     livox_msg.point_num += storage_packet.point_num;
 
     for (uint32_t i = 0; i < storage_packet.point_num; i++) {
       livox_ros_driver::CustomPoint point;
+      point.offset_time = packet_offset_time + i*point_interval;
       point.x = raw_points->x/1000.0f;
       point.y = raw_points->y/1000.0f;
       point.z = raw_points->z/1000.0f;
@@ -336,6 +360,7 @@ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t pack
       livox_msg.points.push_back(point);
     }
 
+    QueuePopUpdate(queue);
     last_timestamp = timestamp;
     ++published_packet;
   }

livox_ros_driver/livox_lidar.cpp

@@ -49,6 +49,7 @@
 
 const uint64_t kPacketTimeGap      = 1000000;     // 1ms = 1000000ns
 const uint64_t kMaxPacketTimeGap   = 1700000;     // the threshold of packet continuous
+const uint64_t kDeviceDisconnectThreshold = 1000000000;     // the threshold of device disconect
 const uint64_t kNsPerSecond        = 1000000000;  // 1s  = 1000000000ns
 const uint32_t kPublishIntervalMs  = 50;         // unit:ms
 
@@ -154,12 +155,20 @@ void InitStoragePacketPool(void) {
   }
 }
 
-uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
+static void QueueProPop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
   uint32_t mask = queue->mask;
   uint32_t rd_idx = queue->rd_idx & mask;
 
   memcpy(storage_packet, queue->storage_packet[rd_idx], sizeof(StoragePacket));
+}
+
+static void QueuePopUpdate(StoragePacketQueue* queue) {
   queue->rd_idx++;
+}
+
+uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
+  QueueProPop(queue, storage_packet);
+  QueuePopUpdate(queue);
 
   return 1;
 }
@@ -231,6 +240,15 @@ static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) {
   }
 }
 
+static uint32_t GetPointInterval(uint32_t device_type) {
+  if ((kDeviceTypeLidarTele == device_type) || \
+      (kDeviceTypeLidarHorizon == device_type)) {
+    return 4167; // 4167 ns
+  } else {
+    return 10000; // ns
+  }
+}
+
 /* for pointcloud convert process */
 static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \
                                    uint8_t handle) {
@@ -246,7 +264,7 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu
 
   StoragePacket storage_packet;
   while (published_packet <  packet_num) {
-    QueuePop(queue, &storage_packet);
+    QueueProPop(queue, &storage_packet);
     LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
     LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
 
@@ -273,6 +291,7 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu
       cloud->points.push_back(point);
     }
 
+    QueuePopUpdate(queue);
     last_timestamp = timestamp;
     ++published_packet;
   }
@@ -289,6 +308,8 @@ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t pack
   uint64_t timestamp = 0;
   uint64_t last_timestamp = 0;
   uint32_t published_packet = 0;
+  uint32_t point_interval = GetPointInterval(lidars[handle].info.type);
+  uint32_t packet_offset_time = 0; // ns
 
   /* init livox custom msg */
   livox_ros_driver::CustomMsg livox_msg;
@@ -298,30 +319,33 @@ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t pack
   ++msg_seq;
   livox_msg.header.stamp = ros::Time::now();
   livox_msg.timebase = 0;
-  livox_msg.timestep = 10000; // 10us = 10^4ns;
   livox_msg.point_num = 0;
   livox_msg.lidar_id  = handle;
 
   StoragePacket storage_packet;
   while (published_packet <  packet_num) {
-    QueuePop(queue, &storage_packet);
+    QueueProPop(queue, &storage_packet);
     LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
     LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
 
     timestamp = GetStoragePacketTimestamp(&storage_packet);
     if (published_packet && \
-        ((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
+        ((timestamp - last_timestamp) > kDeviceDisconnectThreshold)) {
       ROS_INFO("packet loss : %ld", timestamp);
       break;
     }
     if (!livox_msg.timebase) {
       livox_msg.timebase = timestamp; // to us
-      ROS_DEBUG("[%d]:%ld", handle, livox_msg.timebase);
+      packet_offset_time = 0;         // first packet
+      ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval);
+    } else {
+      packet_offset_time = (uint32_t)(timestamp - livox_msg.timebase);
     }  
     livox_msg.point_num += storage_packet.point_num;
 
     for (uint32_t i = 0; i < storage_packet.point_num; i++) {
       livox_ros_driver::CustomPoint point;
+      point.offset_time = packet_offset_time + i*point_interval;
       point.x = raw_points->x/1000.0f;
       point.y = raw_points->y/1000.0f;
       point.z = raw_points->z/1000.0f;
@@ -331,6 +355,7 @@ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t pack
       livox_msg.points.push_back(point);
     }
 
+    QueuePopUpdate(queue);
     last_timestamp = timestamp;
     ++published_packet;
   }

livox_ros_driver/msg/CustomMsg.msg

@@ -2,7 +2,6 @@
 
 Header header             # ROS standard message header
 uint64 timebase           # The time of first point
-uint32 timestep           # Time interval between adjacent point clouds
 uint32 point_num          # Total number of pointclouds
 uint8  lidar_id           # Lidar device id number
 uint8[3]  rsvd            # Reserved use

livox_ros_driver/msg/CustomPoint.msg

@@ -1,5 +1,6 @@
 # Livox costom pointcloud format.
 
+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